IMU流形预积分

预备知识

1.IMU测量模型（B系）${}_B\tilde{\omega }{,}_B\tilde{a}$

2.世界系下运动模型（W系）$R_{WB}^W{,}_{W}v{,}_{W}p$

2.1 运动模型

2.2 IMU测量帧运动积分

2.3 离散形式（单位时间间隔保持加速度不变）

2.4 代入测量值

3.两视觉关键帧间运动积分（IMU测量帧叠加）（W系）

4.消除线性化时间点的影响，定义相对运动增量（B系）$\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}$

其中应该为$\frac{1}{2}g\Delta t_{ij}^2$

5.分离测量值和噪声$\Delta \widetilde{R_{ij}},\Delta \widetilde{v_{ij}},\Delta \widetilde{p_{ij}},\delta {\varphi }_{ij},\delta v_{ij},\delta p_{ij}$

6.预积分测量模型

7.噪声信息矩阵求解 ${\eta }_{ij}^{\Delta }={\left[\begin{array}{ccc}\delta {\varphi }_{ij}^T& \delta v_{ij}^T& \delta p_{ij}^T\end{array}\right]}^T,{\eta }_k^d=\left[\begin{array}{cc}{\eta }_k^{gd}& {\eta }_k^{ad}\end{array}\right]$

7.1 噪声分布形式

7.2 噪声增量式更新

7.3 信息矩阵更新

8. bias更新时测量值的更新 $\Delta \widetilde{\hat{R_{ij}}},\Delta \widetilde{\stackrel{ˉ}{R_{ij}}},\Delta \widetilde{\hat{v_{ij}}},\Delta \widetilde{\stackrel{ˉ}{v_{ij}}},\Delta \widetilde{\hat{p_{ij}}},\Delta \widetilde{\stackrel{ˉ}{p_{ij}}},\delta b_i^g,\delta b_i^a$

9. 残差及Jacobians计算

残差$r_{\Delta p_{ij}},r_{\Delta v_{ij}},r_{\Delta R_{ij}}$（B系）

状态变量$R_i,v_i,p_i,R_j,v_j,p_j,\delta b_i^g,\delta b_i^a$ (W系)

增量${}_B\delta {\varphi }_{b_k{b}_{k+1}}{,}_B\delta p_{b_k{b}_{k+1}}{,}_W\delta v_{b_k{b}_{k+1}},\widetilde{\delta b^g},\widetilde{\delta b^a}$

9.1 残差计算

9.2 状态变量及对应增量

9.3 残差关于状态变量的Jacobians

(1) $r_{\Delta p_{ij}}$

(2) $r_{\Delta v_{ij}}$

(3) $r_{\Delta R_{ij}}$