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.