1
$\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:

% Pp = P-
% Pu = P+
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
        K = [K, Pp*C'*(R + C*Pp*C')^(-1)];     % update gain
        cnt = cnt + 1;
    end
    Pu = (eye(nx) - K(:,cnt)*C)*Pp;            % measurement update
    cnt = cnt + 1;
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$
0
$\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.

$\endgroup$

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.