十一、滤波器基本原理

1. 概述

滤波器是组合导航的核心，是把惯性导航的解算结果和其他传感器输出的观测结果连接在一起的纽带，只有借助它，才能使二者优势互补，

得到一个精确、稳定的导航系统。组合导航中使用的滤波器有很多，常见的有 卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)、迭代扩展卡尔曼滤波(IEKF)、

无迹卡尔曼滤波(UKF)、粒子滤波(PF)，本篇文章会一一介绍他们的原理，只是在这之前，我们需要回忆一下以前学过的一些参数估计的基础知识。

 

2. 参数估计思想

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

1) 极大似然估计

我们以最常见的抛硬币的场景来举例子。假设抛硬币抛了10次，6次为正面，4次为反面（正正反正正正反反正反）。

现在我们想问，单次抛硬币正面朝上的概率是多少，才最有可能出现这样的结果，这即是 最大似然的含义。

                                                                    举的例子很好，深入理解呀

数学问题还是要先用数学语言描述一下，记总的样本为，它包含每个样本的结果，即正面还是反面，用

表示单次抛硬币时结果为正面的概率， 表示 取不同值时，当前样本结果出现的概率，最大似然问题就是求取多少时，

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

 

2) 最大后验估计

面对这个抛硬币的问题，总是忍不住会问，正面朝上的概率为什么不是0.5，因为经验告诉我们它应该是这样的。

但是反过来想，如果它一定是0.5，那么还抛硬币干什么，直接取不就行了。之所以不能这样做，是因为我们认为0.5只是一个 "最可能" 的值，

而不是 "一定是"，因为硬币可能真不是正反面均匀的。那么现在我们的问题就变成了，我猜测它最可能是0.5，但样本告诉我它最可能是0.6，

应该信谁的问题，不如折中一下，把它俩都考虑在内，做个加权，用数学表示就是原来求 取最大时对应的值，而现在要求

取最大值时对应的值，其中表示的就是 在 0 到1 区间的概率分布。我们可以假设

3) 贝叶斯估计

虽然最大后验估计在极大似然估计的基础上考虑了先验猜测，但是我们仍然还有疑问，既然不能保证硬币一定是正反面均匀的，

0.5只是一个最可能的值，那么在当前观测样本下， 就可能为0到1中间的任何值，而我们只取了一个最可能后验估计值，其他值呢？

如果我们能够给出所有可能值的概率，那应该是对后验最全面的描述了，贝叶斯估计就是为了解决这个问题。

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

4) 三者之间的联系

 

3. 滤波器估计原理

组合导航中，先验是imu解算的值，观测是gps等传感器给出的值，融合的目的是找到概率最大的那个值。上面介绍的三种方法都是对先验和观测的融合，

由于极大似然要求先验为平均分布，而组合导航中，先验和观测都假设为高斯，导致只能用最大后验估计和贝叶斯估计，又由于贝叶斯估计要求观测的

全概率（即上面贝叶斯公式里的P(X)），带来了很大的复杂性，因此往往都是使用最大后验估计。Kalman就是最大后验的一种典型实现形式，当然，

在线性高斯情况下它和贝叶斯估计是等价的，使用贝叶斯方法同样能推导出kalman，只是在非线性情况下二者不再等价。

在这里如果要把kalman所有的推导过程全部列出来，会显得有点啰嗦，我们换一个角度理解这个问题。在上面扔硬币的例子里，我们已经能够理解，

融合就是加权。具体到高斯的假设下，就是需要知道各自的均值、方差，并计算一个加权系数。

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

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

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

其中

这三个式子已经比较像kalman里面的状态更新方程、协方差更新方程和增益计算方程了。但是还差两个式子，

这是因为x和`$\mu_x$`其实是预测值，我们并没有把“预测”体现在式子里，如果把它加进来，就有

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，只不过现在的新版本已经用图优化来做融合了