Regarding kalman filter implementation

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.

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.

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.

• I think we have a misunderstanding in terminology, time update is used in many textbooks as a colloquialism for the prediction step when one is working with an open form Kalman filter, i.e. separate prediction and estimation. – Morten Nissov Jan 28 '20 at 19:57
• The Pu is a typo that I had not noticed, I will fix this. – Morten Nissov Jan 28 '20 at 19:57
• The dual for loops is to account for speed differences when information is available for prediction vs estimation steps. For example a gnss unit at 1 Hz + IMU at 100Hz in an INS context. – Morten Nissov Jan 28 '20 at 19:59
• Ok. Your post also has the measurement update and time update wrong. They need to be switched. Your code still seems to have problems. The Kalman gain K should not be in the prediction step(but instead in the measurement step). Also it only makes sense right now if m the number of prediction steps is constant between the measurement updates. – edwinem Jan 29 '20 at 7:17
• I'm not sure where it is written that measurement necessarily must happen before time or vice verse. – Morten Nissov Jan 29 '20 at 16:05