ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解2——IMU初始化

3.IMU初始化（视觉惯性联合初始化）
终于来到视觉惯性初始化阶段了，这段是视觉和惯性数据融合的第一步，是一段松耦合过程。理论上是将准确的视觉数据（但缺少尺度）和快速的IMU数据（需要重力加速度又存在零偏误差）相结合。具体介绍在VINS的博客中也同样说过，这部分关于最小二乘、尺度收敛等问题请参考我的博客：视觉SLAM常见的QR分解SVD分解等矩阵分解方式求解满秩和亏秩最小二乘问题（最全的方法分析总结）
https://blog.csdn.net/wangshuailpp/article/details/80209863
（1）陀螺仪偏置标定（零偏）
这一部分比较简单，直接联立N-1个相机做旋转矩阵的最小二乘即可，然后通过高斯牛顿方法即可得到零偏bg。需要注意一点，当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确。

（2）尺度恢复和重力加速度预估
首先建立预估状态向量X=[s,gw]，其中s是尺度，gw是世界坐标系下的重力加速度也是第一个相机坐标系下的重力加速度。ORB_SLAM2中世界坐标选取的是第一个相机对应的坐标（VINS则不同），这样做会存在一个问题，因为第一个相机可能自身存在一定的旋转倾斜导致整个世界坐标看起来是歪着的，画轨迹的时候有一种倾斜的即视感，所以我觉得还是尽量固定好z方向，使轨迹没有横滚和俯仰。这里使用了三个关键帧联立视觉和IMU预积分数据构建一个AX=B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但精度高。


（3）加速度计偏置标定和尺度重力加速度优化
上面计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，很有可能会导致系统病态性问题，文中提出了重力加速度的大小G，假设其是一个不变值，优化重力加速度的方向。

最后优化的状态量变成了X=[s,deta_θ,ba],也就是说我们只优化重力加速度的方向，每次优化得到一个deta_θ都会代入到论文中的公式（17）计算出最新的gw然后继续优化，直到变量收敛。