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 \tilde{R_{ij}},\Delta \tilde{v_{ij}},\Delta \tilde{p_{ij}},\delta\phi_{ij},\delta v_{ij},\delta p_{ij}$

6.预积分测量模型

7.噪声信息矩阵求解 $\eta_{ij}^{\Delta}= \left[ \begin{matrix} \delta\phi_{ij}^T&\delta v_{ij}^T& \delta p_{ij}^T\end{matrix} \right]^T,\eta_{k}^{d}= \left[ \begin{matrix}\eta_{k}^{gd}&\eta_{k}^{ad}\end{matrix} \right]$

7.1 噪声分布形式

7.2 噪声增量式更新

7.3 信息矩阵更新

8. bias更新时测量值的更新 $\Delta \tilde{\hat{R_{ij}}},\Delta \tilde{\bar{R_{ij}}},\Delta \tilde{\hat{v_{ij}}},\Delta \tilde{\bar{v_{ij}}},\Delta \tilde{\hat{p_{ij}}},\Delta \tilde{\bar{p_{ij}}},\delta b^g_i,\delta b^a_i$

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^g_i,\delta b^a_i$ (W系)

增量 $_{B}\delta\phi_{b_kb_{k+1}},_{B}\delta p_{b_kb_{k+1}},_{W}\delta v_{b_kb_{k+1}},\tilde{\delta b^g},\tilde{\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}}$