看完这篇博客建议再看:http://www.xinliang-zhong.vip/msckf_notes/
目录
一、简介
MSCKF 是明尼苏达州大学 Mourikis 等人提出的一种基于 EKF 的 VIO 紧耦 合的 SLAM 框架[1][2]。该框架的最大创新点在于并未将路标点的位置加入到状 态向量中(因为加进去会导致状态向量一直增加,效率会越来越慢),而是等某 个路标点不见或者太老时,先通过 GN 优化方法计算出该路标点的 3D 位置,然 后将多个相机对这个路标点的观测作为一种约束,整合到 EKF 更新中,这样既 不损失信息,又不增加状态向量,有点像边缘化 Marg 的思路。
MSCKF 算法流程如下图所示,分为预测、图像注册、更新:
二、符号说明
三、状态向量
3.1 真实状态向量 true state
当前帧即第
k
时刻的所有误差状态向量,包括两部分:当前第
k
时刻的
IMU 状态,和 N
个相机位姿:
3.2 误差状态向量 error state
四、IMU 预测
4.1 连续形式的误差运动方程
4.2 连续形式的协方差传播
第
k
时刻的系统协方差矩阵为:
下一个 IMU 来时,即 k+1 时刻的系统协方差可写为:
五、相机位姿状态增广
5.1 状态向量增广
当获得一张新的相机图像时,需要将相机位姿加入到当前状态向量中,相机位姿可以通过 IMU
估计出来 (
相机和IMU建立的联系
):
5.2 协方差矩阵增广
当来一帧新图像后,新的协方差矩阵可写成:
六、视觉测量模型
6.1 视觉测量残差
6.2 残差线性化近些
6.3 边缘化路标点的位置误差
对于
EKF
,残差线性化需要满足如下形式,即残差与误差项成线性化关系, 且噪声项为与状态向量无关的零均值的高斯分布:
下图给出了直接将公式
(32)
用于
EKF
的结果,对比公式
(34)
的结果可发现, 当噪声项与状态向量相关时,EKF
估计出来的结果将偏移得更严重
[5]
。
七、更新
八、参考文献
[1] A. I. Mourikis. A multi-state constraint kalman filter for vision-aided inertial
navigation. Proc. ICRA, 2007.
[2] A. I. Mourikis. A multi-state constraint kalman filter for vision-aided inertial
navigation. 2006. Dept. of Computer Science and Engineering, University of Minnesota,
Tech. Rep.
[3] F. Gonzalez. Visual inertial odometry for mobile robotics. 2015.
[4] N. Trawny. Indirect kalman filter for 3D attitude estimation. 2005.
[5] L. Clement. The battle for filter supremacy: a comparative study of the multi-state
constraint kalman filter and the sliding window filter. CVR. 2015