IMU初始化

IMU初始化是为了给局部BA和全局BA提供一个更好的初值从而减少IMU噪声积累。

IMU初始化分解为多个子问题：

1. 估计陀螺仪偏置
2. 忽略加速度计偏置，估计尺度和重力矢量
3. 估计加速度计偏置，进一步优化尺度和重力
4. 估计速度

1. 陀螺仪偏置估计

  初始化过程假定$b^g$保持不变，每帧的偏置恒定：$b_i^g=b^g$。优化$b^g$,最小化所有相邻关键帧对之间的陀螺仪（旋转）预积分及从纯视觉ORB-SLAM2直接视觉计算的旋转之间的误差。
$${}^{\ast }{b}^g={\mathrm{argmin}}_{\theta }\sum _{k=1}^{N-1}\Vert Log((\Delta {\tilde{R}}_{k,k+1}Exp(J_{\Delta }^g{b}^g){)}^T{R}_{BW}^{k+1}{R}_{WB}^k){\Vert }^2$$  其中$\Delta {\tilde{R}}_{k,k+1}=Exp((\widetilde{{\omega }_k}-b^g)\Delta t)$,为陀螺仪的预积分值。N为关键帧的数量。$R_{WB}^{(\cdot )}=R_{WC}^{(\cdot )}\times R_{CB}^{(\cdot )}$,$R_{WC}^{(\cdot )}$为ORB-SLAM视觉计算的位姿，$R_{CB}^{(\cdot )}$由标定得到。$b^g$从0开始迭代。

2.尺度及重力估计

  忽略加速度计偏置$b_i^a=b^a=0$，由第一步得到的陀螺仪偏置，可以预积分速度及位移$\Delta v_{ij},\Delta p_{ij}$：
$$\begin{gathered} \Delta v_{ij}=\sum _{k=i}^{j-1}\Delta R_{ik}{\tilde{a}}_k\Delta t \\ \Delta p_{ij}=\sum _{k=i}^{j-1}\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}{\tilde{a}}_k\Delta t^2 \\ \Delta R_{ij}=\prod _{k=i}^{j-1}Exp(({\tilde{w}}_k-b^g)\Delta t) \end{gathered}$$

  视觉观测尺度不确定，有$T_{WC}=\left[\begin{array}{cc}{R}_{WC}& s_W{p}_C\\ 0^T& 1\end{array}\right]$,因此，由camera系到body系的转化过程中，有：
$$T_{WB}=\left[\begin{array}{cc}{R}_{WB}& {}_W{p}_B\\ 0^T& 1\end{array}\right]=T_{WC}{T}_{CB}=\left[\begin{array}{cc}{R}_{WC}& s_W{p}_C\\ 0^T& 1\end{array}\right]\cdot \left[\begin{array}{cc}{R}_{CB}& {}_C{p}_B\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{WC}{R}_{CB}& R_{WC}{}_C{p}_B+s_W{p}_C\\ 0^T& 1\end{array}\right]$$  其中$T_{CB}$为标定得到，即$${}_W{p}_B=R_{WC}{}_C{p}_B+s_W{p}_C$$  代入位置关联方程：
$${}_W{p}_B^{i+1}{=}_W{p}_B^i{+}_W{v}_B^i\Delta t_{i,i+1}+\frac{1}{2}{g}_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}$$  得到：$$s_W{p}_C^{i+1}=s_W{p}_C^i{+}_W{v}_B^i\Delta t_{i,i+1}+\frac{1}{2}{g}_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}+(R_{WC}^i-R_{WC}^{i+1}){}_C{p}_B$$  其中，${}_W{p}_C^i,R_{WC}^i,R_{WC}^{i+1}{,}_W{p}_C^{i+1}$由视觉计算得到，$\Delta t_{i,i+1}$已知，$\Delta p_{i,i+1}$由预积分得到,$R_{CB}{,}_C{p}_B$为标定得到。解该线性方程组得到$s、g_W$，然而方程中含有${}_W{v}_B^i$,为避免计算这N个速度，降低复杂度。我们将连续的三帧提取两个关系，利用速度关联方程：$${}_W{v}_B^{i+1}{=}_W{v}_B^i+g_W\Delta t_{i,i+1}+R_{WB}^i\Delta v_{i,i+1}$$  为简化表述，使用1，2，3代替$i,i+1,i+2$，有：
$$\begin{gathered} s_W{p}_C^3=s_W{p}_C^2{+}_W{v}_B^2\Delta t_{23}+\frac{1}{2}{g}_W\Delta t_{23}^2+R_{WB}^2\Delta p_{23}+(R_{WC}^2-R_{WC}^3){}_C{p}_B \\ {s}_W{p}_C^2=s_W{p}_C^1{+}_W{v}_B^1\Delta t_{12}+\frac{1}{2}{g}_W\Delta t_{12}^2+R_{WB}^1\Delta p_{12}+(R_{WC}^1-R_{WC}^2){}_C{p}_B \\ {}_W{v}_B^2{=}_W{v}_B^1+g_W\Delta t_{12}+R_{WB}^1\Delta v_{12} \end{gathered}$$  第一式乘$\Delta t_{12}$与第二式乘$\Delta t_{23}$做差，结合代入第三式，得到：
$$\left(\begin{array}{cc}\lambda (i)& \beta (i)\end{array}\right)\left(\begin{array}{c}s\\ g_W\end{array}\right)=\gamma (i)$$

  其中，$\lambda (i)$为3×1的矩阵，$\beta (i)$为3×3矩阵，$\left(\begin{array}{c}s\\ g_W\end{array}\right)$为4×1矩阵，$\gamma (i)$为3×1矩阵。写成$AX=B$的形式，$A_{3\times 4},X_{4\times 1},B_{3\times 1}$，四个未知数，一组关键帧（3帧）有三个方程，所以至少需要两个关键帧组（至少4帧）。若N个关键帧，有N-2组，则$A_{3(N-2)\times 4},X_{4\times 1},B_{3(N-2)\times 1}$,可以使用最小二乘或SVD求解。

3.加速度计偏置估计及尺度、重力修正

  由于在之前的估计中忽略了加速度计偏置，导致我们计算的重力实际上也混杂了偏置的影响，与实际的重力方向有偏差。引入惯性系$I$以及实际重力$G$，在惯性系下实际重力的方向向量坐标为${\hat{g}}_I=(0,0,-1)$，我们估计的重力在世界系下的方向为$\widehat{g_W}=\frac{g_W^{\ast }}{\Vert g_W^{\ast }\Vert }$.由此，我们可以计算惯性系到世界系的旋转矩阵：
$$\begin{gathered} R_{WI}=Exp(\hat{v}\theta ) \\ \hat{v}=\frac{{\hat{g}}_I\times {\hat{g}}_W}{\Vert {\hat{g}}_I\times {\hat{g}}_W\Vert },\theta =atan2(\Vert {\hat{g}}_I\times {\hat{g}}_W\Vert ,{\hat{g}}_I\cdot {\hat{g}}_W) \end{gathered}$$  将实际重力惯性系坐标旋转到世界系中，即实际重力在世界系下的坐标为$$g_W=R_{WI}{\hat{g}}_{I}G$$  但是由于我们估计的重力与实际重力是有偏差的，所以旋转矩阵$R_{WI}$并不是实际两坐标系的旋转矩阵，需要加一个修正量$\delta \theta$。
  另外由于惯性系中重力方向沿z方向，所以沿z轴的旋转是没有影响的。旋转轴在xy平面，只有沿x轴与y轴的旋转分量，没有z轴的旋转分量，即$\delta \theta =(\delta {\theta }_x\delta {\theta }_{y}0{)}^T$,综上，有：$$\begin{gathered} g_W=R_{WI}Exp(\delta \theta ){\hat{g}}_{I}G \\ \delta \theta =(\delta {\theta }_{xy}^{T}0{)}^T=(\delta {\theta }_x\delta {\theta }_{y}0{)}^T \end{gathered}$$  一阶近似$$g_W\simeq R_{WI}(I+\delta {\theta }^{\wedge }){\hat{g}}_{I}G=R_{WI}{\hat{g}}_{I}G-R_{WI}{\hat{g}}_I^{\wedge }G\delta \theta$$  除了引入重力修正，我们同时考虑加速度计偏置的影响。代入位置关联方程：$$s_W{p}_C^{i+1}=s_W{p}_C^i{+}_W{v}_B^i\Delta t_{i,i+1}-\frac{1}{2}{R}_{WI}{\hat{g}}_I^{\wedge }G\Delta t_{i,i+1}^2\delta \theta +R_{WB}^i(\Delta p_{i,i+1}+J_{\Delta p}^a{b}^a)+(R_{WC}^i-R_{WC}^{i+1}){}_C{p}_B+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{i,i+1}^2$$  这样，我们就引入了重力方向的修正和加速度计偏置。
  同样地，我们考虑连续的三个连续帧，避免速度计算,并简化表述：
$$\begin{gathered} s_W{p}_C^3=s_W{p}_C^2{+}_W{v}_B^2\Delta t_{23}-\frac{1}{2}{R}_{WI}{\hat{g}}_I^{\wedge }G\Delta t_{23}^2\delta \theta +R_{WB}^2(\Delta p_{23}+J_{\Delta p23}^a{b}^a)+(R_{WC}^2-R_{WC}^3){}_C{p}_B+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{23}^2 \\ {s}_W{p}_C^2=s_W{p}_C^1{+}_W{v}_B^1\Delta t_{12}-\frac{1}{2}{R}_{WI}{\hat{g}}_I^{\wedge }G\Delta t_{12}^2\delta \theta +R_{WB}^1(\Delta p_{12}+J_{\Delta p12}^a{b}^a)+(R_{WC}^1-R_{WC}^2){}_C{p}_B+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{12}^2 \\ {}_W{v}_B^2{=}_W{v}_B^1+g_W\Delta t_{12}+R_{WB}^1(\Delta v_{12}+J_{\Delta v12}^a{b}^a) \end{gathered}$$  得到

  其中，$\lambda (i)$为3×1的矩阵，$\varphi (i)$为3×2矩阵，$\zeta (i)$为3×3矩阵，$\left(\begin{array}{c}s\\ \delta {\theta }_{xy}\\ b^a\end{array}\right)$为6×1矩阵，$\psi (i)$为3×1矩阵。写成$AX=B$的形式，$A_{3\times 6},X_{6\times 1},B_{3\times 1}$，六个未知数，一组关键帧（3帧）有三个方程，所以至少需要两个关键帧组（至少4帧）。若N个关键帧，有N-2组，则$A_{3(N-2)\times 6},X_{6\times 1},B_{3(N-2)\times 1}$,可以使用最小二乘或SVD求解。

4.估计速度

  尺度，重力，陀螺仪、加速度计偏置已知，可以代入原位置关联方程求解速度
$$s_W{p}_C^{i+1}=s_W{p}_C^i{+}_W{v}_B^i\Delta t_{i,i+1}-\frac{1}{2}{R}_{WI}{\hat{g}}_I^{\wedge }G\Delta t_{i,i+1}^2\delta \theta +R_{WB}^i(\Delta p_{i,i+1}+J_{\Delta p}^a{b}^a)+(R_{WC}^i-R_{WC}^{i+1}){}_C{p}_B+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{i,i+1}^2$$$$s_W{p}_C^{i+1}=s_W{p}_C^i{+}_W{v}_B^i\Delta t_{i,i+1}+\frac{1}{2}{g}_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}+(R_{WC}^i-R_{WC}^{i+1}){}_C{p}_B$$  求解邻近关键帧速度，使用速度关联方程：$${}_W{v}_B^{i+1}{=}_W{v}_B^i+g_W\Delta t_{i,i+1}+R_{WB}^i(\Delta v_{i,i+1}+J_{\Delta v}^g{b}^g+J_{\Delta v}^a{b}^a)$$

5.重定位后偏置重新初始化

  当系统运行一段时间后根据位置辨识重定位，需要对偏置进行重新初始化。陀螺仪偏置仍然是求解最优旋转误差方程，加速度偏置通过求解简化的3中的方程组，其中s和$g_W$已知。