Kalman update
[x1,p1,x,p]=kalm(y,x0,p0,f,g,h,q,r)
current system matrices
covariance matrices of dynamics and observation noise
state estimate and error variance at t=0 based on data up to t=-1
current observation Output
updated estimate and error covariance at t=1 based on data up to t=0
updated estimate and error covariance at t=0 based on data up to t=0
This function gives the Kalman update and error covariance. To do this, we have to enter f
,
g
, h
which are based on the state space model:
x(k+1)=
f
*x(k)+g
*u(k)+v(k)
y(k)=
h
*x(k)+w(k)
with v(k)
(resp. w(k)
) is the process noise (resp. the observation noise)
supposed to be drawn from a zero mean Gaussian white noise with the covariance q
(resp. r
).
Precisely, Kalman filter is a recursive estimator which gives the estimate of the current state and the error covariance. Its advantage is the fact that it only needs the estimated state from the previous step and the current measurement.
Algorithm:
Innovation (output error):
E=
y
-h
*x0
Output Error covariance:
S=
h
*p0
*h
'+r
Kalman gain:
K=
p0
*h
'*S^-1
Correction of state estimation:
x
=x0
+K*E
Correction of estimation of error covariance:
p
=p0
-K*h
*p0
State prediction:
x1
=f
*x
Error covariance prediction:
p1
=f
*p
*f
'+g
*q
*g
'
// Construction of the sinusoid w=%pi/4; // angular frequency T=0.1; // period t=0:T:500; signal=cos(w*t); // Sinusoid with noise v=rand(t,"normal"); y=signal+v; // Plot the sinusoid with noise subplot(2,1,1); plot(t,y); xtitle("sinusoid with noise","t"); // System n=2; // system order f=[cos(w*T) -sin(w*T); sin(w*T) cos(w*T)]; g=0; h=[1 0]; p0=[1000 0; 0 0]; R=1; Q=0; x0=zeros(n,1); // Initialize for loop x1=x0; p1=p0; // Kalman filter for i=1:length(t)-1 [x1(:,i+1),p1,x,p]=kalm(y(i),x1(:,i),p1,f,g,h,Q,R); end // Plot the results (in red) to compare with the sinusoid (in green) subplot(2,1,2); plot(t,signal,"color","green"); plot(t,x1(1,:),"color","red"); xtitle("Comparison between sinusoid (green) and extraction with Kalman filter (red)","t"); | ![]() | ![]() |