kalman filter的一个例子

kalman filter 的原理就不讲解了，这里直接写一个一维状态变量的估计例子.参考《probabilistic robotic》这本书。

clear all;
N=500;
q=0.1*randn(1,N);%process noise
r=10*randn(1,N);%observation noise

x(1)=25;
a=1;
c=1;

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

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

p(1)=10;

s(1)=1;

for t=2:N;
    Rq=cov(q(1:t));  
    Rr=cov(r(1:t)); % caculate covariance 
    xhat=a*x(t-1);
    phat=a^2*p(t-1);
    K(t)=c*phat/(c^2*phat+Rr);
    
    s(t)=xhat+K(t)*(z(t)-c*xhat);
    p(t)=phat*(1-K(t)*c);
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');