001_Kalman_Filter

卡尔曼滤波

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

1. 简介

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

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

式中：

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

2. 推导

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

2.1 前提

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

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

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

写成矩阵形式：

2.2 求解

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

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

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

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

把相同项折叠起来，则可写成：

式中：

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

根据之前的推导：

带入 和 得到：

同理：

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

2.3 迭代

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

3.实现

详细代码见notebook。

3.1 初始化

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

0m	1m/s	0.1	0.2

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

3.2 结果

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

3.3 比较

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

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

4.QA

4.1 如何确定标准差？

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