IMU流形预积分

预备知识

1.IMU测量模型（B系）$_{B}\tilde{\omega},_{B}\tilde{a}$B​ω~,B​a~

2.世界系下运动模型（W系）$R_{WB}^W,_{W}v,_{W}p$RWBW​,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}$ΔRij​,Δvij​,Δpij​

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

5.分离测量值和噪声$\Delta \tilde{R_{ij}},\Delta \tilde{v_{ij}},\Delta \tilde{p_{ij}},\delta\phi_{ij},\delta v_{ij},\delta p_{ij}$ΔRij​~​,Δvij​~​,Δpij​~​,δϕij​,δvij​,δpij​

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]$ηijΔ​=[δϕijT​​δvijT​​δpijT​​]T,ηkd​=[ηkgd​​ηkad​​]

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$ΔRij​^​~​,ΔRij​ˉ​~​,Δvij​^​~​,Δvij​ˉ​~​,Δpij​^​~​,Δpij​ˉ​~​,δbig​,δbia​

9. 残差及Jacobians计算

残差$r_{\Delta p_{ij}},r_{\Delta v_{ij}},r_{\Delta R_{ij}}$rΔpij​​,rΔvij​​,rΔRij​​（B系）

状态变量$R_i,v_i,p_i,R_j,v_j,p_j,\delta b^g_i,\delta b^a_i$Ri​,vi​,pi​,Rj​,vj​,pj​,δbig​,δbia​ (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}$B​δϕbk​bk+1​​,B​δpbk​bk+1​​,W​δvbk​bk+1​​,δbg~,δba~

9.1 残差计算

9.2 状态变量及对应增量

9.3 残差关于状态变量的Jacobians

(1) $r_{\Delta p_{ij}}$rΔpij​​

(2) $r_{\Delta v_{ij}}$rΔvij​​

(3) $r_{\Delta R_{ij}}$rΔRij​​