kalman filter的一个例子

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

clear all;N=500;q=0.1*randn(1,N);%process noiser=10*randn(1,N);%observation noisex(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');