滤波器方法（贝叶斯/EKF/UKF/ESKF/MSCKF）_贝叶斯滤波

1. 背景概述

移动机器人、无人机或者无人船等是不能够像工业机器人利用关节处的力矩传感器和编码器的读数直接进行位姿的解算的，抛开工业机械设计制造及其装配时带来的误差，移动机器人、无人机或者无人船等内置的传感器往往会因为轮子打滑、imu噪声等问题引入难以忽略的误差，由此机器人学中的状态确认就成了一种概率性质的状态估计与更新方法论了。简单的问题分类与描述如下：

• 建图---让机器“理解”周围是什么包围了它；

• 定位---让机器“理解”它在环境中的位置；

• 建图+定位---结合成了SLAM问题；

• 建图、定位、SLAM的方法都是基于状态估计与更新；

那么结合上面的问题和描述，需要解决的事情大致就这么几件：明确状态量的组成（变量？定位？地图模型？）获取各个位置的传感器读数、整合传感器数据生成地图、将传感器读数与地图模型建立关系、计算在地图模型中的位置、估计位置和模型的确定性、提升状态估计的正确性。

针对以上问题，各理论方法在过去几十年的时间内被提出和应用在机器人学中的基于概率（贝叶斯）理论的位姿估计问题上，从80年代甚至更早，高斯模型、线性近似模型和位置跟踪的方法论“组成”了最初的Kalman Filters框架，在95年代，离散的方法论开始在拓扑型表征和栅格模型表征上进行全局定位、重定位和不确定处理（POMDPs）的研究，再到了99年，无参滤波器Particle Filters开始在基于采样方法的定位、重定位方向上大展身手，而千禧年后各个假说和前提条件下的滤波器研究成了研究的热门，如EKF的各个变体：ESKF、MSCKF和IKF都是针对不同的应用情况和追求被提出并得到了广泛的应用和优化。

本文将从贝叶斯公式及其相关的概率基础知识入手，遍历KF、EKF和UKF等基本的带参数的滤波器原理与公式，再谈谈ESKF、IKF和MSCKF这类EKF的变体滤波器，最后进入无参滤波器PF及其优化的策略。

2. 概率基础

2.1. 贝叶斯公式

联合概率密度分解成条件概率密度和边缘概率密度的乘积：

重新整理可以得到贝叶斯公式：

如果移植先验概率密度函数P(x)，以及传感器模型P(y|x)，那么就可以根据贝叶斯公式推断出后验概率密度，即：

以上过程即贝叶斯推断，也称为贝叶斯估计。

2.2. 高斯概率密度函数

一维高斯分布：

多维高斯分布：

例如，二维高斯分布为：

其中，

为x,y的相关系数，当x,y线性无关时，上式即等于0.

2.3. 联合高斯概率密度函数

若有高斯分布：

则它们的联合概率密度函数为

根据舒尔补公式：

称为矩阵D关于原矩阵的舒尔补。利用舒尔补公式，联合分布的方差矩阵可以写成：

联合高斯分布 的指数部分的二次项包括以下内容：

因此有

2.4. 高斯随机变量的线性分布

假设 y=Gx+n ， G 是一个常量矩阵， n 为零均值白噪声，在实际中指的是观测噪声。则有：

3. 贝叶斯滤波

所谓的机器人状态估计，其实就是估计机器人在当前时刻的状态x_t。这是一个向量，包含了我们想要估计的量，比如机器人的位置、姿态和速度。估计所需的信息来自两个方面。内部传感器提供的信息记作u__t，通常是运动信息，比如里程计或IMU提供的位移、速度等信息。外部传感器提供的信息记作z_t，通常是观测信息，比如激光测距仪提供的距离信息、相机提供的像素信息等。于是，状态估计问题可以描述为：在给定从1时刻到t时刻的所有传感器信息后，求条件概率:

该条件概率称为贝叶斯后验概率，记作bel(x_t)。所谓“后验”是结合了观测之后的概率。既然如此，当然就存在一个“先验”概率，即结合观测之前的概率，记作

为了计算简便，不可能每次都用所有时刻的传感器信息来估计后验概率。最明智的做法是迭代估计，用上一时刻的后估计当前时刻的后验:

其中，分母与x_t无关，可以当做常量，用归一化因子代替。注意，这里用的是所谓的条件贝叶斯公式，

始终作为条件变量，不受贝叶斯公式影响。

这个概率所依赖的条件太多，为了能够方便地求解它，我们不妨做一假设:

马尔可夫假设：系统当前时刻的状态只与上一个时刻有关，与之前任意时刻的状态都没有关系。当然，马尔可夫假设的成立是有条件的，它要求状态变量 具有完整性，换句话说，系统的全部信息都包含在了 中，不存在其它随机变量对系统状态产生影响。

考虑马尔可夫假设后，后验概率可以进一步简化为：

也就是说，当前时刻的观测z_t只依赖于当前状态x_t，与之前的状态和传感器数据无关。仔细观察s上式，可以发现右侧的第二个概率恰好是

即：

现在唯一的问题是后验概率如何计算，将其按照全概率公式展开：

这里利用了连续情况的全概率公式，考虑所有可能的x_(t-1)所能生成的x_t。再次考虑马尔可夫假设，将上式简化：

其中，第一行去掉了对

的依赖，因为它们所产生的影响已经完全包含在x_(t-1)中。第二行去掉了对u_t的依赖，因为u_t不可能对上一时刻的状态x_(t-1)产生影，再者可以发现积分号中的第二个概率恰好是上一时刻的后验概率，即：

至此，我们实际上把贝叶斯滤波分成了两个步骤。第一步称为预测，从bel[x_(t-1)]预测bel[x_t]。第二步称为更新，从bel[x_t]更新到其后验概率。在应用贝叶斯滤波时，需要事先根据具体的传感器类型确定两个条件概率：

然后利用传感器数据u_t和z_t不断进行预测和更新，得到当前时刻机器人的状态。

4. 卡尔曼滤波

在实际应用中，如何根据具体的传感器类型确刚刚上述的那两个条件概率往往会使得计算很复杂，甚至无法顺利进行，特别是

还包括一个积分。卡尔曼滤波则是为我们提供了一套针对在线性系统中的解决方案，保证求解的时效性。在卡尔曼滤波中，系统的状态改变和观测服从下面的线性方程：

其中，状态量中引入高斯随机变量，表示由状态转移引进的不确定性；观测量中也是同理引入分布是均值为0，方差为 Q的高斯随机变量。那么刚刚上述的那两个条件概率分别表示为：

对于条件概率来说，已知的条件已经不再是随机变量，因此第一个式子中的x_(t-1)和u_t，第二个式子中的x_t应按照常量对待。只在状态量和观测量中引入的噪声是服从高斯分布的随机变量。因此，两个式子得到的结果仍然是高斯分布：

4.1. KF第一部分：预测

接下来，仍然按照贝叶斯滤波的两个步骤：

直接求积分很困难，但可以尝试利用概率密度函数来求。我们就可以把拆分成两部分

上右边的第一项满足高斯分布指数部分的形式，第二部分是后面的常数项。利用以下操作：

为了计算二次型的系数，计算 Lt的一阶、二阶导数：

假设

因此可以将二次型函数 定义为：

最后可以得到，预测部分的分布为：

4.2. KF第二部分：更新

5. 扩展卡尔曼滤波算法（EKF）

卡尔曼滤波的前提是系统的状态改变和观测是线性的，但是实际应用中系统是非线性的。EKF通过一个一阶泰勒展开的线性系统来近似非线性。假设系统的状态和观测方程如下：

6. 无迹卡尔曼滤波算法（UKF）

该算法的核心思想是：采用UT变换，利用一组Sigma采样点来描述随机变量的高斯分布，然后通过非线性函数的传递，再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。

相比于EKF，UKF的估计精度能够达到泰勒级数展开的二阶精度。其本质是：近似非线性函数的均值和方差远比近似非线性函数本身更容易。

6.1. UT变换

y=f(x)是一个非线性变换, x为 n 维随机变量，UT变换根据一定的采样策略，获得一组Sigma采样点，并设定均值权值W和方差权值，来近似非线性函数的后验均值和方差。利用选取的Sigma采样点集进行非线性函数传递可得：

其中，y_i为Sigma采样经过非线性函数传递后对应的点。根据加权统计线性回归技术，可以近似得到y的统计特性：

6.2. Sigma点采样策略

下面介绍两种经常使用的采样策略：比例采样和比例修正对称采样。

根据Sigma点采样策略不同，相应的Sigma点以及均值权值和方差权值也不尽相同，因此UT变换的估计精度也会有差异。但总体来说，其估计精度能够达到泰勒级数展开的二阶精度。

使用参数beta对高斯表示的附加的分布信息进行编码。如果分布是精确的高斯分布，则beta=2是最佳的选择。

6.3. UKF采样原则

为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性，所以采样点的选取应满足：

式中{}内的符号分别为Sigma采样点，均值权值和方差权值组成的集合；L为采样点个数，P为随机变量x的密度函数，g[~]确定x的相关信息。如果密度函数P(x)只有一、二阶矩时，可以写成：

6.4. UKF的特点

UKF相比于EKF的精度更高一些，其精度相当于二阶泰勒展开，但速度会略慢一点。UKF另一个巨大优势是不需要计算雅克比矩阵，而有些时候雅克比矩阵也确实的我们无法获得的。

另外UKF与PF(粒子滤波)也有相似之处，只是无迹变换中选择的粒子是明确的，而粒子滤波中的粒子是随机的。随机的好处是可以用于任意分布，但也具有其局限性。因此对于分布近似为高斯分布的，采用UKF将更有效。

7. Error-state卡尔曼滤波（ESKF）

ESKF是一种典型的间接法滤波，其预测和更新过程都是针对系统的误差状态，再用修正后的误差状态修正系统状态。

其核心在于将系统状态真值分解为系统名义状态值和误差状态值的结合。状态分解为两个部分组成：

真实状态运动学：

名义状态运动学：

于是可以直接得到误差状态的运动学：

7.1. ESKF的优缺点

KF、EKF、UKF的系统模型都是直接描述系统状态，属于直接法滤波；ESKF的模型系统描述的是系统误差，而不是直接描述系统状态，需要通过转换得到系统状态，属于间接法滤波。ESKF的Error-State的值一般趋近于0，可以避免一些一阶部分可能出现的奇点，应用于惯导系统时的万向锁问题，提供所有时间内的线性有效性保证。而且Error-State很小，可以保证在泰勒展开中的二次部分忽略不计，使得雅可比的计算简单快速（计算复杂度UKF>EKF>ESKF>KF）。由于Error-State的动力系统通常比较缓慢，因此所有较大的分量被集中到计算名义状态，以为着可以使卡尔曼滤波的更新过程频率低于预测过程。

8. 迭代卡尔曼滤波(IKF\IEKF)

IKF全称为Iterated Kalman filter，是基于Extended Kalman filter(EKF)的滤波框架，在其中应用高斯牛顿迭代法而改良的方法，该方法的策略是牺牲掉少量计算时间，以优化EKF线性近似时产生的近似误差。本质上是在EKF框架中构建非线性优化模型以求解状态后验的最大似然，以提升EKF的精度。

IKF的预测与EKF几乎相同，IKF算法的主要贡献在于其更新阶段的优化，同时算法将预测阶段的状态估计值作为了当前阶段的观测值。以典型的IKF实现案例（Fast-LIO系列）来说，整个系统中有两个关键的时间点：利用每一次IMU的数据进行积分预测，利用每一次激光数据进行观测迭代更新。引申到通用的模式下，其实也就是需要在更新开始前确认清楚当前的预测状态量、对应的预测状态量协方差、当前的观测量及其观测量协方差，以找到最好的位姿状态估计及其协方差估计。

IKF的算法流程如下：

• 预测积分；

• 残差计算;

• 迭代更新；

针对第一个步骤预测积分就是先根据IMU的读数进行状态量预测和协方差预测

上式中的f即代表状态转移方程，Q为高斯白噪声w的协方差，Fw和Fx的定义式如下：

在上述中提到的两个关键时间点，该预测积分的过程会在每一次IMU读数到来的时候进行积分传播，直到下一帧激光数据来临时才结束进行更新操作 。

对于协方差的迭代更新，使用Matrix Inversion Lemma:

上式中P即为状态量的协方差矩阵，H表达为 的简写。进一步的分析可以看到，状态量的迭代更新与观测量z及其协方差R、预测状态及其协方差、观测方程g及其一阶导数H，整个迭代更新过程的converge条件就是达到局部最小值

由此可得，整个状态向量的迭代过程若仅仅进行一次迭代，则IKF系统就成了EKF。

9. 多状态约束下的卡尔曼滤波(MSCKF)

MSCKF是用于估算VIO的滤波算法，全称是Multi-State Constraint Kalman Filter，最早是在论文《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》中出现。

MSCKF是沿用EKF的框架，优化具体的实现以解决EKF-SLAM的维数爆炸问题为目标。在传统融合IMU数据和VO的EKF-SLAM框架中会将VO中的各个特征点信息加入到待解算的状态向量中（其中还包括了IMU的数据信息状态），也就是随着系统的运行，VO的关键帧越来越多，其VO中的特征点也会随之变得非常多，导致需要进行解算的状态向量维数会变得非常大。MSCKF则通过将不同时刻的VO相机关键位姿加入到状态向量，而不再将特征点加入到状态向量，如此操作的原因在于特征点可以被多个相机看到从而能够与多个相机状态（Multi-State）之间构建约束（Constraint），进而利用约束的信息构建观测模型对EKF进行更新。由于相机位姿的个数会远小于特征点的个数，MSCKF状态向量的维度相较EKF-SLAM大大降低，并通过滑动窗口（Sliding Window）历史的VO状态会不断移除从而对MSCKF后端的计算量进行限定，由此MSCKF的计算消耗较之传统方法更低，运行更加鲁棒。

所以根据上述MSCKF的原理，其中预测过程和EKF一致（利用IMU数据信息进行积分），关键的在于使用VO的信息来构建约束（利用3D特征点到VO相机的重投影坐标和实际观测到坐标构建误差方程）以更新IMU预测的状态向量。

MSCKF算法步骤如下：

• 预测（IMU积分）：先利用IMU加速度和角速度对状态向量中的IMU状态进行预测，一般会处理多帧IMU观测数据。

• 相机状态扩增：每来一张图片后，计算当前相机状态并加入到状态向量中, 同时扩充状态协方差.

• 特征点三角化：然后根据历史相机状态三角化估计3D特征点（特征点跟丢时才进行操作）

• 特征更新：再利用特征点对多个历史相机状态的约束，来更新状态向量。注意：这里不只修正历史相机状态，因为历史相机状态和IMU状态直接也存在关系(相机与IMU的外参)，所以也会同时修正IMU状态。

• 滑动窗口历史相机状态移除：如果相机状态个数超过N，则剔除最老或最近的相机状态以及对应的协方差.

• 重复1-5

上述步骤中，MSCKF中观测更新的时机是特征点跟丢，此时利用所有的历史观测信息进行三角化以尽可能地确保三角化的精度。

10. 粒子滤波

10.1. 蒙特卡洛定位算法（MCL）

蒙特卡洛定位算法时将合适的概率运动和感知模型带入到particle_filters中，MCL定位在概率滤波下同样用条件概率表示置信度：

AMCL定位在概率滤波下同样用条件概率表示置信度：

在粒子滤波中，使用一系列从后验中得到的随机状态采样后来表示后验。后验分布的样本表示为粒子集：

式中：Xt机器人t时刻的位姿状态的一个具体实例，每个粒子都是根据真实状态在时刻 的一种可能假设；N粒子集的粒子数量。粒子滤波就是用一系列粒子来近似状态X近似置信度bel(Xt) ，伪代码如下：

程序第3行基于粒子和控制量产生t时刻机器人可能位姿的粒子集，通过从采样获取。程序第4行计算重要性因子，通过观测量在粒子采样状态下的概率获取：

程序第8~11行是粒子滤波的重采样过程，根据计算重要性因子，从粒子集中采样获取N个粒子，获得新的粒子集，使用该粒子集来表示机器人近似表示机器人状态X置信bel。

10.2. 随机粒子蒙特卡洛定位：失效恢复

在蒙特卡洛定位算法中，在重采样过程中可能意外的丢失所有正确位姿附近的粒子（机器人绑架或者全局定位失效），问题可以通过探索算法解决：增加随机粒子到粒子集合。将增加随机粒子跟平均测量概率结合，在多个时间步上对这个测量概率求平均来平滑。设测量似然的长期平均，短期平均 。

在重采样过程中，随机采样以以下概率增加：

增加随机采样的概率考虑长期似然和短期似然平均的散度：短期似然优于或等于长期似然时，不增加随机采样。根据

短期平均的指数滤波器的衰减率更加接近当前的实际似然值。

如果 存在：

说明当前似然值高于之前，则当前的定位情况比之前好，则无需增加随机粒子。

并在RBpf算法的基础上改进提议分布和选择性重采样，改善粒子退化的问题。

10.3. KLD采样

粒子滤波通过粒子来实现机器人的位姿估计，粒子数量N直接影响到位姿估计的精度和定位算法的效率。粒子集的数量N必须足够大，才能避免重采样过程中的粒子消耗引起的发散问题，减少“粒子退化”带来的影响，保证机器人的定位精度。但是，粒子集的数量过大，则算法的数据处理计算量增加，造成定位的效率低。在定位早期阶段，需要使用数量足够大的粒子集来精确表示机器人的状态置信。但是，一旦机器人的位姿确定，只需要很小一部分粒子就可以跟踪机器人的位置。AMCL定位通过KLD（Kullback-Leibler Divergence, KLD）采样，根据采样的近似质量来调整所需的粒子数，在保证定位精度的前提下，减少用于定位的粒子的数量。KLD的伪代码如下：

程序第18行给出了采样所需的粒子数。粒子越分散， k（表示直方图中的非空bin）值越大，M越大，采样的粒子数也就越多。在机器人导航中，机器人的初始位姿未知，使用初始置信 来反映机器人的初始位姿信息，通过在地图上所有自由位姿空间的均匀分布来初始化：

式中：X地图中所有位姿空间的体积。通过初始置信，在机器人的自由位姿空间上采样，获得机器人初始状态下的粒子分布。

11. 优化和滤波的区别

• 卡尔曼滤波器：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验（马尔可夫假设）；

• 扩展卡尔曼滤波器：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性（马尔可夫假设）；

• BA优化：把一路上的所有坐标点（像素坐标对应的空间点等）与位姿整体放在一起作为自变量进行非线性优化

• PoseGraph优化：先通过一路递推方式算出的各点位姿，通过数学方式计算得到一个位姿的变换A，再通过单独拿出两张图像来算出一个位姿变换B，争取让B=A

• 增量因子图优化：保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，需要重新计算的再去计算，从而避免冗余计算。iSAM是增量的处理后端优化。由于机器人是运动的，不同的边和节点会被不断加入图中。每加入一个点，普通图优化是对整个图进行优化，所以比较麻烦和耗时。iSAM的话相当于是保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。以这种方式加速计算，避免冗余计算。

从全文的整理来看，特别是在IKF章节，我们可以发现滤波算法其实就是将Sliding Window的大小设置为1的优化算法，不论是优化算法还是滤波算法都是期望求解出问题概率模型的最大似然估计（MLE），本质上滤波就是基于马尔可夫假设将优化问题的建模进行了“特征化”的处理。再进一步分析，正因为滤波器进行了范围上维度的“简化”、模型的近似处理，所以其计算消耗较于优化算法更低，但由此引发的代价就是精度上的损失。另一个需要考虑的方面是，滤波器是由先验信息+运动模型+观测信息三个方面点顺序执行，以实现位姿状及其协方差的估计和更新，也正因为滤波器的框架如此，若是先验（上一时刻）状态出现了问题，比如位姿跟丢、计算错误等，那么在该时刻之后的状态都会出现问题以致纠正不回来了，而基于优化方法的位姿求解则会考虑更多时刻（关键帧）下的状态信息和观测信息，即使有某一时刻的状态量和协方差是outlier，系统也有一定的能力维稳。

如果在处理器算力充足且精度为第一需求的前提下，那么在位姿估算问题处理上是首推优化算法，但若是效率是第一前提条件，那么就需要根据实际的应用情况和机器人问题模型选择合适的滤波器了。