Regarding kalman filter implementation

Question

I think I might have originally posted this in the wrong stack exchange forum, link. I think this might be the right place, I am not sure if posting again is considered "bad style", I apologize if this is the case.

My questions is regarding the implementation of a discrete time Kalman filter assuming the time update occurs much more often than measurement update. I'll be specifically looking at the covariance propagation and Kalman gain equations.

Given a D.T. KF with the following state space model: $$ \hat{x}_{k+1} = F \hat{x}_k + G \omega_k $$ $$ \hat{y}_k = C \hat{x}_k + \upsilon_k $$ and assuming $\hat{x}^-(0)$ and $P^-(0)$ are known as well as the process and measurement noise intensities (Q and R respectively) the relevant equations are:

Gain update: $$ K = P^-C^T (CP^- C^T + R)^{-1} $$ Measurement update $$ P^- = F P^+ F^T + Q $$ Time Update: $$ P^+ = (I-KC)P^- $$

The difficulty I am having is with respect to implementation and how to properly initialize. A pseudo code example of what I think should be done is the following:

% Assume P0 is given
Pp = P0;                                       % Initializing P-
for i=1:N                                      % N = number of measurement updates
    for j=1:m                                  % m = number of time updates in one measurement update
        Pp = F*Pu*F' + Qd;                     % covariance prop
    end
    K = Pp*C'*(R + C*Pp*C')^(-1)        % update gain
    Pu = (eye(nx) - K*C)*Pp;            % measurement update
end

But this has the problem that the first iteration Pp cannot compute because there has yet to be a measurement update. This is easily solved by forcing a measurement update before any time updates. Maybe it's just me but it seems kind of incorrect to NEED a measurement update before any time updates.

Answer 1

D.T Kalman filter time update equations (predict) with initial estimates for $\hat{x}_k$ and $P_{k}$

1. Project the state ahead: $\hat{x}^-_{k+1} = F\hat{x}_{k}+Bu_{k}$
2. Project the error covariance ahead: $P^-_{k+1} = FP_{k}F^T+Q$

Measurement equations (correct)

1. Compute the Kalman gain: $K_{k+1} = P^-_{k+1}C^T(CP^-_{k+1}C^T+R)^{-1}$
2. Update estimate with measurement: $\hat{x}_{k+1} = \hat{x}^-_{k+1}+K_{k+1}(y_{k+1}-C\hat{x}^-_{k+1})$
3. Update the error covariance: $P_{k+1}= (I-K_{k+1}C)P^-_{k+1}$

Note the super minus sign is for a priori estimate.

Answer 2

I believe you are misunderstanding some of the Kalman filter equations. There is no such thing as a time update.

What you have shown here

Time Update:

$$P^+ = (I-KC)P^-$$

is the updating of the uncertainty/covariance from the measurement update. You can see this labeled properly on the wikipedia article on the kalman filter.

In your code your initial value of Pu should be set to the uncertainty of your initial value. If you don't know this then usually Identity will work and it will update itself accordingly.

Also in general I don't really see how your code is going to work. You shouldn't really have two for loops. You should have 1 for loop and and an if state for the measurement update. Generally I would also separate the update and prediction into two functions.

I don't know your application and this is an EKF but try to structure your code similarly to this example.