误差

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)

• 陀螺仪
  ω~wbb(t)=ωwbb(t)+bg(t)+ηg(t)\\tilde{\\omega } ^{b}_{wb}\\left ( t \\right ) = \\omega ^{b}_{wb}\\left ( t \\right ) +b_g\\left ( t\\right ) +\\eta _g\\left ( t \\right ) ω~wbb​(t)=ωwbb​(t)+bg​(t)+ηg​(t)
• 加速计

ab(t)=RbwT(aw−gw)+ba(t)+ηa(t)a^b\\left ( t \\right ) = {\\color{Red} R_{b}^{wT}} \\left ( a^{w}-g^{w} \\right ) +b_a\\left ( t\\right ) +\\eta _a \\left( t \\right ) ab(t)=RbwT​(aw−gw)+ba​(t)+ηa​(t)

• ω~,ab(t)\\widetilde{\\omega}, a^b\\left ( t \\right )ω,ab(t)：测量值
• ω,aw\\omega,a^{w}ω,aw：真实值
• 偏置项和噪声项都位于t时刻的载体坐标系(b)
• RbwR_{b}^{w}Rbw​：从载体坐标系(b)到世界坐标系(w)的旋转
• gwg^{w}gw：世界坐标系下的重力加速度

预积分

估计的状态：[Rbw(t),pw(t),vw(t)R_b^w(t), p^{w}(t), v^{w}(t)Rbw​(t),pw(t),vw(t)]

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

1.运动方程

使用欧拉积分，可以得到运动方程的离散形式为：
Rb(t+Δt)w=Rb(t)wExp⁡(ωwbb(t)⋅Δt)vw(t+Δt)=vw(t)+aw(t)⋅Δtpw(t+Δt)=pw(t)+vw(t)⋅Δt+12aw(t)⋅Δt2\\begin{array}{l} \\mathbf{R}_{b(t+\\Delta t)}^{w}=\\mathbf{R}_{b(t)}^{w} {\\color{Red}\\operatorname{Exp} } \\left({\\color{Red} \\boldsymbol{\\omega}_{w b}^{b}(t)} \\cdot \\Delta t\\right) \\\\ \\mathbf{v}^{w}(t+\\Delta t)=\\mathbf{v}^{w}(t)+{\\color{Red} \\mathbf{a}^{w}(t)} \\cdot \\Delta t \\\\ \\mathbf{p}^{w}(t+\\Delta t)=\\mathbf{p}^{w}(t)+\\mathbf{v}^{w}(t) \\cdot \\Delta t+{\\color{Red} \\frac{1}{2} \\mathbf{a}^{w}(t)} \\cdot \\Delta t^{2} \\end{array} Rb(t+Δt)w​=Rb(t)w​Exp(ωwbb​(t)⋅Δt)vw(t+Δt)=vw(t)+aw(t)⋅Δtpw(t+Δt)=pw(t)+vw(t)⋅Δt+21​aw(t)⋅Δt2​
其中， wwbb(t)w_{wb}^b(t)wwbb​(t)表示t时刻角速度矢量在b系下的坐标，wwbb(t)⋅Δtw_{wb}^b(t)\\cdot \\Delta twwbb​(t)⋅Δt表示旋转矢量在b系下的坐标，Exp⁡(ωwbb(t)⋅Δt){\\color{Red}\\operatorname{Exp} } \\left({\\color{Red} \\boldsymbol{\\omega}_{w b}^{b}(t)} \\cdot \\Delta t\\right)Exp(ωwbb​(t)⋅Δt)表示在b系下从$t+\Delta t$时刻到$t$时刻的旋转变换(Rb(t+Δt)b(t)R_{b \\left (t + \\Delta t \\right)}^{b \\left ( t \\right)}Rb(t+Δt)b(t)​)。

在采样频率不变，也就是$\Delta t$不变，将测量模型代入离散运动方程为：
Rk+1=Rk⋅Exp⁡((ω~k−bkg−ηkgd)⋅Δt)vk+1=vk+Rk⋅(f~k−bka−ηkad)⋅Δt+g⋅Δtpk+1=pk+vk⋅Δt+12g⋅Δt2+12Rk⋅(f~k−bka−ηkad)⋅Δt2\\begin{array}{l} \\mathbf{R}_{k+1}=\\mathbf{R}_{k} \\cdot \\operatorname{Exp}\\left(\\left(\\tilde{\\boldsymbol{\\omega}}_{k}-\\mathbf{b}_{k}^{g}-\\boldsymbol{\\eta}_{k}^{g d}\\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}-\\boldsymbol{\\eta}_{k}^{a d}\\right) \\cdot \\Delta t+{\\color{Red}\\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}-\\mathbf{\\eta}_{k}^{a d}\\right) \\cdot \\Delta t^{2} \\end{array} Rk+1​=Rk​⋅Exp((ω~k​−bkg​−ηkgd​)⋅Δt)vk+1​=vk​+Rk​⋅(f~k​−bka​−ηkad​)⋅Δt+g⋅Δtpk+1​=pk​+vk​⋅Δt+21​g⋅Δt2+21​Rk​⋅(f~k​−bka​−ηkad​)⋅Δt2​
其中，
R(t)≐Rb(t)w;ω(t)≐ωwbb(t);f(t)=fb(t);v(t)≐vw(t);p(t)≐pw(t);g≐gw\\mathbf{R}(t) \\doteq \\mathbf{R}_{b(t)}^{w} ; \\boldsymbol{\\omega}(t) \\doteq \\boldsymbol{\\omega}_{w b}^{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} R(t)≐Rb(t)w​;ω(t)≐ωwbb​(t);f(t)=fb(t);v(t)≐vw(t);p(t)≐pw(t);g≐gw

2.预积分

根据欧拉积分，可以利用$i$时刻到$j-1$时刻的所有IMU测量，其中$j$时刻的Rj,vj,pjR_j, v_j, p_jRj​,vj​,pj​(世界坐标系)可以直接由$i$时刻的Ri,vi,piR_i, v_i,p_iRi​,vi​,pi​(世界坐标系)更新得到。

• RjR_jRj​
  Rj=Ri∏k=ij−1Exp((ω~k−bkg−ηkgd)⋅△t)R_{j} =R_{i} \\prod_{k=i}^{j-1} Exp\\left ( \\left ( \\widetilde{\\omega}_{k} -b_k^g-\\eta _{k}^{gd} \\right ) \\cdot \\bigtriangleup t \\right ) Rj​=Ri​k=i∏j−1​Exp((ωk​−bkg​−ηkgd​)⋅△t)

• vjv_jvj​
  vj=vi+g⋅Δtij+∑k=ij−1Rk⋅(fk~−bka−ηkad)⋅Δtv_{j} =v_{i} +{\\color{Purple} g\\cdot \\Delta t_{ij}} +\\sum_{k=i}^{j-1} {\\color{Red} R_k} \\cdot \\left ( \\widetilde{f_k}-b_k^a-\\eta _{k}^{ad} \\right )\\cdot \\Delta t vj​=vi​+g⋅Δtij​+k=i∑j−1​Rk​⋅(fk​​−bka​−ηkad​)⋅Δt
  其中，(fk~−bka−ηkad)⋅Δt\\left ( \\widetilde{f_k}-b_k^a-\\eta _{k}^{ad} \\right )\\cdot \\Delta t(fk​​−bka​−ηkad​)⋅Δt相对于的是IMU坐标系，需要转换到世界坐标系。

• pjp_jpj​
  pj=pi+∑k=ij−1vk⋅Δt+j−i2g⋅Δt2+12∑k=ij−1Rk⋅(fk~−bka−ηkad)⋅Δt2=pi+∑k=ij−1[vk⋅Δt+12g⋅Δt2+12Rk⋅(fk~−bka−ηkad)⋅Δt2]\\begin{align} p_j & = p_i+\\sum_{k = i}^{j-1} v_k\\cdot \\Delta t+{\\color{Purple} \\frac{j-i}{2} g\\cdot \\Delta t^2} +\\frac{1}{2} \\sum_{k = i}^{j-1}{\\color{Red} R_k} \\cdot \\left ( \\widetilde{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} {\\color{Red} R_k} \\cdot \\left ( \\widetilde{f_k}-b_k^a-\\eta _k^{ad} \\right )\\cdot \\Delta t^2 \\right ] \\end{align} pj​​=pi​+k=i∑j−1​vk​⋅Δt+2j−i​g⋅Δt2+21​k=i∑j−1​Rk​⋅(fk​​−bka​−ηkad​)⋅Δt2=pi​+k=i∑j−1​[vk​⋅Δt+21​g⋅Δt2+21​Rk​⋅(fk​​−bka​−ηkad​)⋅Δt2]​​
  其中，Δtij=∑k=ij−1Δt=(j−i)Δt\\Delta t_{ij}=\\sum_{k=i}^{j-1} \\Delta t=\\left ( j-i \\right ) \\Delta tΔtij​=∑k=ij−1​Δt=(j−i)Δt

为了避免每次更新初始的Ri,vi,piR_i, v_i, p_iRi​,vi​,pi​都需要重新计算Rj,vj,pjR_j, v_j, p_jRj​,vj​,pj​，引出预积分项：
ΔRij≜RiTRj=∏k=ij−1Exp⁡((ω~k−bkg−ηkgd)⋅Δt)Δvij≜RiT(vj−vi−g⋅Δtij)=∑k=ij−1ΔRik⋅(f~k−bka−ηkad)⋅ΔtΔpij≜RiT(pj−pi−vi⋅Δtij−12g⋅Δtij2)=∑k=ij−1[Δvik⋅Δt+12ΔRik⋅(f~k−bka−ηkad)⋅Δt2]\\begin{aligned} \\Delta \\mathbf{R}_{i j} & \\triangleq \\mathbf{R}_{i}^{T} \\mathbf{R}_{j} \\\\ &=\\prod_{k=i}^{j-1} \\operatorname{Exp}\\left(\\left(\\tilde{\\mathbf{\\omega}}_{k}-\\mathbf{b}_{k}^{g}-\\eta_{k}^{g d}\\right) \\cdot \\Delta t\\right) \\\\ \\Delta \\mathbf{v}_{i j} & \\triangleq \\mathbf{R}_{i}^{T}\\left(\\mathbf{v}_{j}-\\mathbf{v}_{i}-\\mathbf{g} \\cdot \\Delta t_{i j}\\right) \\\\ &=\\sum_{k=i}^{j-1} \\Delta \\mathbf{R}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{k}^{a}-\\eta_{k}^{a d}\\right) \\cdot \\Delta t \\\\ \\Delta \\mathbf{p}_{i j} & \\triangleq \\mathbf{R}_{i}^{T}\\left(\\mathbf{p}_{j}-\\mathbf{p}_{i}-\\mathbf{v}_{i} \\cdot \\Delta t_{i j}-\\frac{1}{2} \\mathbf{g} \\cdot \\Delta t_{i j}^{2}\\right) \\\\ &=\\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}_{k}^{a}-\\mathbf{\\eta}_{k}^{a d}\\right) \\cdot \\Delta t^{2}\\right] \\end{aligned} ΔRij​Δvij​Δpij​​≜RiT​Rj​=k=i∏j−1​Exp((ω~k​−bkg​−ηkgd​)⋅Δt)≜RiT​(vj​−vi​−g⋅Δtij​)=k=i∑j−1​ΔRik​⋅(f~k​−bka​−ηkad​)⋅Δt≜RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)=k=i∑j−1​[Δvik​⋅Δt+21​ΔRik​⋅(f~k​−bka​−ηkad​)⋅Δt2]​

预积分测量值和测量噪声

假设预积分计算区间内的bias相等，即big=bi+1g=...=bjgb_i^g = b_{i+1}^g =...=b_j^{g}big​=bi+1g​=...=bjg​以及bia=bi+1a=...=bjab_i^a = b_{i+1}^a =...=b_j^abia​=bi+1a​=...=bja​

1. ΔRij\\Delta R_{ij}ΔRij​项
   根据性质“当δϕ→\\delta \\overrightarrow{\\phi }δϕ​ ”是小量时，有
   Exp⁡(ϕ⃗+δϕ⃗)≈Exp⁡(ϕ⃗)⋅Exp⁡(Jr(ϕ⃗)⋅δϕ⃗)\\operatorname{Exp}(\\vec{\\phi}+\\delta \\vec{\\phi}) \\approx \\operatorname{Exp}(\\vec{\\phi}) \\cdot \\operatorname{Exp}\\left(\\mathbf{J}_{r}(\\vec{\\phi}) \\cdot \\delta \\vec{\\phi}\\right) Exp(ϕ​+δϕ​)≈Exp(ϕ​)⋅Exp(Jr​(ϕ​)⋅δϕ​)
   指数映射的Adjoint性质：
   R⋅Exp⁡(ϕ⃗)⋅RT=exp⁡(Rϕ⃗∧RT)=Exp⁡(Rϕ⃗)⇔Exp⁡(ϕ⃗)⋅R=R⋅Exp⁡(RTϕ⃗)\\begin{array}{l} \\mathbf{R} \\cdot \\operatorname{Exp}(\\vec{\\phi}) \\cdot \\mathbf{R}^{T} = \\exp \\left(\\mathbf{R} \\vec{\\phi}^{\\wedge} \\mathbf{R}^{T}\\right) = \\operatorname{Exp}(\\mathbf{R} \\vec{\\phi}) \\\\ \\Leftrightarrow \\operatorname{Exp}(\\vec{\\phi}) \\cdot \\mathbf{R} = \\mathbf{R} \\cdot \\operatorname{Exp}\\left(\\mathbf{R}^{T} \\vec{\\phi}\\right) \\end{array} R⋅Exp(ϕ​)⋅RT=exp(Rϕ​∧RT)=Exp(Rϕ​)⇔Exp(ϕ​)⋅R=R⋅Exp(RTϕ​)​
   ΔRij=∏k=ij−1Exp⁡((ω~k−big)Δt⏟ϕ→−ηkgdΔt⏟δϕ→)≈(1)∏k=ij−1{Exp⁡((ω~k−big)Δt)⋅Exp⁡(−Jr((ω~k−big)Δt)⋅ηkgdΔt)}=(2)ΔR~ij⋅∏k=ij−1Exp⁡(−ΔR~k+1jT⋅Jrk⋅ηkgdΔ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)​
   其中，令：
   Jrk=Jr((ω~k−big)Δt)ΔR~ij=∏k=ij−1Exp⁡((ω~k−big)Δt)Exp⁡(−δϕ⃗ij)=∏k=ij−1Exp⁡(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt)\\begin{array}{l} \\mathbf{J}_{r}^{k}=\\mathbf{J}_{r}\\left(\\left(\\tilde{\\boldsymbol{\\omega}}_{k}-\\mathbf{b}_{i}^{g}\\right) \\Delta t\\right)\\\\ \\Delta \\tilde{\\mathbf{R}}_{i j}=\\prod_{k=i}^{j-1} \\operatorname{Exp}\\left(\\left(\\tilde{\\boldsymbol{\\omega}}_{k}-\\mathbf{b}_{i}^{g}\\right) \\Delta t\\right) \\\\ \\operatorname{Exp}\\left(-\\delta \\vec{\\phi}_{i j}\\right)=\\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{array} Jrk​=Jr​((ω~k​−big​)Δt)ΔR~ij​=∏k=ij−1​Exp((ω~k​−big​)Δt)Exp(−δϕ​ij​)=∏k=ij−1​Exp(−ΔR~k+1jT​⋅Jrk​⋅ηkgd​Δt)​
   其中，ΔR~jj=I\\Delta \\widetilde{R}_{jj}=IΔRjj​=I，则可以得到：
   ΔRij≜ΔR~ij⋅Exp⁡(−δϕ⃗ij)\\Delta \\mathbf{R}_{i j} \\triangleq \\Delta \\tilde{\\mathbf{R}}_{i j} \\cdot \\operatorname{Exp}\\left(-\\delta \\vec{\\phi}_{i j}\\right) ΔRij​≜ΔR~ij​⋅Exp(−δϕ​ij​)
   ΔR~ij\\Delta\\widetilde{R}_{ij}ΔRij​即旋转量预积分测量值，它由陀螺仪测量值和对陀螺仪bias的估计得到，δϕ→ij\\delta\\overrightarrow{\\phi }_{ij}δϕ​ij​为测量的噪声。

2. Δvij\\Delta v_{ij}Δvij​项：
   将ΔRij≜ΔR~ij⋅Exp⁡(−δϕ⃗ij)\\Delta \\mathbf{R}_{i j} \\triangleq \\Delta \\tilde{\\mathbf{R}}_{i j} \\cdot \\operatorname{Exp}\\left(-\\delta \\vec{\\phi}_{i j}\\right)ΔRij​≜ΔR~ij​⋅Exp(−δϕ​ij​)代入到Δvij\\Delta v_{ij}Δvij​中
   Δvij=∑k=ij−1ΔRik⋅(f~k−bia−ηkad)⋅Δt≈∑k=ij−1ΔR~ik⋅Exp⁡(−δϕ⃗ik)⏟I−δϕ⃗ik∧⋅(f~k−bia−ηkad)⋅Δt≈(1)∑k=ij−1ΔR~ik⋅(I−δϕ⃗ik∧)⋅(f~k−bia−ηkad)⋅Δt≈(2)∑k=ij−1[ΔR~ik⋅(I−δϕ⃗ik∧)⋅(f~k−bia)⋅Δt⏟a∧⋅b=−b∧⋅a−ΔR~ikηkadΔt+ΔR~ikδϕ→ij∧ηkad‾]=(3)∑k=ij−1[ΔR~ik⋅(f~k−bia)⋅Δt+ΔR~ik⋅(f~k−bia)∧⋅δϕ⃗ik⋅Δt−ΔR~ikηkadΔt]=∑k=ij−1[ΔR~ik⋅(f~k−bia)⋅Δt]⏟Δv~ij+∑k=ij−1[ΔR~ik⋅(f~k−bia)∧⋅δϕ⃗ik⋅Δt−ΔR~ikηkadΔt]⏟−δvij\\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+ΔRik​δϕ​ij∧​ηkad​​​=(3)k=i∑j−1​[ΔR~ik​⋅(f~k​−bia​)⋅Δt+ΔR~ik​⋅(f~k​−bia​)∧⋅δϕ​ik​⋅Δt−ΔR~ik​ηkad​Δt]=Δvij​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]​​​
   可以得到：
   Δvij≜Δv~ij−δvij\\Delta v_{ij} \\triangleq \\Delta \\widetilde{v}_{ij}-\\delta v_{ij} Δvij​≜Δvij​−δvij​
   Δv~ij\\Delta \\widetilde{v}_{ij}Δvij​为速度增量预积分测量值，它由IMU的测量值和对bias的估计得到，δvij\\delta v_{ij}δvij​为测量噪声。

3. Δpij\\Delta p_{ij}Δpij​项
   将ΔRij≜ΔR~ij⋅Exp⁡(−δϕ⃗ij)\\Delta \\mathbf{R}_{i j} \\triangleq \\Delta \\tilde{\\mathbf{R}}_{i j} \\cdot \\operatorname{Exp}\\left(-\\delta \\vec{\\phi}_{i j}\\right)ΔRij​≜ΔR~ij​⋅Exp(−δϕ​ij​)和Δvij≜Δv~ij−δvij\\Delta v_{ij} \\triangleq \\Delta \\widetilde{v}_{ij}-\\delta v_{ij}Δvij​≜Δvij​−δvij​代入到Δpij\\Delta p_{ij}Δpij​中
   Δpij=∑k=ij−1[Δvik⋅Δt+12ΔRik⋅(f~k−bia−ηkad)⋅Δt2]≈∑k=ij−1[(Δv~ik−δvik)⋅Δt+12ΔR~ik⋅Exp⁡(−δϕ⃗ik)⋅(f~k−bia−ηkad)⋅Δt2]≈(1)∑k=ij−1[(Δv~ik−δvik)⋅Δt+12ΔR~ik⋅(I−δϕ⃗ik∧)⏟Exp⁡(ϕ⃗)=I+ϕ⃗∧⋅(f~k−bia−ηkad)⋅Δt2]≈(2)∑k=ij−1[(ΔV~ik−δvik)⋅Δt+12ΔR~ik⋅(I−δϕ⃗ik∧)⋅(f~k−bia)⋅Δt2−12ΔR~ikηkadΔt2+12ΔR~ikδϕ⃗ik∧ηkadΔt2‾]=(3)∑k=ij−1[ΔV~ikΔt+12ΔR~ik⋅(f~k−bia)Δt2⏟Δp~ij+12ΔR~ik⋅(f~k−bia)∧δϕ⃗ikΔt2⏟a∧⋅b=−b∧⋅a−12ΔR~ikηkadΔt2−δvikΔt⏟δpij]\\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∧⋅a21​ΔR~ik​⋅(f~k​−bia​)∧δϕ​ik​Δt2​​−21​ΔR~ik​ηkad​Δt2−δvik​Δt​​​​