MSCKF算法流程框架

1. 初始化

   • 摄像机参数、
     • 噪声方差（图像噪声、IMU噪声、IMU的bias）、
     • 初始的IMU协方差、
     • IMU和摄像机的外参数、
     • IMU和摄像机的时间偏移量*
   • MSCKF参数：
     + 状态向量里滑动窗口大小的范围、
     + 空间点三角化误差阈值、
     + 是否做零空间矩阵构造 和 QR分解
   • 构造MSCKF状态向量

2. IMU数据，估计新的MSCKF状态变量和对应的协方差矩阵

3. 图像数据处理

   • MSCKF状态向量 中 增加当前帧的摄像机位姿；
     若位姿数大于滑动窗口大小的范围，
     去除状态变量中最早的视图对应的摄像机位姿.
   • 提取图像特征并匹配，去除外点.
   • 处理所有提取的特征。
     判断当前特征是否是之前视图中已经观察到的特征
     • 如果当前帧还可以观测到该特征，则加入该特征的track列表
     • 如果当前帧观测不到该特征(Out_of_View),将该特征的track加入到featureTracksToResidualize，用于更新MSCKF的状态变量.
     • *给该特征分配新的featureID，并加入到当前视图可观测特征的集合
   • 循环遍历featureTracksToResidualize中的track，用于更新MSCKF的状态变量*
     • 计算每个track对应的三维空间点坐标 (利用第一幅视图和最后一幅视图计算两视图三角化，使用逆深度参数化和高斯牛顿优化求解)，若三角化误差小于设置的阈值，则加入map集合.
     • 计算视觉观测(即图像特征)的估计残差，并计算图像特征的雅克比矩阵.
     • 计算图像特征雅克比矩阵的左零空间矩阵和QR分解，构造新的雅克比矩阵.
   • 计算新的MSCKF状态向量的协方差矩阵.
     • 计算Kalman增益.
     • 状态矫正.
     • 计算新的协方差矩阵.
   • 状态变量管理
     • 查找所有无feature track可见的视图集合deleteIdx.
     • 将deleteIdx中的视图对应的MSCKF中的状态去除掉.
     • 绘制运动轨迹.