代码解析_VINS公式推导与代码解析

介绍一个大牛的总结StevenCui，里面有包括MSCKF, ROVIO, ICE_BA, VINS的公式推导和代码解析。这篇就主要介绍MSCKF。

主流VIO

崔神从 耦合方案， 后端方案， 前端，视觉误差，初始化，回环，精度和效率角度总结了VIO主流方案视频讲解。

耦合方案

• 松耦合

将视觉约束后的位姿加入到联合优化是松耦合

• 紧耦合

将视觉约束加入到联合优化是紧耦合

后端优化方案

• 滤波
• 图优化

VIO坐标系

在 VIO器件中，通常集成了相机和IMU器件，当这个VIO传感器在世界中运动时，我们就有了世界坐标系（world, W坐标系），IMU坐标系（Body, B坐标系），相机坐标系（Camera, C坐标系）。如果仅使用视觉，就只需要计算相机到世界的坐标系变换也就是

,仅仅使用IMU的话就只存在IMU到世界坐标系的变换

,使用p表示平移。

IMU 测量

IMU的测量数据为在IMU(B)坐标系下的角速度和加速度，测量信号受噪声和零偏的影响。

状态变量

为了估计位姿，首先要选择状态变量，在紧耦合的方案中一般选择位姿，速度，零偏这几个量作为待估计的状态变量，共15维。

动力学方程，位移的微分为速度，速度的微分为加速度，旋转的微分为角速度。将其写成积分形式，然后由于是离散时刻进行采样，所以得到的是离散时刻的动力学方程，在带入IMU的测量有：

在预积分的论文中，Forster定义了俩个关键帧之间的误差

,这几个量即可以用IMU的测量数据表达，又可以通过两个关键帧的状态量表达，所以，我们可以定义观测和状态之间的残差，然后构建最小二乘求解。接下来的问题就是要考虑，关键帧之间的运动约束受什么噪声影响？IMU的测量噪声受高斯噪声和随机游走噪声影响，但是这里定义的观测量是IMU测量量的复杂非线性组合，于是可以通过一阶泰勒展开保留一阶项，实现对噪声的线性化。