2
$\begingroup$

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.

$\endgroup$
2
$\begingroup$

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.

| improve this answer | |
$\endgroup$
1
$\begingroup$

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.

| improve this answer | |
$\endgroup$
  • $\begingroup$ 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. $\endgroup$ – Morten Nissov Jan 28 at 19:57
  • $\begingroup$ The Pu is a typo that I had not noticed, I will fix this. $\endgroup$ – Morten Nissov Jan 28 at 19:57
  • $\begingroup$ 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. $\endgroup$ – Morten Nissov Jan 28 at 19:59
  • 1
    $\begingroup$ 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. $\endgroup$ – edwinem Jan 29 at 7:17
  • $\begingroup$ I'm not sure where it is written that measurement necessarily must happen before time or vice verse. $\endgroup$ – Morten Nissov Jan 29 at 16:05

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.