A simple Kalman Filtre Algorithme

Sorry, cuz there's no chinese input in lab. I just write down in English.

 

Kalman Filtre Algorithme has been applied in many fields. The most amazing part is the alternant iteration process between state prediction (Time update) and observation update. The core steps of a standard Kalman Filtre Algorithme are depicited by 5 major  équations (cf. http://en.wikipedia.org/wiki/Kalman_filter).

 

Here I just offer u the souce code of MATLAB and the result.

 

clear all
N=500;
q=0.1*randn(1,N); % process white noise, return 1*N random matrix
r=10*randn(1,N);% observation white noise
x(1)=25;
a=1;
c=1;

for k=2:N;
    x(k)=a*x(k-1)+q(k-1);
end; %linear differential equation

for k=1:N
    z(k)=c*x(k)+r(k);
end; % obeservation function

p(1)=10;
s(1)=1;

for t=2:N;
    Rq=cov(q(1:t));
    Rr=cov(r(1:t)); % caculate covariance
    p1(t)=a.^2*p(t-1)+Rq; % Time update of covariance 2nd equation
    Kg(t)=c*p1(t)/(c.^2*p1(t)+Rr);% kalman gain
    s(t)=s(t-1)+Kg(t)*(z(t)-c*s(t-1));%observation update of state
    p(t)=p1(t)-Kg(t)*c*p1(t); % observation update of convariance
end;

t=1:N
error=s-x;% estimate value - state value
plot(t,s,'bo',t,z,'g.',t,x,'r--',t,error,'m*'); % line show kalman; dot line observation; slash line state
legend('Kalman estimate','Observation','State value','error');