MSCKF_VIO算法框架

SCKF_VIO算法
1、MSCKF视觉惯性融合算法简介
惯性导航利用惯性测量单元(IMU)测量得到的角速度、加速度信息进行惯性导航解算得到运载体的位置、速度、姿态(含航向)等信息，具有实时性好、动态性能好等优点；但是由于其积分式特点，使得传感器和算法解算的误差会持续累积，导致长时间精度很低，特别是对于低端IMU。即使是军用战略性武器上使用的高精度惯导系统也在努力与别的信息进行融合，提高实际导航精度。视觉信息包含了丰富的三维场景信息，通过视觉跟踪可以对运载体的运动进行测量，视觉测量可以给出运动增量。惯性与视觉融合算法利用视觉观测与IMU预测联合得到重投影误差作为观测量来进行系统状态估计。分为滑动窗SWF与多状态约束卡尔曼滤波MSCKF两种技术方案。作为惯性导航专业背景的从业人员，比较青睐基于卡尔曼滤波的MSCKF算法。这里给出msckf_vio_mono算法的数学推导过程。这里的公式表达和推导过程按照惯性导航的习惯，对于非惯性导航专业的可能会感觉有点陌生。
2、MSCKF视觉前端
这里给出msckf_vio_mono代码里给出的视觉前端算法模型。

3、MSCKF滤波更新