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');