# 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:

% 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.

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.