# Regarding implementation of kalman filter

I am not sure if this is the right stack exchange but please say so if it isn't and I will try to post my question in the relevant place.

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.

• You are talking about a discrete time system but your state equation seems to indicated a continuous time system. I assume the dot should the state at the next time step instead of a derivative? And why are you updating the process noise covariance, I assume it should be the error covariance ($P$) instead? – Kwin van der Veen Sep 24 at 11:04
• Ahh yes sorry for the silly mistakes, was looking at a summary of the equations from a book and didn't realize their notation was slightly different. – Morten Nissov Sep 24 at 11:32

Your code is wrong for a number of reasons. First you do not want to compute m times the same value of Pp:

Pp = F*Pu*F' + Qd; %this will be the same value computed over and over

since F,Pu and Q_d don't change. I assume you meant to write

Pp = F*Pp*F' + Qd;

Let's rewrite your outer loop body as:

for j=1:m
Pp = F*Pp*F' + Qd;
K = [K, Pp*C'*(R + C*Pp*C')^(-1)];
cnt = cnt + 1;
end

Your issue is now almost fixed. Let's set set m=1 as the issue will hold regardless of m:

Pp = P0
for i=1:N
Pp = F*Pp*F' + Qd;
K = [K, Pp*C'*(R + C*Pp*C')^(-1)];
Pu = (eye(nx) - K(:,cnt)*C)*Pp;
cnt = cnt + 1
end

the code will run correctly for the first loop, but for i=2 step Pp = F*Pp*F' + Qd; will be wrong (correctness of value computed - it will still run). We fix this with the following

Pu = P0
for i=1:N
Pp=Pu
Pp = F*Pp*F' + Qd;
K = [K, Pp*C'*(R + C*Pp*C')^(-1)];
Pu = (eye(nx) - K(:,cnt)*C)*Pp;
cnt = cnt + 1
end

Now your Kalman filter matches the classic definition. Note that Pp is usually defined as the predict covariance and Pu update covariance. For initializing the Kalman filter Pu is defined (and not Pp like you did). Putting it all together:

Pu = P0
for i=1:N
Pp = Pu
for j=1:m
Pp = F*Pp*F' + Qd;
K = [K, Pp*C'*(R + C*Pp*C')^(-1)];
Pu = (eye(nx) - K(:,cnt)*C)*Pp;    ]
cnt = cnt + 1
end
Pu = (eye(nx) - K(:,cnt)*C)*Pp;
cnt = cnt + 1
end
• Regarding your first comment I do tho, because continuing time updates while waiting for measurement can be necessary depending on the use case. Secondly Pp = FPpF' + Qd; is just not the right equation for the time update step. Finally you say Pu, $P^+$, is the one initialized; from what I've seen in text books I can't agree with that. Given I have not exhaustively searched through all KF textbooks, but the ones I have seen initialize $P^-$. – Morten Nissov Sep 27 at 5:47
• I am not arguing against your use of propagating the model while waiting for the measurement - it is just that your code was wrong as it computed the same value over and over again (Pp = F*Pu*F' + Qd;). "just not the right equation for the time update step. " - you're confusing correctness of a mathematical formula for how math gets translated to code. Pu is indeed the one initialized (the one required in the first 'measurement' update) - see en.wikipedia.org/wiki/Kalman_filter – stantheman Sep 28 at 6:12
• $P_{0|0}$ (Pu or $P^{+}$) is initialized in order to be used in the predict phase to compute $P_{1|0}$ (Pp or $P^{-}$). en.wikipedia.org/wiki/Kalman_filter#Details – stantheman Sep 28 at 6:17
• I see what you mean regarding the calculation of Pp in the time update, yes the implementation is definitely not 1:1 with the math. There should probably have been some kind of carry over between successive time updates. – Morten Nissov Sep 29 at 13:26
• With regards to initialization I am not quite sure what to make of it. The wikipedia article has an example on "hybrid KF" in which they initialize P- and x-. And if you look at textbooks like "Aided Navigation: GPS with High Rate Sensors" or "Linear Systems Control: Deterministic and Stochastic Methods" they both describe initializing as setting x- and P- to x(0) and P(0). Of course textbooks are not necessarily 100% correct but I do not see much support for initializing x+ and P+, if you have some other source I would love to see it. – Morten Nissov Sep 29 at 13:27