kalm

Arguments

f,g,h

current system matrices

q, r

covariance matrices of dynamics and observation noise

x0,p0

state estimate and error variance at t=0 based on data up to t=-1

y

current observation Output

x1,p1

updated estimate and error covariance at t=1 based on data up to t=0

x,p

updated estimate and error covariance at t=0 based on data up to t=0

Description

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'

Example: Extraction of a sinusoid from noise with Kalman filter

// 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");

See also

• srkf — square root Kalman filter
• sskf — steady-state Kalman filter