<< iirlp Filters lev >>

Scilab Help >> Signal Processing > Filters > kalm

kalm

Kalman update

Syntax

[x1,p1,x,p]=kalm(y,x0,p0,f,g,h,q,r)

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:

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


Report an issue
<< iirlp Filters lev >>