详解神奇的卡尔曼滤波（Kalman filter）算法

详解神奇的卡尔曼滤波（Kalman filter）算法

---

前言

任何算法都有一个基本思想，或者基于源于一些假设。所以说在学习算法的时候一定要先弄清楚它的基本思想和假设前提，然后再把注意力放到细节上。个人建议不要上来就一点点的抠细节，很可能在算法里绕来绕去走不出来。

一、卡尔曼滤波要解决的问题

大家都说卡尔曼滤波神奇，那么卡尔曼滤波要解决什么问题呢？卡尔曼滤波用来减小数据误差，优化状态变量。如何直观理解卡尔曼滤波要解决的问题这篇文章写的非常生动和详细。

二、贝叶斯滤波

在详细推导卡尔曼滤波之前，我们先说一下贝叶斯滤波，如果看过一些卡尔曼滤波的文章，你会多次看到这句话：卡尔曼滤波是贝叶斯滤波的具体实现。那么贝叶斯滤波是什么呢？贝叶斯滤波算法它做的工作就是根据已有的信息来计算概率。比如我想根据一些信息识别某个人性别是不是女，那贝叶斯滤波算法要做的是就是计算这个人是女的概率P(女）。来了新的信息就计算新的概率。如果你不理解这句话看接下来这个例子你就懂了。假如现在贝叶斯滤波算法是要根据你已输入的信息分析某个人是男是女。贝叶斯滤波算法最终的输出是计算出根据现有信息判断这个人是男性的概率是多少，女性的概率是多少。然后你的算法就选择概率更大的那种情况输出结果。如果有新的信息进来（比如“那个人有长头发”），贝叶斯算法要重新计算这个人是男性的概率是多少，女性的概率是多少。
总结一句话：贝叶斯滤波做的事情就是求条件概率，条件概率可能很多人不太熟悉，那么后验概率应该熟悉吧，在一般情况下，你可以认为条件概率和后验概率是一样的。换句话说：贝叶斯滤波做的事情就是求后验概率。那么说到后验概率，就不得不提到贝叶斯公式，下面我们先回忆一下贝叶斯公式，然后我将详细推导多条件下的贝叶斯公式。
首先是条件概率公式：

然后是贝叶斯公式：


由此我们可以得到：

下面重点来了，多条件下的贝叶斯公式：

那么它是不是就这么一种形式呢，显然不是，上面是在z成立的条件下推导的，如果我们在y成立的条件下推导呢，很显然：我们把上面z和y的位置换一下就行了：

好了现在我们的准备工作已经结束了，下面我们将进行卡尔曼滤波的详细推导。

三、卡尔曼滤波的详细推导

下面我将从vslam的角度即移动机器人的状态估计方向上对卡尔曼滤波进行详细的推导。机器人状态估计就是：根据机器人之前的状态，传感器观测数据和控制器控制命令来估计当前机器人的状态。那么什么是机器人状态？机器人离障碍物距离，机器人的速度，机器人的姿态，机器人在世界坐标系下的地址这些都是机器人的状态。我就以机器人离某个障碍物的距离作为机器人状态来举例，假设机器人在时刻t，它观测到自己离障碍物距离是Zt，它控制自己移动的命令是让它移动Ut 这么远的距离，算法根据传感器数据和控制器数据估计出离某个障碍物的距离是Xt，算法上个时刻估算出的距离是Xt-1。由于传感器，控制器，上个状态这些数据总会有误差。我怎么知道当前估计出机器人离障碍物距离Xt是不是正确的呢？我们只需要计算概率：

只要这个概率够大那不就是大概率是对的。这个概率公式指的是在知道传感器数据是Zt ，控制器数据是Ut ，上个状态是Xt-1 的情况下当前状态是Xt的概率。下面重点来了，卡尔曼滤波的假设是究竟什么呢？卡尔曼滤波的假设如下：
假设1：假设马尔可夫性，马尔可夫性不了解没关系，很简单，记住下面两句话：当前观测值Zt只与当前状态Xt有关，跟机器人的控制命令Ut和上个状态Xt-1无关。当前状态Xt 只与上个时刻状态Xt-1 以及与控制命Ut 有关。
假设2： 移动机器人所有的状态和过程中的误差的概率分布全部服从正态分布（高斯分布）
假设3：移动机器人的运动和观测方程是线性的
对于移动机器人的状态估计，我们有运动方程和观测方程：

上面的是运动方程，表示在已知机器人K-1时刻的状态和K时刻的控制命令的情况下，机器人在K时刻的状态，Wk是状态误差，下面的是观测方程，表示机器人在K时刻状态下观测到距离Zk的物体，或者表示距离传感器数据为Zk，Vk表示传感器误差。我们从最简单的线性系统入手，也就是说上面方程中的函数为线性方程，可得：

根据假设2：

上面这些变量的概率分布全部服从正态分布，特别的，记这里的噪声服从零均值高斯分布：

假设我们知道了 k − 1时刻的后验（在 k − 1 时刻看来）状态估计：

和它的协方差

现在要根据 k 时刻的输入和观测数据，确定 状态Xk的后验分布。为区分推导中的先验和后验，我们在记号上作一点区别：

根据正态分布加法运算规则，我们可得：


它显示了如何从上一个时刻的状态，根据输入信息，推断当前时刻的状态分布。这个分布也就是先验。记这里的：

另外，根据正态分布加法运算规则，我们同样可得：

前面已经知道，我们需要求的是：

我们利用贝叶斯公式往下推导，由于含有Zt的只有一个已知，并且是一个条件概率Zt还在竖线的坐边，因此我们把所有的含有Zt的联合概率用Zt都在坐边的那种形式的条件概率表示：

根据假设1，我们可以得到：

所以上面的推导化简为：

由上式可得，分子往往为定值，往往我们把它设为1。那么：

这就是我们在机器人状态估计中最常见的公式，无论是贝叶斯估计还是卡尔曼滤波都是这个公式。现在我已经将它推导的细节进行了还原。根据上面公式，为了得到后验概率，我们想要计算两个正态分布的乘积。但是我们知道最后会得到一个关于 Xk 的正态分布（根据假设2），但计算上是有一丁点儿麻烦的，我们先把结果设为：

那么根据我们推导的公式：

我们可以得到：

这里我们稍微用点讨巧的方法。既然我们已经知道等式两侧都是正态分布，那就只需比较指数部分即可，而无须理会正态分布前面的因子部分。指数部分很像是一个二次型的配方，我们来推导一下。首先我们先看下正态分布即高斯分布对应的函数：

然后我们把上面等式左右指数部分展开，这个展开需要你自己动手写下，单纯看可能看不明白：

我们把两边展开，并比较 Xk 的二次和一次系数。对于二次系数，有：

在上式左右各乘

同时我们定义一个中间变量：

可以得到：


这里的K就是卡尔曼增益，然后我们再比较Xk一次项的系数：

消去系数并转置得：

左右两边同时乘以：

我们可以得到：

总而言之，上面的步骤可以归纳为“预测”（Predict）和“更新”（Update）两个步骤，
预测：根据K-1时刻状态的后验概率来得到K时刻状态的先验概率
更新：根据K时刻状态的先验概率来得到K时刻状态的后验概率
总的来说：卡尔曼滤波就是利用K-1时刻状态的后验概率来得到K时刻状态的后验概率，这个时候你再想想我们上面的贝叶斯滤波，就能明白为什么说卡尔曼滤波是贝叶斯滤波的具体实现。

至此，我们推导了经典的卡尔曼滤波器的整个过程。事实上卡尔曼滤波器有若干种推导方式，而我们使用的是从概率角度出发的最大后验概率估计的形式。我们看到，在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

四、扩展卡尔曼滤波（EKF）

卡尔曼滤波是针对线性系统成立的，移动机器人的运动方程和观测方程通常是非线性函数，我们希望把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器（Extended Kalman Filter，EKF）。通常的做法是，在某个点附近考虑运动方程以及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。然后按照线性系统进行推导。令 k−1 时刻的均值与协方差矩阵为：

在 k 时刻，我们把运动方程和观测方程在k-1时刻的均值和协方差矩阵处进行一阶泰勒展开：

记这里的偏导数为：

同样的，对于观测方程，亦有：

记这里的偏导数为：

那么，在预测步骤中，根据运动方程和正态分布加法运算有：

这些推导和卡尔曼滤波是十分相似的。为方便表述，记这里Xk先验和协方差的均值为

同样根据观测方程，可得：

根据我们上面详细推导的公式：

在上面公式，右边我们已经根据运动方程和观测方程求出来了，然后我们按照上面卡尔曼滤波预测和更新的步骤走一遍后续推导：设Xk的后验和协方差，公式左右展开，比较一阶项系数，比较二阶项系数，推导 EKF 的预测与更新方程。最后我们会得到卡尔曼增益为：

后验概率形式为：

EKF 形式简洁、应用广泛，同时节省资源，在早期SLAM过程中占有主导地位，后面逐渐被非线性优化算法取代。EKF的局限性主要有下面三个方面：
1.假设马尔可夫性，无法解决回环检测问题，非线性优化考虑到了这一问题，但是需要更多的计算资源
2.EKF只考虑了线性近似的情况，如果运动模型和观测模型有强烈的非线性，EKF会存在非线性误差
3.EKF需要存储状态量的均值和协方差，并进行更新，如果把移动机器人观测的所有路标都放入状态，存储量会呈平方增长，不太适用于大型场景。
参考书籍《视觉SLAM十四讲 从理论到实践》

---

如果您认可我的文章，希望能够关注我的微信公众号，我会不定期更新工作中学到的东西和一些技术比较前沿的东西。