kalman滤波预测速度之只有位置观测量的小汽车匀速直线运动模型

最新推荐文章于 2023-12-14 17:18:14 发布

原创最新推荐文章于 2023-12-14 17:18:14 发布 · 1.1k 阅读

4 ·

CC 4.0 BY-SA版权

信号处理同时被 2 个专栏收录

7 篇文章

订阅专栏

matlab

4 篇文章

订阅专栏

本文介绍了一种使用MATLAB实现卡尔曼滤波器的方法，通过生成一组理论位置和速度数据，然后添加噪声来模拟真实环境。卡尔曼滤波器通过预测和更新步骤，有效地从这些带噪数据中估计出更准确的位置和速度。

clear all;
R=2;
N=1000;
v=normrnd(0,sqrt(R),[1,N]);
k=[0:N-1];
dt=0.1;
vel_theory=10;%理论速度
pos_theory=12+k*vel_theory*dt;
pos_ob=pos_theory+v;
vel_pred=linspace(0,0,N);%预测的速度
pos_pred=linspace(0,0,N);
A=[1 dt;0 1];
H=[1 0];
xk=[0 0]';
P=[0.01 0.01;0.01 0.001];
Q=[0.01 0;0 0.01];
for i=1:1:N
    xk=A*xk;
    P=A*P*A'+Q;
    Kg=P*H'/(H*P*H'+R);
    xk=xk+Kg*(pos_ob(i)-H*xk);
    pos_pred(i)=xk(1);
    vel_pred(i)=xk(2);
end
plot(vel_pred,'r')