视觉SLAM笔记(51) 非线性系统和 EKF

视觉SLAM笔记(51) 非线性系统和 EKF


1. 非线性系统

必须要澄清一点: SLAM 中的运动方程和观测方程通常是非线性函数
尤其是视觉 SLAM 中的相机模型
需要使用相机内参模型以及李代数表示的位姿,更不可能是一个线性系统

一个高斯分布,经过非线性变换后,往往不再是高斯分布
所以在非线性系统中,必须取一定的近似,将一个非高斯的分布近似成一个高斯分布


2. 扩展卡尔曼滤波器

希望把卡尔曼滤波器的结果拓展到非线性系统中来
称为扩展卡尔曼滤波器(Extended Kalman Filter, EKF)

通常的做法是:
在某个点附近考虑运动方程以及观测方程的一阶泰勒展开,只保留一阶项,即线性的部分,然后按照线性系统进行推导
令 k−1 时刻的均值与协方差矩阵为 x^\hat{x}k−1, P^\hat{P}k−1
在 k 时刻,把运动方程和观测方程,在 x^\hat{x}k−1, P^\hat{P}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 的局限

  1. 首先,滤波器方法在一定程度上假设了马尔可夫性
    也就是 k 时刻的状态只与 k − 1时刻相关,而与 k − 1 之前的状态和观测都无关(或者和前几个有限时间的状态相关)
    这有点像是在视觉里程计中,只考虑相邻两帧关系一样
    如果当前帧确实与很久之前的数据有关(例如回环),那么滤波器就会难以处理这种情况
    而非线性优化方法则倾向于使用所有的历史数据
    它不光考虑邻近时刻的特征点与轨迹关系,更会把考虑很久之前的状态也考虑进来
    称为全体时间上的 SLAM(FullSLAM)
    在这种意义下,非线性优化方法使用了更多信息,当然也需要更多的计算

  2. EKF 滤波器仅在 x^k−1 处做了一次线性化,然后就直接根据这次线性化结果,把后验概率给算了出来
    这相当于在说,认为该点处的线性化近似,在后验概率处仍然是有效的
    而实际上,当离开工作点较远的时候,一阶泰勒展开并不一定能够近似整个函数
    这取决于运动模型和观测模型的非线性情况
    如果它们有强烈的非线性,那线性近似就只在很小范围内成立,不能认为在很远的地方仍能用线性来近似
    这就是 EKF 的非线性误差,是它的主要问题所在
    在优化问题中,尽管也做一阶(最速下降)或二阶(G-N 或 L-M)的近似
    但每迭代一次,状态估计发生改变之后,会重新对新的估计点做泰勒展开,而不像 EKF 那样只在固定点上做一次泰勒展开
    这就导致优化方法适用范围更广,则在状态变化较大时亦能适用

  3. 从程序实现上来说, EKF 需要存储状态量的均值和方差,并对它们进行维护和更新
    如果把路标也放进状态的话,由于视觉 SLAM 中路标数量很大,这个存储量是相当可观的,且与状态量呈平方增长(因为要存储协方差矩阵)
    因此, EKF SLAM 普遍被认为不可适用于大型场景


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(50) 线性系统和 KF
视觉SLAM笔记(49) 后端状态估计
视觉SLAM笔记(48) 局部地图
视觉SLAM笔记(47) 优化 PnP 的结果
视觉SLAM笔记(46) 基本的 VO


谢谢!

发布了233 篇原创文章 · 获赞 322 · 访问量 296万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览