扩展卡尔曼滤波_SLAM从0到1——9. 推导思路：卡尔曼滤波与扩展卡尔曼滤波

本文介绍了SLAM后端部分的卡尔曼滤波器与扩展卡尔曼滤波器的推导思路（个人总结，若有不规范请勿采纳）

之前介绍过SLAM状态估计问题的优化求解方法。除了优化方法，对于状态估计问题还有一类经典“古老”的方法，那就是卡尔曼滤波方法。关于卡尔曼滤波的介绍有很多，在这里给出笔者自己对推导过程的理解和整理。

在SLAM中，如果我们不像视觉里程计一样只考虑相邻两帧(或相邻几帧)的运动估计，而是考虑较长时间内的运动估计，那么对这些多个时刻(大量)数据的处理思路可以分为两类：

1. k时刻的状态和前面1到k-1个时刻的状态都有关，利用前面所有状态对k时刻的状态进行估计。这种处理方式是非线性优化方法。
2. k时刻的状态只和k-1时刻的状态有关，即假设运动具有马尔可夫性，并且是一阶马尔可夫性。这类方法称为滤波器方法（卡尔曼滤波器）。

1.线性系统与卡尔曼滤波

考虑复杂的非线性系统之前，先看一下如何对线性系统进行状态估计。假设噪声信号服从最简单的高斯分布：

运动方程：

观测方程：

现在假设我们已经有了k-1时刻的状态估计和协方差，根据k时刻的运动输入和观测数据，来获得k时刻的状态估计（后验分布）。

根据贝叶斯法则， 后验 -> 似然 * 先验。

>>>所以卡尔曼滤波思路可以概括为：计算求得满足马尔可夫性的先验分布和似然概率，相乘求得后验分布（状态估计）<<<

① 通过运动方程确定

的先验分布：

② 根据观测方程计算k时刻的似然（观测数据分布）：

。这一步通过技巧简化计算后，可以直接计算

卡尔曼增益K。

③ 计算后验概率（似然和先验的乘积）。根据计算技巧，转换为直接计算后验的状态估计

和协方差矩阵

。

2.非线性系统与扩展卡尔曼滤波

有了上面线性系统的推导和结论，把卡尔曼滤波器推广至非线性系统就称为扩展卡尔曼滤波EKF。

由于直接对非线性系统进行求解非常复杂，所以我们对该非线性系统在

处进一阶泰勒展开，运动方程与观测方程相比于线性系统会多出来一项，但是后面求解思路和过程都是一样的，

求后验概率转换为求似然和先验的乘积。

① 通过运动方程确定

的先验分布：

② 根据观测方程计算k时刻的似然（观测数据分布）：

。这一步通过技巧简化计算后，可以直接计算

卡尔曼增益

。

③ 计算后验概率（似然和先验的乘积）。根据计算技巧，转换为直接计算后验的状态估计

和协方差矩阵

。

3.总结

卡尔曼滤波器给出了线性化后状态变量分布的变化过程，在SLAM中给出了非线性系统在一阶线性近似后的最大后验估计（就是那个我们一直在求解的）。