ORB_SLAM3_IMU预积分理论推导(预积分项)

误差

1.确定性误差

• Bias：偏置
• Scale：实际数值和传感器输出值之间的比值。
• Misalignment：非正交误差。
• 标定的方法：六面法标定加速度。

2.随机误差

• Allan方差
• 随机游走

IMU器件测量模型

1.角标符号说明

• b：body坐标系
• a：加速计(acc)
• g：陀螺仪(gyro)
• w：世界坐标系
• d：离散(discrete)

2.假设

• 假设：地球是静止的，忽略自转，运行平面为水平面，重力指向固定，模值恒定。
  1. 运行场景小；
  2. 运行时间短；
  3. 精度低(mems)；

3.测量模型(gyro/acc)

• 陀螺仪
  $${\tilde{\omega }}_{wb}^b\left(t\right)={\omega }_{wb}^b\left(t\right)+b_g\left(t\right)+{\eta }_g\left(t\right)$$
• 加速计

$$a^b\left(t\right)=R_b^{wT}\left(a^w-g^w\right)+b_a\left(t\right)+{\eta }_a\left(t\right)$$

• $\omega ,a^b\left(t\right)$：测量值
• $\omega ,a^w$：真实值
• 偏置项和噪声项都位于t时刻的载体坐标系(b)
• $R_b^w$：从载体坐标系(b)到世界坐标系(w)的旋转
• $g^w$：世界坐标系下的重力加速度

预积分

估计的状态：[$R_b^w(t),p^w(t),v^w(t)$]

• $p^w(t)$和$v^w(t)$为世界坐标系下IMU的位置和速度
• [$R_b^w(t),p^w(t)$]将帧从B映射到W

1.运动方程

使用欧拉积分，可以得到运动方程的离散形式为：
$$\begin{array}{l}{\mathbf{R}}_{b(t+\Delta t)}^w={\mathbf{R}}_{b(t)}^w\mathrm{Exp}\left({\mathbf{\omega }}_{wb}^b(t)\cdot \Delta t\right)\\ {\mathbf{v}}^w(t+\Delta t)={\mathbf{v}}^w(t)+{\mathbf{a}}^w(t)\cdot \Delta t\\ {\mathbf{p}}^w(t+\Delta t)={\mathbf{p}}^w(t)+{\mathbf{v}}^w(t)\cdot \Delta t+\frac{1}{2}{\mathbf{a}}^w(t)\cdot \Delta t^2\end{array}$$
其中， $w_{wb}^b(t)$表示t时刻角速度矢量在b系下的坐标，$w_{wb}^b(t)\cdot \Delta t$表示旋转矢量在b系下的坐标，$\mathrm{Exp}\left({\mathbf{\omega }}_{wb}^b(t)\cdot \Delta t\right)$表示在b系下从$t+\Delta t$时刻到$t$时刻的旋转变换($R_{b\left(t+\Delta t\right)}^{b\left(t\right)}$)。

在采样频率不变，也就是$\Delta t$不变，将测量模型代入离散运动方程为：
$$\begin{array}{l}{\mathbf{R}}_{k+1}={\mathbf{R}}_k\cdot \mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g-{\mathbf{\eta }}_k^{gd}\right)\cdot \Delta t\right)\\ {\mathbf{v}}_{k+1}={\mathbf{v}}_k+{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\mathbf{\eta }}_k^{ad}\right)\cdot \Delta t+\mathbf{g}\cdot \Delta t\\ {\mathbf{p}}_{k+1}={\mathbf{p}}_k+{\mathbf{v}}_k\cdot \Delta t+\frac{1}{2}\mathbf{g}\cdot \Delta t^2+\frac{1}{2}{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\end{array}$$
其中，
$$\mathbf{R}(t)\doteq {\mathbf{R}}_{b(t)}^w;\mathbf{\omega }(t)\doteq {\mathbf{\omega }}_{wb}^b(t);\mathbf{f}(t)={\mathbf{f}}^b(t);\mathbf{v}(t)\doteq {\mathbf{v}}^w(t);\mathbf{p}(t)\doteq {\mathbf{p}}^w(t);\mathbf{g}\doteq {\mathbf{g}}^w$$

2.预积分

根据欧拉积分，可以利用$i$时刻到$j-1$时刻的所有IMU测量，其中$j$时刻的$R_j,v_j,p_j$(世界坐标系)可以直接由$i$时刻的$R_i,v_i,p_i$(世界坐标系)更新得到。

• $R_j$
  $$R_j=R_i\prod _{k=i}^{j-1}Exp\left(\left({\omega }_k-b_k^g-{\eta }_k^{gd}\right)\cdot △t\right)$$

• $v_j$
  $$v_j=v_i+g\cdot \Delta t_{ij}+\sum _{k=i}^{j-1}{R}_k\cdot \left(f_k-b_k^a-{\eta }_k^{ad}\right)\cdot \Delta t$$
  其中，$\left(f_k-b_k^a-{\eta }_k^{ad}\right)\cdot \Delta t$相对于的是IMU坐标系，需要转换到世界坐标系。

• $p_j$
  $$\begin{array}{rl}{p}_j& =p_i+\sum _{k=i}^{j-1}{v}_k\cdot \Delta t+\frac{j-i}{2}g\cdot \Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k\cdot \left(f_k-b_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\\ & =p_i+\sum _{k=i}^{j-1}\left[v_k\cdot \Delta t+\frac{1}{2}g\cdot \Delta t^2+\frac{1}{2}{R}_k\cdot \left(f_k-b_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\right]\end{array}$$
  其中，$\Delta t_{ij}={\sum }_{k=i}^{j-1}\Delta t=\left(j-i\right)\Delta t$

为了避免每次更新初始的$R_i,v_i,p_i$都需要重新计算$R_j,v_j,p_j$，引出预积分项：
$$\begin{array}{rl}\Delta {\mathbf{R}}_{ij}& \triangleq {\mathbf{R}}_i^T{\mathbf{R}}_j\\ & =\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\omega }}_k-{\mathbf{b}}_k^g-{\eta }_k^{gd}\right)\cdot \Delta t\right)\\ \Delta {\mathbf{v}}_{ij}& \triangleq {\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\\ & =\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t\\ \Delta {\mathbf{p}}_{ij}& \triangleq {\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)\\ & =\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\cdot \Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\right]\end{array}$$

预积分测量值和测量噪声

假设预积分计算区间内的bias相等，即$b_i^g=b_{i+1}^g=...=b_j^g$以及$b_i^a=b_{i+1}^a=...=b_j^a$

1. $\Delta R_{ij}$项
   根据性质“当$\delta \varphi$ ”是小量时，有
   $$\mathrm{Exp}(\varphi +\delta \varphi )\approx \mathrm{Exp}(\varphi )\cdot \mathrm{Exp}\left({\mathbf{J}}_r(\varphi )\cdot \delta \varphi \right)$$
   指数映射的Adjoint性质：
   $$\begin{array}{l}\mathbf{R}\cdot \mathrm{Exp}(\varphi )\cdot {\mathbf{R}}^T=\exp \left(\mathbf{R}{\varphi }^{\wedge }{\mathbf{R}}^T\right)=\mathrm{Exp}(\mathbf{R}\varphi )\\ \iff \mathrm{Exp}(\varphi )\cdot \mathbf{R}=\mathbf{R}\cdot \mathrm{Exp}\left({\mathbf{R}}^T\varphi \right)\end{array}$$
   Δ R i j = ∏ k = i j − 1 Exp ⁡ ( ( ω ~ k − b i g ) Δ t ⏟ ϕ → − η k g d Δ t ⏟ δ ϕ → ) ≈ ( 1 ) ∏ k = i j − 1 { Exp ⁡ ( ( ω ~ k − b i g ) Δ t ) ⋅ Exp ⁡ ( − J r ( ( ω ~ k − b i g ) Δ t ) ⋅ η k g d Δ t ) } = ( 2 ) Δ R ~ i j ⋅ ∏ k = i j − 1 Exp ⁡ ( − Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ t ) \begin{aligned} \Delta \mathbf{R}_{i j} &=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\underbrace{\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t}_{\overrightarrow{\phi } }\underbrace{-\boldsymbol{\eta}_{k}^{g d} \Delta t}_{\delta \overrightarrow{\phi } } \right) \\ & \stackrel{(1)}{\approx} \prod_{k=i}^{j-1}\left\{\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \boldsymbol{\eta}_{k}^{g d} \Delta t\right)\right\} \\ & \stackrel{(2)}{=} \Delta \tilde{\mathbf{R}}_{i j} \cdot \prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \boldsymbol{\eta}_{k}^{g d} \Delta t\right) \end{aligned} ΔRij​​=k=i∏j−1​Exp ​ϕ ​ (ω~k​−big​)Δt​​δϕ ​ −ηkgd​Δt​​ ​≈(1)k=i∏j−1​{Exp((ω~k​−big​)Δt)⋅Exp(−Jr​((ω~k​−big​)Δt)⋅ηkgd​Δt)}=(2)ΔR~ij​⋅k=i∏j−1​Exp(−ΔR~k+1jT​⋅Jrk​⋅ηkgd​Δt)​
   其中，令：
   $$\begin{array}{l}{\mathbf{J}}_r^k={\mathbf{J}}_r\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g\right)\Delta t\right)\\ \Delta {\tilde{\mathbf{R}}}_{ij}={\prod }_{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g\right)\Delta t\right)\\ \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)={\prod }_{k=i}^{j-1}\mathrm{Exp}\left(-\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r^k\cdot {\mathbf{\eta }}_k^{gd}\Delta t\right)\end{array}$$
   其中，$\Delta R_{jj}=I$，则可以得到：
   $$\Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)$$
   $\Delta R_{ij}$即旋转量预积分测量值，它由陀螺仪测量值和对陀螺仪bias的估计得到，$\delta {\varphi }_{ij}$为测量的噪声。

2. $\Delta v_{ij}$项：
   将$\Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)$代入到$\Delta v_{ij}$中
   Δ v i j = ∑ k = i j − 1 Δ R i k ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ∑ k = i j − 1 Δ R ~ i k ⋅ Exp ⁡ ( − δ ϕ ⃗ i k ) ⏟ I − δ ϕ ⃗ i k ∧ ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ( 1 ) ∑ k = i j − 1 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ( 2 ) ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a ) ⋅ Δ t ⏟ a ∧ ⋅ b = − b ∧ ⋅ a − Δ R ~ i k η k a d Δ t + Δ R ~ i k δ ϕ → i j ∧ η k a d ‾ ] = ( 3 ) ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t + Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a d Δ t ] = ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] ⏟ Δ v ~ i j + ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a d Δ t ] ⏟ − δ v i j \begin{aligned} \Delta \mathbf{v}_{i j} &=\sum_{k=i}^{j-1} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \approx \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot \underbrace{{\color{Red} \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right)}}_{\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \stackrel{(1)}{\approx} \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \stackrel{(2)}{\approx} \sum_{k=i}^{j-1}\left[\underbrace{\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t}_{{\color{Red} a^{\wedge } \cdot b = -b^{\wedge }\cdot a} } -\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t +\underline{\Delta\widetilde{R}_{ik}\delta \overrightarrow{\phi} _{ij}^{\wedge }\eta _{k}^{ad}} \right] \\ & \stackrel{(3)}{=} \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t+\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right] \\ &=\underbrace{\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right]}_{\Delta \widetilde{v}_{ij} } +\underbrace{\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right]}_{-\delta v_{ij}} \end{aligned} Δvij​​=k=i∑j−1​ΔRik​⋅(f~k​−bia​−ηkad​)⋅Δt≈k=i∑j−1​ΔR~ik​⋅I−δϕ ​ik∧​ Exp(−δϕ ​ik​)​​⋅(f~k​−bia​−ηkad​)⋅Δt≈(1)k=i∑j−1​ΔR~ik​⋅(I−δϕ ​ik∧​)⋅(f~k​−bia​−ηkad​)⋅Δt≈(2)k=i∑j−1​ ​a∧⋅b=−b∧⋅a ΔR~ik​⋅(I−δϕ ​ik∧​)⋅(f~k​−bia​)⋅Δt​​−ΔR~ik​ηkad​Δt+ΔR ik​δϕ ​ij∧​ηkad​​ ​=(3)k=i∑j−1​[ΔR~ik​⋅(f~k​−bia​)⋅Δt+ΔR~ik​⋅(f~k​−bia​)∧⋅δϕ ​ik​⋅Δt−ΔR~ik​ηkad​Δt]=Δv ij​ k=i∑j−1​[ΔR~ik​⋅(f~k​−bia​)⋅Δt]​​+−δvij​ k=i∑j−1​[ΔR~ik​⋅(f~k​−bia​)∧⋅δϕ ​ik​⋅Δt−ΔR~ik​ηkad​Δt]​​​
   可以得到：
   $$\Delta v_{ij}\triangleq \Delta v_{ij}-\delta v_{ij}$$
   $\Delta v_{ij}$为速度增量预积分测量值，它由IMU的测量值和对bias的估计得到，$\delta v_{ij}$为测量噪声。

3. $\Delta p_{ij}$项
   将$\Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)$和$\Delta v_{ij}\triangleq \Delta v_{ij}-\delta v_{ij}$代入到$\Delta p_{ij}$中
   Δ p i j = ∑ k = i j − 1 [ Δ v i k ⋅ Δ t + 1 2 Δ R i k ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ∑ k = i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ Exp ⁡ ( − δ ϕ ⃗ i k ) ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ( 1 ) ∑ k = i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⏟ Exp ⁡ ( ϕ ⃗ ) = I + ϕ ⃗ ∧ ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ( 2 ) ∑ k = i j − 1 [ ( Δ V ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a ) ⋅ Δ t 2 − 1 2 Δ R ~ i k η k a d Δ t 2 + 1 2 Δ R ~ i k δ ϕ ⃗ i k ∧ η k a d Δ t 2 ‾ ] = ( 3 ) ∑ k = i j − 1 [ Δ V ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ⏟ Δ p ~ i j + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 ⏟ a ∧ ⋅ b = − b ∧ ⋅ a − 1 2 Δ R ~ i k η k a d Δ t 2 − δ v i k Δ t ⏟ δ p i j ] \begin{array}{l} \Delta \mathbf{p}_{i j}=\sum_{k=i}^{j-1}\left[\Delta \mathbf{v}_{i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \approx \sum_{k=i}^{j-1}\left[{\color{Red} \left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right)} \cdot \Delta t+\frac{1}{2} {\color{Red} \Delta \tilde{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right)} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \stackrel{(1)}{\approx} \sum_{k=i}^{j-1}\left[\left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t+\frac{1}{2} \underbrace{\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right)}_{\operatorname{Exp}\left (\vec{ \phi} \right)=I+\vec{\phi}^{\wedge}} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \stackrel{(2)}{\approx} \sum_{k=i}^{j-1}\left[\left(\Delta \tilde{\mathbf{V}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t^{2}-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a d} \Delta t^{2} + \underline{\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \delta \vec{\phi}_{i k}^{\wedge} \mathbf{\eta}_{k}^{a d} \Delta t^{2}} \right] \\ \stackrel{(3)}{=} \sum_{k=i}^{j-1}\left[\underbrace{\Delta \tilde{\mathbf{V}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}}_{\Delta \widetilde{p} _{ij}} +\underbrace{\underbrace{\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \vec{\phi}_{i k} \Delta t^{2}}_{a^{\wedge }\cdot b=-b^{\wedge }\cdot a} -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a d} \Delta t^{2}-\delta \mathbf{v}_{i k} \Delta t} _{\delta p_{ij}}\right] \\ \end{array} Δpij​=∑k=ij−1​[Δvik​⋅Δt+21​ΔRik​⋅(f~k​−bia​−ηkad​)⋅Δt2]≈∑k=ij−1​[(Δv~ik​−δvik​)⋅Δt+21​ΔR~ik​⋅Exp(−δϕ ​ik​)⋅(f~k​−bia​−ηkad​)⋅Δt2]≈(1)∑k=ij−1​ ​(Δv~ik​−δvik​)⋅Δt+21​Exp(ϕ ​)=I+ϕ ​∧ ΔR~ik​⋅(I−δϕ ​ik∧​)​​⋅(f~k​−bia​−ηkad​)⋅Δt2 ​≈(2)∑k=ij−1​[(ΔV~ik​−δvik​)⋅Δt+21​ΔR~ik​⋅(I−δϕ ​ik∧​)⋅(f~k​−bia​)⋅Δt2−21​ΔR~ik​ηkad​Δt2+21​ΔR~ik​δϕ ​ik∧​ηkad​Δt2​]=(3)∑k=ij−1​ ​Δp ​ij​ ΔV~ik​Δt+21​ΔR~ik​⋅(f~k​−bia​)Δt2​​+δpij​ a∧⋅b=−b∧⋅a 21​ΔR~ik​⋅(f~k​−bia​)∧δϕ ​ik​Δt2​​−21​ΔR~ik​ηkad​Δt2−δvik​Δt​​ ​​

可以得到：
$$\Delta p_{ij}\triangleq \Delta p_{ij}-\delta p_{ij}$$
$\Delta p_{ij}$为速度增量预积分测量值，它由IMU的测量值和对bias的估计得到，$\delta p_{ij}$为测量噪声。
预积分理想值：
$$\begin{array}{l}\Delta {\mathbf{R}}_{ij}\triangleq {\mathbf{R}}_i^T{\mathbf{R}}_j\\ \Delta {\mathbf{v}}_{ij}\triangleq {\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\\ \Delta {\mathbf{p}}_{ij}\triangleq {\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)\end{array}$$
将预积分理想值代入：测量值 = 理想值 + 噪声
$$\begin{array}{l}\Delta {\tilde{\mathbf{R}}}_{ij}\approx \Delta {\mathbf{R}}_{ij}\mathrm{Exp}\left(\delta {\varphi }_{ij}\right)={\mathbf{R}}_i^T{\mathbf{R}}_j\mathrm{Exp}\left(\delta {\varphi }_{ij}\right)\\ \Delta {\tilde{\mathbf{v}}}_{ij}\approx \Delta {\mathbf{v}}_{ij}+\delta {\mathbf{v}}_{ij}={\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)+\delta {\mathbf{v}}_{ij}\\ \Delta {\tilde{\mathbf{p}}}_{ij}\approx \Delta {\mathbf{p}}_{ij}+\delta {\mathbf{p}}_{ij}={\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)+\delta {\mathbf{p}}_{ij}\end{array}$$