《组合导航系列文章》是《从零开始做自动驾驶定位》系列的第二阶段，从本阶段开始，文章在《泡泡机器人》公众号上首发，知乎用来备份和以后可能出现的必要更正。

泡泡机器人文章链接：组合导航系列文章（十二）：滤波器基本原理

以下是原文：

1. 概述

滤波器是组合导航的核心，是把惯性导航的解算结果和其他传感器输出的观测结果连接在一起的纽带，只有借助它，才能使二者优势互补，得到一个精确、稳定的导航系统。组合导航中使用的滤波器有很多，常见的有卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)、迭代扩展卡尔曼滤波(IEKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)，本篇文章会一一介绍他们的原理，只是在这之前，我们需要回忆一下以前学过的一些参数估计的基础知识。

2. 参数估计思想

理解参数估计思想，重点要区分的是三个方法：极大似然估计、最大后验估计、贝叶斯估计。

1) 极大似然估计

我们以最常见的抛硬币的场景来举例子。假设抛硬币抛了10次，6次为正面，4次为反面（正正反正正正反反正反）。现在我们想问，单次抛硬币正面朝上的概率是多少，才最有可能出现这样的结果，这即是最大似然的含义。

数学问题还是要先用数学语言描述一下，记总的样本为

，它包含每个样本的结果，即正面还是反面，用`$theta$`表示单次抛硬币时结果为正面的概率，

表示

取不同值时，当前样本结果出现的概率，最大似然问题就是求

取多少时，

可以取到最大值。由于样本之间是独立观测的，因此

表示单个样本出现的概率，正面时为

，反面时为

，于是有

想求

取何值时它取最大值，最简单的方法就是求它关于

的导数，使导数为零即可。

让它取0，即让

取0，因此可以得到

，也就是说当正面朝上的概率是0.6时，10次样本出现6次正面4次反面的概率最高，这就是极大似然估计的结果。

2) 最大后验估计

面对这个抛硬币的问题，总是忍不住会问，正面朝上的概率为什么不是0.5，因为经验告诉我们它应该是这样的。但是反过来想，如果它一定是0.5，那么还抛硬币干什么，直接取

不就行了。之所以不能这样做，是因为我们认为0.5只是一个"最可能"的值，而不是"一定是"，因为硬币可能真不是正反面均匀的。那么现在我们的问题就变成了，我猜测它最可能是0.5，但样本告诉我它最可能是0.6，应该信谁的问题，不如折中一下，把它俩都考虑在内，做个加权，用数学表示就是原来求

取最大时对应的

值，而现在要求

取最大值时对应的

值，其中

表示的就是

在0到1区间的概率分布。我们可以假设

其中

是概率密度函数。当然，这种先验假设可以有很多种，这只是随意选取的一种，它表示在

处先验概率最大，A只是为了使

在0到1的区间内积分结果为1而添加的一个系数。那么，现在我们要求的就是下面这个式子取最大值时对应的

值是多少。

求解的方式仍然是求导，然后让导数为零，由于它的导数公式展开比较复杂，我们直接给出结果，在

后验概率会达到最大。这个值介于先验概率最大时对应的

和似然概率最大值

两者之间，其实就是加权的正常现象。如果把

的方差调小，那么加权之后的结果会更接近0.5，反之会更接近0.6，这个方法影响的就是加权的权重。

3) 贝叶斯估计

虽然最大后验估计在极大似然估计的基础上考虑了先验猜测，但是我们仍然还有疑问，既然不能保证硬币一定是正反面均匀的，0.5只是一个最可能的值，那么在当前观测样本下，

就可能为0到1中间的任何值，而我们只取了一个最可能后验估计值，其他值呢？如果我们能够给出所有可能值的概率，那应该是对后验最全面的描述了，贝叶斯估计就是为了解决这个问题。

贝叶斯估计使用的是著名的贝叶斯公式。

其中

描述的就是

取不同值时的概率，

和

之前已经介绍过了。新面孔是

，它表示当前样本出现的概率。可以用下式求解

到这里，贝叶斯的完整表达就呼之欲出了。

虽然比较复杂，但我们知道它完整表达了每个可能的

出现的概率就可以了。

4) 三者之间的联系

极大似然估计是最大后验估计的一个特例：上面提到，最大似然是估计

取最大值时对应的

值，而最大后验是估计

取最大值时对应的

值，区别在于后者比前者多了一个

，如果

是区间0到1上的平均分布，那么

，最大后验就和最大似然相等了，即最大似然是最大后验在

为平均分布时的一个特例。脱离公式，从直观上理解，平均分布表示

的平均分布表示它取0到1上

任何值得概率都是一样的，这种先验猜测猜了等于没猜，提供的信息量为零，因此极大似然估计可认为是没有任何先验信息的最大后验估计。

极大似然估计和最大后验估计包含于贝叶斯估计：贝叶斯估计求的是

取所以可能值时的概率，而最大后验估计是找概率最大的那个

的取值，因此它只是贝叶斯的所有取值之一，同样的，极大似然估计是没有先验信息情况下，概率最大的那个

值，仍然是贝叶斯估计的取值之一。

3. 滤波器估计原理

组合导航中，先验是imu解算的值，观测是gps等传感器给出的值，融合的目的是找到概率最大的那个值。上面介绍的三种方法都是对先验和观测的融合，由于极大似然要求先验为平均分布，而组合导航中，先验和观测都假设为高斯，导致只能用最大后验估计和贝叶斯估计，又由于贝叶斯估计要求观测的全概率（即上面贝叶斯公式里的P(X)），带来了很大的复杂性，因此往往都是使用最大后验估计。Kalman就是最大后验的一种典型实现形式，当然，在线性高斯情况下它和贝叶斯估计是等价的，使用贝叶斯方法同样能推导出kalman，只是在非线性情况下二者不再等价。

在这里如果要把kalman所有的推导过程全部列出来，会显得有点啰嗦，我们换一个角度理解这个问题。在上面扔硬币的例子里，我们已经能够理解，融合就是加权。具体到高斯的假设下，就是需要知道各自的均值、方差，并计算一个加权系数。

假设有高斯分布的状态x和观测z

则把这二者融合在一起形成的新的高斯分布`$N(mu,sigma)$`满足

稍加整理一下可以得到这样的形式

其中

这三个式子已经比较像kalman里面的状态更新方程、协方差更新方程和增益计算方程了。但是还差两个式子，这是因为x和`$mu_x$`其实是预测值，我们并没有把“预测”体现在式子里，如果把它加进来，就有

这里面

和

可以分别认为是上一次后验估计的均值和方差结果，

和

就分别当做均值和方差的预测函数吧。

这样我们就凑齐了五个公式，他们其实就是kalman的五大公式，只是符号不一样，我们一一对应起来

kalman公式里面的各个符号应该不用解释，想必各位都是熟悉的。

4. 滤波器改进

以上所有的推导都是在线性高斯假设下进行的，当传递函数为非线性时，这个更新过程便无法进行。这里给出《概率机器人》里面的一张图，对比说明线性和非线性之间的差异。

除了经典kalman以外，其他kalman的改进形式基本都是为了解决这个问题。主要包括这样几种：

1) 扩展kalman(EKF)

面对非线性问题，EKF采用了一种最简单的解决办法，就是把非线性近似成线性，采用的是泰勒展开的方法。由于这种线性只是近似出来的，它的结果自然是有偏的，而且泰勒展开的点是当前估计值，即便是在滤波收敛之前，估计值和真值还有很大差异，它仍然只能采用这种不准确的值去近似。这就造成了一个问题，如果状态模型收敛性很好，就可以形成一个良性循环“当前值处泰勒展开->估计得到更精确值->更精确值处展开->估计进一步精确”，但是如果收敛性不好，状态量的波动比较大，那么良性循环则可能变成恶性循环，导致滤波器发散。

2) 迭代扩展kalman(IEKF)

它比扩展卡尔曼多了个“迭代”，这个“迭代”是在同样也是按照"当前值处展开->估计->估计值处展开->再估计"的循环进行的，它和EKF的那个良性循环的区别在于，这是在一个kalman更新周期内进行的。这样做比单纯的EKF的好处在于，每次的精度损失会更小。

3) 粒子滤波(PF)

粒子滤波强行回避了非线性问题，既然非线性无论怎么近似都有误差，无法根据先验用表达式给出预测的分布的解析表达，那么就回归分布的本源，用统计来做。按照先验的分布随机给出一定数量的值，把这些值按照传递函数运算得到观测的预测，那么我们就可以得到同样数量的预测值，直接对这些值进行概率统计，不就可以得到它的分布了吗。这些值就是粒子，这种用粒子解决估计问题的方法就是粒子滤波。通过原理可以看出，粒子滤波属于贝叶斯估计，而不是最大后验估计，粒子滤波也是这些滤波算法中唯一的一个贝叶斯估计。当然粒子滤波也有它自己的问题，为了得到精确的分布，粒子的数量必须足够多，而且非线性越强，对数量的要求就越高，这意味着计算量的增加。除了计算量以外，更严重的是粒子退化问题，基本上大部分粒子滤波的研究都是为了解决这个问题。

4）无迹kalman(UKF)

UKF既不想做线性化，又不想采取像PF那样暴力的做法，而是采取了一种中庸的思想，只使用少量粒子来找出预测的分布，既然数量比较少，就不能浪费，于是在均值和均值两侧按照一定距离去设计点的位置。

5. 怎么选

有了多种方法，必然会面临怎么选的问题。面对实际问题，很少有绝对的线性，也很少有绝对的高斯，一切只是接近，是否要把它当做非高斯来处理，要看这种接近能达到什么程度。

1) 什么时候用KF

当有高精度惯导时，虽然它的完整模型时非线性的，而且是强非线性，但是由于惯导精度高，可以把状态量都当成小量，这时候就可以把矩阵中的一些三角函数简化，比如

，

，除此之外，还可以把小量之间的相乘直接消掉，即

，所以大家看到的高精度惯性导航的状态方程都是线性的，除了一些还未完成初始对准，即失准角比较大的情况。

2) 什么时候用EKF和IEKF

首先是状态误差无法用小量来近似，必须用非线性形式来表达的时候。其次则是在无人机里场出现的，直接用全量而不是误差量进行建模的情况，比如直接用姿态的四元数做状态量，而不再是失准角。同样的，能用EKF的地方都可以用IEKF。

3) 什么时候用PF

首先是状态模型的非线性非常强，但这种在多传感器融合定位中其实还比较少见，多数是能够用雅克比进行展开的。在实际工程中使用PF更多地是因为它的另一个优点，即它不要求噪声是高斯的。在低成本的多传感器融合中，这一点太重要了，如果你仔细看过低精度mems的实验数据，会发现它和高斯真的离得太远了，低成本的观测也是同样问题。由于影响滤波器精度和稳定性的除了模型，还要噪声，因此在这种情况下，优先采用PF有时候会是更合理的选择。

4) 什么时候用UKF

UKF其实用的比较少，这是它的特性决定的，当非线性不强的时候，EKF已经够用，当非线性和非高斯性强的时候，精度又不如PF，所以除非你正好有一定的非线性，但是对运算量的要求又很高，则可以用UKF。Cartographer的早期版本用的就是UKF，只不过现在的新版本已经用图优化来做融合了。

上一篇：组合导航系列文章（十一）：惯性导航误差传播规律

下一篇：组合导航系列文章（十三）：构建一个基本的组合导航系统