kalman filter的一个例子

最新推荐文章于 2023-12-06 10:38:24 发布

原创最新推荐文章于 2023-12-06 10:38:24 发布 · 4.3k 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#slam

机器人专栏收录该内容

7 篇文章

订阅专栏

本文通过一个一维状态变量估计的例子展示了Kalman滤波器的应用。使用MATLAB代码模拟了过程噪声和观测噪声，并进行了Kalman滤波估计，最终绘制了Kalman估计值、观测值及真实状态值的对比图。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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