卡尔曼滤波系列_实例（二）均加速运动的卡尔曼滤波

卡尔曼滤波实例——均加速运动的卡尔曼滤波

此系列（一）对卡尔曼滤波的原理进行了简单的阐述，总结了卡尔曼滤波的两大过程：预测和更新。接下来举例对卡尔曼滤波的使用进行介绍，加深对卡尔曼滤波的理解。

均加速小车的状态估计（位置和速度）

1.场景介绍

如上图所示，可知小车的初始速度为0，初始位置也为0，小车向前的加速度为1,小车感知自身状态（位置）是通过GPS完成的，而GPS的精度较低，其探测精度的方差为10；且在这个系统中还存在着其他影响小车运动的因素，如风阻，地面摩擦力和其他环境因素等等，所以我们不能完全通过GPS的测量完成对小车的定位，这种情况下，卡尔曼滤波闪亮登场。
为了完成对小车的定位（状态估计），首先我们要确定小车运动的状态方程，分析整个运动系统后，我们得知

假定每次估计的时间差值为1,带入加速度a=1,其状态方程表达式如下

由卡尔曼滤波公式（1）确定


观测方程为


故

接下来利用卡尔曼滤波公式（2）求解状态方程的协方差矩阵预测值

令R为

再利用卡尔曼滤波公式（3），求解卡尔曼滤波的增益系数矩阵

这里令Q=10即可。
利用卡尔曼滤波公式(4)对当前状态进行最优估计，

继续更新状态方程协方差矩阵的估计值，为下一次状态估计做准备。
以上过程，完成了一次卡尔曼滤波状态估计。

2.MATLAB仿真

根据场景介绍描述，若不使用卡尔曼滤波的方法，单独使用GPS数据对小车位置进行定位，其在前30s的位置信息如下图所示

横轴表示时间变化，竖轴表示小车定位数据。
使用卡尔曼滤波后的前30s位置信息如下图所示。

两者对比图如下

从对比图可以看出，使用卡尔曼滤波后位置信息中的噪声数据得到明显遏制，位置信息变化更为平滑。

3.MATLAB仿真代码

上述仿真模型代码如下
Z=1:30;
noise = 10randn(1,30);
Z=Z.^2;
Z=Z+noise;
plot(Z);
X=[0; 0]; %初始状态X
P=[1 0; 0 1];%初始状态协方差矩阵
A=[1 1; 0 1];%状态转移矩阵
B=[0.5 1];
a=1;
R=[0.01 0; 0 0.01];%状态转移协方差
C=[1 0];%观测矩阵
Q=10;
% figure;
hold on;
for i=1:30
X_=AX+Ba;
P_=APA’+R;
K=P_C’/(CP_C’+Q);
X=X_ + K(Z(i)-CX_);
P=(eye(2)-K*C)*P_;
plot(i,X(1),‘.’);
% pause(1)
end

第一个实例介绍完成，后续会接着对拓展卡尔曼滤波等卡尔曼滤波变种进行研究。

觉得不错的话，点个赞鼓励一下哦