视觉SLAM笔记（51） 非线性系统和 EKF
1. 非线性系统
必须要澄清一点： SLAM 中的运动方程和观测方程通常是非线性函数
尤其是视觉 SLAM 中的相机模型
需要使用相机内参模型以及李代数表示的位姿，更不可能是一个线性系统
一个高斯分布，经过非线性变换后，往往不再是高斯分布
所以在非线性系统中，必须取一定的近似，将一个非高斯的分布近似成一个高斯分布
2. 扩展卡尔曼滤波器
希望把卡尔曼滤波器的结果拓展到非线性系统中来
称为扩展卡尔曼滤波器（Extended Kalman Filter， EKF）
通常的做法是：
在某个点附近考虑运动方程以及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导
令 k−1 时刻的均值与协方差矩阵为
k−1,
k−1
在 k 时刻，把运动方程和观测方程，在
k−1,
k−1 处进行线性化（相当于一阶泰勒展开），有：
记这里的偏导数为：
同样的，对于观测方程，亦有：
记这里的偏导数为：
那么，在预测步骤中，根据运动方程有：
这些推导和卡尔曼滤波是十分相似的
为方便表述，记这里先验和协方差的均值为：
然后，考虑在观测中有：
最后，根据最开始的 贝叶斯展开式，可以推导出 xk 的后验概率形式
简而言之，会先定义一个卡尔曼增益 Kk：
在卡尔曼增益的基础上，后验概率的形式为：
在线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计
而在 SLAM 这种非线性的情况下，它给出了单次线性近似下最大后验估计（MAP）
3. EKF
EKF 以形式简洁、应用广泛著称
当想要在某段时间内估计某个不确定量时，首先想到的就是 EKF
在早期的 SLAM 中， EKF 占据了很长一段时间的主导地位，研究者们讨论了各种各样滤波器在 SLAM 中的应用，如 IF（信息滤波器）、 IEKF[63]（Iterated KF）、 UKF（Unscented KF）和粒子滤波器， SWF（Sliding Window Filter） 等等，或者用分治法等思路改进 EKF 的效率
直至今日，尽管认识到非线性优化比滤波器占有明显的优势
但是在计算资源受限，或待估计量比较简单的场合， EKF 仍不失为一种有效的方式
EKF 的局限
-
首先，滤波器方法在一定程度上假设了马尔可夫性
也就是 k 时刻的状态只与 k − 1时刻相关，而与 k − 1 之前的状态和观测都无关（或者和前几个有限时间的状态相关）
这有点像是在视觉里程计中，只考虑相邻两帧关系一样
如果当前帧确实与很久之前的数据有关（例如回环），那么滤波器就会难以处理这种情况
而非线性优化方法则倾向于使用所有的历史数据
它不光考虑邻近时刻的特征点与轨迹关系，更会把考虑很久之前的状态也考虑进来
称为全体时间上的 SLAM（FullSLAM）
在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算 -
EKF 滤波器仅在 x^k−1 处做了一次线性化，然后就直接根据这次线性化结果，把后验概率给算了出来
这相当于在说，认为该点处的线性化近似，在后验概率处仍然是有效的
而实际上，当离开工作点较远的时候，一阶泰勒展开并不一定能够近似整个函数
这取决于运动模型和观测模型的非线性情况
如果它们有强烈的非线性，那线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似
这就是 EKF 的非线性误差，是它的主要问题所在
在优化问题中，尽管也做一阶（最速下降）或二阶（G-N 或 L-M）的近似
但每迭代一次，状态估计发生改变之后，会重新对新的估计点做泰勒展开，而不像 EKF 那样只在固定点上做一次泰勒展开
这就导致优化方法适用范围更广，则在状态变化较大时亦能适用扫描二维码关注公众号，回复： 8638028 查看本文章
-
从程序实现上来说， EKF 需要存储状态量的均值和方差，并对它们进行维护和更新
如果把路标也放进状态的话，由于视觉 SLAM 中路标数量很大，这个存储量是相当可观的，且与状态量呈平方增长（因为要存储协方差矩阵）
因此， EKF SLAM 普遍被认为不可适用于大型场景
参考：
相关推荐：
视觉SLAM笔记（50） 线性系统和 KF
视觉SLAM笔记（49） 后端状态估计
视觉SLAM笔记（48） 局部地图
视觉SLAM笔记（47） 优化 PnP 的结果
视觉SLAM笔记（46） 基本的 VO
谢谢！
