超全！SLAM中的滤波算法大全

快速获得最新干货

本文转载自自动驾驶与AI

概率论基础

边缘概率

边缘概率是相对于联合概率而言的的，虽然你有两个变量（x,y）但是你可以只考虑x或者y的分部，好像另外一个不存在一样，写作 P(x) 或者 P(y) 。

离散概率和为1，即：

为了简化符号，在可能时通常省略随机变量的明确表示，而是使用常见的缩写 P(x) 代替 P(X=x) 。

联合概率和独立

两个随机变量X和Y的联合概率表示两件事都发生的概率，由下式给出：

如果X和Y相互独立，有 P(x,y)=P(x)P(y).

独立与条件独立

左边是独立的定义，右边是条件独立的定义。可以看出：条件独立是指在某个条件已知的前提下独立。

注意：独立与条件独立不能互推。

https://zhuanlan.zhihu.com/p/58593725

条件独立的理解

“

如果 A,B 关于 C 条件独立：C 的发生，使本来不独立的 A 和 B 变得独立起来，也即事件 C 的发生，解开了 A 和 B 的依赖关系；

”

条件概率

假定已经知道Y=y，想知道基于以上事实条件X=x的概率。这样的概率表示为

称为条件概率。

如果p(y)>0，则条件概率定义为 .

“

通过统计方法分析事件发生关系的占比，以做到已知结果倒推原因。

”

如果X和Y相互独立，则有 .

联合概率、边缘概率与条件概率之间的关系

将概率转化为面积来理解：

“

联合概率P(X=a,Y=b)满足X=a且Y=b的面积 边缘概率P(X=a)不考虑Y的取值，所有满足X=a的区域的总面积 条件概率P(X=a|Y=b)在Y=b的前提下，满足X=a的面积（比例）

”

三种概率的关系

全概率定理

从条件概率和概率测量公理得出的一个有趣事实经常被称为全概率定理：

贝叶斯定理

https://www.cnblogs.com/HuZihu/p/9368355.html

该定理将条件概率p(x|y)与其“逆”概率p(ylx)联系起来。如此处所阐述的，准则要求p(y)>0:

如果x是一个希望由y推测出来的数值，其中y称为数据(data)（结果），也就是传感器测量值。P(x) 是先验概率（prior probability）， P(y|x) 是条件概率（conditional probability）（原因）， P(x}y) 是后验概率（posterior probability）。

“

SLAM中很多理论，诸如滤波方法、BA和图优化等，都是基于贝叶斯定理的思想而来的。

”

贝叶斯滤波

贝叶斯滤波基于马尔可夫假设。

马尔可夫假设

“

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

”

公式展示

公式推导

证明来自《概率机器人》p23~25页，用到了马尔可夫假设、全概率定理、条件独立、贝叶斯定理等。

卡尔曼滤波（KF和EKF）

https://zhuanlan.zhihu.com/p/558449409

EKF局限性

所以EKF面临的一个重要问题是，当一个高斯分布经过非线性变换后，如何用另一个高斯分布近似它？按照它现在的做法，存在以下的局限性：（注意是滤波器自己的局限性，还没谈在SLAM问题里的局限性）。

1. 即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
2. 系统本身线性化过程中，丢掉了高阶项。
3. 线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的，也不是最好的。
4. 在估计非线性输出的均值时，EKF算的是的形式。这个结果几乎不会是输出分布的真正期望值。协方差也是同理。

解决办法

那么，怎么克服以上的缺点呢？途径很多，主要看我们想不想维持EKF的假设。如果我们只是希望维持高斯分布假设，可以这样子改：

1. 为了克服第2条，可以使用ESKF。它的线性化是总是在0附近，并且雅可比矩阵的形式简单，计算迅速。因此线性化比EKF更准确。虽然，ESKF主要用于有imu的系统。
2. 为了克服第3条工作点的问题，我们以EKF估计的结果为工作点，重新计算一遍EKF，直到这个工作点变化够小。是为迭代EKF（Iterated EKF, IEKF）。
3. 为了克服第4条，我们除了计算 ， f()再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。
4. 如果我们不要高斯分布假设，凭什么要用高斯去近似一个长得根本不高斯的分布呢？于是问题变为，丢掉高斯假设后，怎么描述输出函数的分布就成了一个问题。一种比较暴力的方式是：用足够多的采样点，来表达输出的分布。这种蒙特卡洛的方式，也就是粒子滤波的思路。
5. 如果再进一步，可以丢弃滤波器思路，说：为什么要用前一个时刻的值来估计下一个时刻呢？我们可以把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型。这样就会得到非线性优化的方法，在SLAM里就走向图优化那条路上去了。不过，非线性优化也需要对误差函数不断地求梯度，并根据梯度方向迭代，因而局部线性化是不可避免的。

无迹卡尔曼滤波（UKF）

https://blog.csdn.net/gangdanerya/article/details/105215446

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

“

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

”

UT变换

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

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

Sigma点采样策略

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

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

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

其中11行，参考 Will：EKF公式推导 中的证明的协方差的其他形式。

UKF采样原则

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

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

UKF的特点

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

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

信息滤波（IF）

卡尔曼滤波(KF)的对偶滤波算法就是信息滤波(Information Filter，IF)，形式和KF差不多。

其中，用信息向量 和信息矩阵 表示KF中的 和.转换关系为：

Error-state卡尔曼滤波（ESKF）

ESKF(error state Kalman filter)是Kalman滤波的一种特殊形式，采用ESKF的原因是由于对误差的线性化要比直接对函数进行线性化更加符合实际情况。如下图：

状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF。

ESKF数学模型

Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：

• true state：实际状态，系统实际的运行状态
• nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
• error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）

基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。

ESKF的全过程

• 对IMU数据进行积分，获得名义状态X，注意这个X并没有考虑噪声，所以必然引入了累计误差；
• 利用KF算法估计Error State，包括状态更新和测量更新，这个过程是考虑了噪声的，而且由于这个误差状态的方程式近似线性的，直接使用KF就可以；
• 将Error State加到Nominal State上，获取“真值”；
• 重置Error State，等待下一次更新；

ESKF的优势

Error State中的参数数量与运动自由度是相等的，避免了过参数化引起的协方差矩阵奇异的风险；
Error State总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性；
Error State总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速；
Error State的变化平缓，因此KF修正的频率不需要太高。

公式推导

https://zhuanlan.zhihu.com/p/557496107

迭代卡尔曼滤波（IKF/IEKF）

多状态约束下的卡尔曼滤波（MSCKF）

粒子滤波（PF）

https://blog.csdn.net/weixin_38145317/article/details/82852580

总结和比较

卡尔曼滤波和其他方法的比较

1. 卡尔曼滤波是递归的线性高斯系统最优估计。
2. EKF将NLNG系统在工作点附近近似为LG进行处理。
3. IEKF对工作点进行迭代。
4. UKF没有线性化近似，而是把sigma point进行非线性变换后再用高斯近似。
5. PF去掉高斯假设，以粒子作为采样点来描述分布。
6. 优化方式同时考虑所有帧间约束，迭代线性化求解。

滤波和优化方法的区别

EKF的局限性

滤波方法（EKF）：

（1）滤波方法具有一阶马尔可夫假设，k时刻的状态只与k-1时刻相关。

（2）可以粗略认为EKF仅是优化中的一次迭代，除非是使用IKF/IEKF

（3）计算量小，实时性较好，容易积累误差。

优化方法（BA）：

（1）没有马尔可夫假设，更倾向于使用所有的历史数据。称为全体时间上的SLAM（Full-SLAM），计算量变大

（2）没有迭代次数的限制

（3）累计误差少。最理想的情况是回环检测之后，可以消除整个轨迹的误差。

（4）计算量大。不过目前基于雅各比和海森矩阵稀疏性的研究，使得计算资源的消耗大大降低。

“

下文采自：Sky Shaw：多传感器融合 ｜ 各类滤波器方法整理 https://zhuanlan.zhihu.com/p/542440266

”

卡尔曼滤波器：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验（马尔可夫假设）；
扩展卡尔曼滤波器：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性（马尔可夫假设）；
BA优化：把一路上的所有坐标点（像素坐标对应的空间点等）与位姿整体放在一起作为自变量进行非线性优化
PoseGraph优化：先通过一路递推方式算出的各点位姿，通过数学方式计算得到一个位姿的变换A，再通过单独拿出两张图像来算出一个位姿变换B，争取让B=A
增量因子图优化：保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，需要重新计算的再去计算，从而避免冗余计算。iSAM是增量的处理后端优化。由于机器人是运动的，不同的边和节点会被不断加入图中。每加入一个点，普通图优化是对整个图进行优化，所以比较麻烦和耗时。iSAM的话相当于是保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。以这种方式加速计算，避免冗余计算。

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

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

参考

Sky Shaw：多传感器融合 ｜ 各类滤波器方法整理 https://zhuanlan.zhihu.com/p/542440266

SLAM中的EKF，UKF，PF原理简介 - 半闲居士 - 博客园 https://www.cnblogs.com/gaoxiang12/p/5560360.html

SLAM 中的 Kalman Filter 推导
https://www.guyuehome.com/38267

学机器人SLAM/3D视觉，就上cvlife.net ！

点击领取学习资料 → 机器人SLAM学习资料大礼包

独家重磅课程官网：cvlife.net

全国最大的机器人SLAM开发者社区

技术交流群

—   版权声明  —