无人驾驶——2.定位之卡尔曼滤波

找遍全网，个人认为这篇讲的最好。

参考：https://blog.csdn.net/young_gy/article/details/78177291

http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies（详细计算实例）

卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。

什么是卡尔曼滤波

首先定义问题：对于某一系统，知道当前状态XtX_t，存在以下两个问题：

1. 经过时间

后，下个状态

如何求出？

1. 假定已求出

，在t+1t+1时刻收到传感器的非直接信息

，如何对状态

进行更正？

这两个问题正是卡尔曼滤波要解决的问题，形式化两个问题如下：

1. 预测未来
2. 修正当下

下面，将以机器人导航为例，从预测未来和修正当下两个角度介绍卡尔曼滤波器。

卡尔曼滤波的原理

问题场景如下：一个机器人，我们想知道它实时的状态

，同时也想做到预测未来和修正当下这两件事。

其状态xx表示为一维大小为2的向量，元素分别表示位置信息与速度信息：

可是状态xx不一定是精准的，其不确定性用协方差表示：

预测未来

只考虑自身状态

只考虑自身状态的情况下，根据物理公式，可得：

用矩阵表示如下：

在状态变化的过程中引入了新的不确定性，根据协方差的乘积公式可得：

考虑外部状态

外部状态，这里以加速度为例，引入变量

同时，环境仍然存在我们无法刻画的误差，以

表示，最终的预测公式如下：

从上述式子可见：

1. 新的最优估计是之前最优估计之前最优估计}}的预测加上已知的外界影响已知的外界影响}}的修正。
2. 新的不确定度是预测的不确定度加上环境的不确定度。

修正当下

我们已得到

，下面要通过观测到的测量值

对

进行更新。

因为

和

的数据尺度不一定相同，例如

包含了笛卡尔的坐标信息，使用radar得到的

则包含极坐标信息。所以首先应该把两者放在相同的尺度下去比较，尺度转换使用Hkmathbf{H}_k将预测信息转化为测量信息的尺度。

这样一来，便得到测量尺度上的两个分布：

1. 测量值的分布

1. 预测值变换后的分布

下面一个问题就是如何用这个两个分布组成新的分布。

简单的一维情况如下：

总结

预测未来

修正当下

卡尔曼滤波需要内存少，计算速度快，适合实时性情况与嵌入式设备的需要。