卡尔曼滤波

卡尔曼滤波

本文参考文章《Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation》,使用火车运动的例子进行卡尔曼滤波的推导,并用Python实现。

1. 简介

卡拉曼滤波广泛应用于数据融合领域,如阿波罗导航系统,汽车多传感器融合等。

卡尔曼滤波可以用如下公式表示:

式中:

  •  表示 t 时刻的最优估计。
  •  表示 t-1 时刻对 t 时刻的预测。
  •  系统增益。
  •  表示 t 时刻的测量值。
  •  表示转换矩阵,把测量值映射到预测值,统一变量单位。
  •  表示预测值标准差。
  •  表示测量值的标准差。

2. 推导

卡拉曼滤波算法利用了正太分布的特性,系统预测值和测量值都是正太分布,两者相乘得到最优估计的正太分布,减小了标准差,提高了精度。算法计算量小,易收敛。

2.1 前提

如图所示,火车沿铁轨运动,要求解火车天线的位置, 包含了火车位置和速度:

火车受到  的力,火车质量 。火车输入信号 

在  时间内,位置、速度和作用力之间的关系:

写成矩阵形式:

卡尔曼滤波_第1张图片

2.2 求解

如图所示,火车的  初始位置,红色表示位置的高斯分布。箭头指向右边,表示火车的初速度。

卡尔曼滤波_第2张图片

下图红色表示预测值的的正态分布:

卡尔曼滤波_第3张图片

下图蓝色表示测量值的正太分布:

卡尔曼滤波_第4张图片

利用正太分布的特性,得到下图绿色估计值的正太分布:

卡尔曼滤波_第5张图片

把相同项折叠起来,则可写成:

式中:

如果测量值是无线电信号传播时间,则还需要把单位统一,增加光速系数 

根据之前的推导:

带入  和  得到:

同理:

推导最终结论和卡尔曼方程一致:

2.3 迭代

预测值正太分布和测量值正态分布相乘得到新的正太分布,作为下次预测值正态分布。通过不断迭代,逐渐向真实值收敛。

3.实现

详细代码见notebook。

3.1 初始化

假设火车匀速运动,初始位置、速度、标准差如下,不考虑质量、推力等因素。且预测值和测量值单位相同。

||||| |:--:|:--:|:--:|:--:| |0m|1m/s|0.1|0.2|

预测初始值和最优估计初始值是10。

3.2 结果

从图中可以看出,即使初始值偏差较大时,经过迭代,系统仍可以收敛到真实值,且最优估计比测量值和预测值更精确。

卡尔曼滤波_第6张图片

3.3 比较

系统标准差  和  代表了预测模型和测量模型的可信度。提高测量模型的可信度,系统更快收敛:

卡尔曼滤波_第7张图片

系统的初始值也很重要,也会影响迭代的速度。

4.QA

4.1 如何确定标准差?

一般根据传感器特性,通过测量得到。

你可能感兴趣的