视觉SLAM ch9 卡尔曼和扩展卡尔曼滤波

状态估计的概率解释：位姿x和路标y服从某种概率分布，目的是通过某些运动数据u（比如惯性测量传感器IMU输入）和观测数据z（比如拍摄到的照片像素点的值）来确定状态量x和y的分布。

一、关于卡尔曼滤波器和扩展卡尔曼滤波器的推导有博主写过，过程很详细。

(7条消息) SLAM14讲学习笔记（六）后端（最难一章：卡尔曼滤波器推导、理解以及扩展）_zkk9527的博客-CSDN博客_swm恒等式

二、关于扩展卡尔曼滤波器的局限性

1. 假设了马尔可夫性，认为k时刻状态只与k-1时刻有关；
2. SLAM非线性较强，EKF只做了一次线性化，非线性误差较大
3. 在推导中我们x的记号中实际包含了相机位姿和路标点，随着相机的运动，新增路标点越来越多，存储的状态量平方增长（因为存储协方差矩阵），因此不适用于大尺度场景。
4. EKF没有异常检测机制，但是在SLAM中误匹配十分常见

因此，目前采用非线性优化是更好的选择。

三、非线性优化——BA优化

ch7中也用到过BA，这里和之前的区别在于，之前的BA中误差项都是针对于单个位姿和路标点的，但是在整体的BA目标函数上，必须把自变量定义成所有待优化的变量。也就是说，这章讲的是所有的误差合并到一起，做一个整体的矩阵进行优化。

这部分内容高博书讲的很详细，重点是H矩阵的稀疏性以及Schur消元求解   

四、鲁棒核函数

BA中存在一个严重的问题：如果处于误匹配等原因，明明某个相机位姿处没有观测到路标点 pj ，但是误以为观测到了（表现在图中就是多加了一个本不应该存在的边），于是在BA过程中会产生一个误差极大的一项，于是为了减小这一项的误差，会使得其他正确的边全都受很大影响。

根本原因在于：当误差很大时，二范数增长太快，因此二范数容易受到某一个极大误差项的影响。

解决方法：采用鲁棒核函数，让阈值δ内使用二范数，阈值δ外使用其他增长慢的范数。