MSCKF 公式推导

看完这篇博客建议再看：http://www.xinliang-zhong.vip/msckf_notes/

目录

一、简介

二、符号说明

三、状态向量

3.1 真实状态向量 true state

3.2 误差状态向量 error state

四、IMU 预测

4.1 连续形式的误差运动方程

4.2 连续形式的协方差传播

五、相机位姿状态增广

5.1 状态向量增广

5.2 协方差矩阵增广

六、视觉测量模型

6.1 视觉测量残差

6.2 残差线性化近些

6.3 边缘化路标点的位置误差

七、更新

八、参考文献

MSCKF 是明尼苏达州大学 Mourikis 等人提出的一种基于 EKF 的 VIO 紧耦 合的 SLAM 框架[1][2]。该框架的最大创新点在于并未将路标点的位置加入到状 态向量中（因为加进去会导致状态向量一直增加，效率会越来越慢），而是等某 个路标点不见或者太老时，先通过 GN 优化方法计算出该路标点的 3D 位置，然 后将多个相机对这个路标点的观测作为一种约束，整合到 EKF 更新中，这样既 不损失信息，又不增加状态向量，有点像边缘化 Marg 的思路。

MSCKF 算法流程如下图所示，分为预测、图像注册、更新：

下一个 IMU 来时，即 k+1 时刻的系统协方差可写为：

当来一帧新图像后，新的协方差矩阵可写成：