流形上的预积分

论文：IMU Preintegration on Manifold for Effificient Visual-Inertial Maximum-a-Posteriori Estimation

引言

Recent results in monocular visual-inertial navigation (VIN) have shown that optimization-based approaches outperform filtering methods in terms of accuracy due to their capability to relinearize past states. However, the improvement comes at the cost of increased computational complexity. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes. The preintegration allows us to accurately summarize hundreds of inertial measurements into a single relative motion constraint. Our first contribution is a preintegration theory that properly addresses the manifold structure of the rotation group and carefully deals with uncertainty propagation. The measurements are integrated in a local frame, which eliminates the need to repeat the integration when the linearization point changes while leaving the opportunity for belated bias corrections. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated in a visual-inertial pipeline under the unifying framework of factor graphs. This enables the use of a structureless model for visual measurements, further accelerating the computation. The third contribution is an extensive evaluation of our monocular VIN pipeline: experimental results confirm that our system is very fast and demonstrates superior accuracy with respect to competitive state-of-the-art filtering and optimization algorithms, including off-the-shelf systems such as Google Tango [1].

接上文…

B.噪声传播

• 旋转噪声

从旋转噪声开始:
ΔR~ij=Ri⊤RjExp⁡(δϕij)⇒Exp⁡(−δϕij)≐∏k=ij−1Exp⁡(−ΔR~k+1j⊤JrkηkgdΔt)(32)\Delta \tilde{\mathrm{R}}_{i j} =\mathrm{R}_{i}^{\top} \mathrm{R}_{j} \operatorname{Exp}\left(\delta \phi_{i j}\right) \\ \Rightarrow \operatorname{Exp}\left(-\delta \phi_{i j}\right) \doteq \prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t\right) \tag{32} ΔR~ij​=Ri⊤​Rj​Exp(δϕij​)⇒Exp(−δϕij​)≐k=i∏j−1​Exp(−ΔR~k+1j⊤​Jrk​ηkgd​Δt)(32)
两边取Log并改变符号：
δϕij=−Log(∏k=ij−1Exp⁡(−ΔR~k+1j⊤JrkηkgdΔt))(33)\delta \phi_{i j}=-\text{Log} \left(\prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t\right)\right) \tag{33} δϕij​=−Log(k=i∏j−1​Exp(−ΔR~k+1j⊤​Jrk​ηkgd​Δt))(33)
重复应用一阶近似得到（ 回想一下 ηgd\boldsymbol{\eta}^{gd}ηgd 和 δϕij\delta \boldsymbol{\phi}_{ij}δϕij​ 是小旋转噪声，因此右雅可比矩阵接近恒等 ）：
δϕij≃∑k=ij−1ΔR~k+1j⊤JrkηkgdΔt(34)\delta \phi_{i j} \simeq \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t \tag{34} δϕij​≃k=i∑j−1​ΔR~k+1j⊤​Jrk​ηkgd​Δt(34)
在一阶范围内，因为噪声 δϕij\delta \boldsymbol{\phi}_{ij}δϕij​ 是零均值噪声项 ηkgd\boldsymbol{\eta}^{gd}_{k}ηkgd​ 的线性组合，所以 δϕij\delta \boldsymbol{\phi}_{ij}δϕij​ 为零均值高斯分布。这是可取的，因为它将旋转测量模型(31)精确地转化为(12)的形式。

R~=RExp⁡(ϵ),ϵ∼N(0,Σ)(12)\tilde{\mathrm{R}}=\mathrm{R} \operatorname{Exp}(\epsilon), \quad \epsilon \sim \mathcal{N}(0, \Sigma) \tag{12} R~=RExp(ϵ),ϵ∼N(0,Σ)(12)

其中 R\bold{R}R 是 一个给定的无噪声旋转(均值), ϵ\epsilonϵ 为均值为零的小的正态分布扰动。

现在处理噪声项 δvij\delta \bold{v}_{ij}δvij​ 和 δpij\delta \bold{p}_{ij}δpij​ 很容易: 它们是加速度噪声 ηkad\boldsymbol{\eta}^{ad}_{k}ηkad​ 和预积分旋转噪声 δϕij\delta \boldsymbol{\phi}_{ij}δϕij​ 的线性组合，因此它们也是零均值和高斯分布。

因此可以将噪声完全表征为:
[δϕij⊤,δvij⊤,δpij⊤]⊤∼N(09×1,Σij)(35)\left[\delta \boldsymbol{\phi}_{i j}^{\top}, \delta \mathbf{v}_{i j}^{\top}, \delta \mathbf{p}_{i j}^{\top}\right]^{\top} \sim \mathcal{N}\left(\mathbf{0}_{9 \times 1}, \boldsymbol{\Sigma}_{i j}\right) \tag{35} [δϕij⊤​,δvij⊤​,δpij⊤​]⊤∼N(09×1​,Σij​)(35)

这就是预积分的噪声向量。

在补充材料中提供了协方差 Σij\bold{\Sigma}_{ij}Σij​ 的表达式，其中还表明预积分测量 ΔR~ij\Delta \tilde{\bold{R}}_{ij}ΔR~ij​ ， Δv~ij\Delta \tilde{\bold{v}}_{ij}Δv~ij​ ， Δp~ij\Delta \tilde{\bold{p}}_{ij}Δp~​ij​ 和协方差 Σij\bold{\Sigma}_{ij}Σij​ 都可以增量计算。

补充材料：预积分的协方差

预积分的噪声向量为：
ηijΔ≐[δϕij⊤,δvij⊤,δpij⊤]⊤∼N(09×1,Σij)(A.1)\boldsymbol{\eta}_{i j}^{\Delta} \doteq\left[\delta \boldsymbol{\phi}_{i j}^{\top}, \delta \mathbf{v}_{i j}^{\top}, \delta \mathbf{p}_{i j}^{\top}\right]^{\top} \sim \mathcal{N}\left(\mathbf{0}_{9 \times 1}, \boldsymbol{\Sigma}_{i j}\right) \tag{A.1} ηijΔ​≐[δϕij⊤​,δvij⊤​,δpij⊤​]⊤∼N(09×1​,Σij​)(A.1)
其中：
δϕij≃∑k=ij−1ΔR~k+1j⊤JrkηkgdΔtδvij≃∑k=ij−1[−ΔR~ik(a~k−bia)∧δϕikΔt+ΔR~ikηkadΔt]δpij≃∑k=ij−1[−32ΔR~ik(a~k−bia)∧δϕikΔt2+32ΔR~ikηkadΔt2](A.2)\begin{aligned} \delta \phi_{i j} & \simeq \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t \\ \delta \mathbf{v}_{i j} & \simeq \sum_{k=i}^{j-1}\left[-\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t+\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right] \\ \delta \mathbf{p}_{i j} & \simeq \sum_{k=i}^{j-1}\left[-\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t^{2}+\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2}\right] \end{aligned} \tag{A.2} δϕij​δvij​δpij​​≃k=i∑j−1​ΔR~k+1j⊤​Jrk​ηkgd​Δt≃k=i∑j−1​[−ΔR~ik​(a~k​−bia​)∧δϕik​Δt+ΔR~ik​ηkad​Δt]≃k=i∑j−1​[−23​ΔR~ik​(a~k​−bia​)∧δϕik​Δt2+23​ΔR~ik​ηkad​Δt2]​(A.2)
这些关系式在一阶下都是有效的。推导 ηijΔ\boldsymbol{\eta}_{ij}^{\Delta}ηijΔ​ 的协方差的方式是将 δϕij\delta \boldsymbol{\phi}_{ij}δϕij​ 代入到 δvij\delta \bold{v}_{ij}δvij​ 和 δpij\delta \bold{p}_{ij}δpij​ 中。这样做的结果将是一个线性表达式，它将 ηijΔ\boldsymbol{\eta}_{ij}^{\Delta}ηijΔ​ 与原始测量噪声 ηijgd\boldsymbol{\eta}_{ij}^{gd}ηijgd​ , ηijad\boldsymbol{\eta}_{ij}^{ad}ηijad​ 联系起来，在这个表达式上可以进行(繁琐的)噪声传播。

为了避免这个过程，我们倾向于将(A.2)写成迭代形式，然后在结果(更简单的)表达式上进行噪声传播。 为了将 ηijΔ\boldsymbol{\eta}_{ij}^{\Delta}ηijΔ​ 写成迭代形式，首先注意到:
δϕij≃∑k=ij−1ΔR~k+1j⊤JrkηkgdΔt=∑k=ij−2ΔR~k+1⊤JrkηkgdΔt+ΔR~jj⊤⏞=I3Jrj−1ηj−1gdΔt=∑k=ij−2(ΔR~k+1j−1ΔR~j−1j)⊤JrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~j−1j⊤∑k=ij−2ΔR~k+1j−1⊤JrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~j−1j⊤δϕij−1+Jrj−1ηj−1gdΔt(A.3)\begin{aligned} \delta \boldsymbol{\phi}_{i j} & \simeq \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t\\ &=\sum_{k=i}^{j-2} \Delta \tilde{\mathrm{R}}_{k+1}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t+\overbrace{\Delta \tilde{\mathrm{R}}_{j j}^{\top}}^{=\mathrm{I}_{3}} \mathrm{J}_r^{j-1} \boldsymbol{\eta}_{j-1}^{g d} \Delta t \\ &=\sum_{k=i}^{j-2}\left(\Delta \tilde{\mathrm{R}}_{k+1 j-1} \Delta \tilde{\mathrm{R}}_{j-1 j}\right)^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t+\mathrm{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g d} \Delta t\\ &=\Delta \tilde{\mathrm{R}}_{j-1 j}^{\top} \sum_{k=i}^{j-2} \Delta \tilde{\mathrm{R}}_{k+1 j-1}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t+\mathrm{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g d} \Delta t \\ &=\Delta \tilde{\mathrm{R}}_{j-1 j}^{\top} \delta \boldsymbol{\phi}_{i j-1}+\mathrm{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g d} \Delta t \end{aligned} \tag{A.3} δϕij​​≃k=i∑j−1​ΔR~k+1j⊤​Jrk​ηkgd​Δt=k=i∑j−2​ΔR~k+1⊤​Jrk​ηkgd​Δt+ΔR~jj⊤​​=I3​​Jrj−1​ηj−1gd​Δt=k=i∑j−2​(ΔR~k+1j−1​ΔR~j−1j​)⊤Jrk​ηkgd​Δt+Jrj−1​ηj−1gd​Δt=ΔR~j−1j⊤​k=i∑j−2​ΔR~k+1j−1⊤​Jrk​ηkgd​Δt+Jrj−1​ηj−1gd​Δt=ΔR~j−1j⊤​δϕij−1​+Jrj−1​ηj−1gd​Δt​(A.3)
其中把最后一项 ( k=j−1k = j−1k=j−1 ) 从和式中提出来，方便地重新排列了这些项。

对 δvij\delta \bold{v}_{ij}δvij​ 重复类似的过程：
δvij=∑k=ij−1[−ΔR~ik(a~k−bia)∧δϕikΔt+ΔR~ikηkadΔt]=∑k=ij−2[−ΔR~ik(a~k−bia)∧δϕikΔt+ΔR~ikηkadΔt]−ΔR~ij−1(a~j−1−bia)∧δϕij−1Δt+ΔR~ij−1ηj−1adΔt=δvij−1−ΔR~ij−1(a~j−1−bia)∧δϕij−1Δt+ΔR~ij−1ηj−1adΔt(A.4)\begin{aligned} \delta \mathbf{v}_{i j} &=\sum_{k=i}^{j-1}\left[-\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t+\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right] \\ &=\sum_{k=i}^{j-2}\left[-\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t+\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right]\\&-\Delta \tilde{\mathrm{R}}_{i j-1}\left(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i j-1} \Delta t+\Delta \tilde{\mathrm{R}}_{i j-1} \boldsymbol{\eta}_{j-1}^{a d} \Delta t \\ &=\delta \mathbf{v}_{i j-1}-\Delta \tilde{\mathrm{R}}_{i j-1}\left(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i j-1} \Delta t+\Delta \tilde{\mathrm{R}}_{i j-1} \boldsymbol{\eta}_{j-1}^{a d} \Delta t \end{aligned} \tag{A.4} δvij​​=k=i∑j−1​[−ΔR~ik​(a~k​−bia​)∧δϕik​Δt+ΔR~ik​ηkad​Δt]=k=i∑j−2​[−ΔR~ik​(a~k​−bia​)∧δϕik​Δt+ΔR~ik​ηkad​Δt]−ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt+ΔR~ij−1​ηj−1ad​Δt=δvij−1​−ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt+ΔR~ij−1​ηj−1ad​Δt​(A.4)

对 δpij\delta \bold{p}_{ij}δpij​ 做同样的处理，注意 δpij\delta \bold{p}_{ij}δpij​ 可以写成 δvij\delta \bold{v}_{ij}δvij​ 的函数(这可以很容易地从(A.2)中 δvij\delta \bold{v}_{ij}δvij​ 的表达式中看出):
δpij=∑k=ij−1[δvikΔt−12ΔR~ik(a~k−bia)∧δϕikΔt2+12ΔR~ikηkadΔt2]=∑k=ij−2[δvikΔt−12ΔR~ik(a~k−bia)∧δϕikΔt2+12ΔR~ikηkadΔt2]+δvij−1Δt−12ΔR~ij−1(a~j−1−bia)∧δϕij−1Δt2+12ΔR~ij−1ηj−1adΔt2=δpij−1+δvij−1Δt−12ΔR~ij−1(a~j−1−bia)∧δϕij−1Δt2+12ΔR~ij−1ηj−1adΔt2(A.5)\begin{aligned} \delta \mathbf{p}_{i j} &=\sum_{k=i}^{j-1}\left[\delta \mathbf{v}_{i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2}\right] \\ &=\sum_{k=i}^{j-2}\left[\delta \mathbf{v}_{i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2}\right] \\ &+\delta \mathbf{v}_{i j-1} \Delta t-\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i j-1}\left(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i j-1} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i j-1} \boldsymbol{\eta}_{j-1}^{a d} \Delta t^{2} \\ &=\delta \mathbf{p}_{i j-1}+\delta \mathbf{v}_{i j-1} \Delta t-\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i j-1}\left(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i j-1} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i j-1} \boldsymbol{\eta}_{j-1}^{a d} \Delta t^{2} \end{aligned} \tag{A.5} δpij​​=k=i∑j−1​[δvik​Δt−21​ΔR~ik​(a~k​−bia​)∧δϕik​Δt2+21​ΔR~ik​ηkad​Δt2]=k=i∑j−2​[δvik​Δt−21​ΔR~ik​(a~k​−bia​)∧δϕik​Δt2+21​ΔR~ik​ηkad​Δt2]+δvij−1​Δt−21​ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt2+21​ΔR~ij−1​ηj−1ad​Δt2=δpij−1​+δvij−1​Δt−21​ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt2+21​ΔR~ij−1​ηj−1ad​Δt2​(A.5)

由(A.3)， (A.4)和(A.5)可知(A.2)可以写成迭代形式:
δϕik+1=ΔR~kk+1⊤δϕik+JrkηkgdΔtδvik+1=δvik−ΔR~ik(a~k−bia)∧δϕikΔt+ΔR~ikηkadΔtδpik+1=δpik+δvikΔt−12ΔR~ik(a~k−bia)∧δϕikΔt2+12ΔR~ikηkadΔt2(A.6)\begin{aligned} &\delta \boldsymbol{\phi}_{i k+1}=\Delta \tilde{\mathrm{R}}_{k k+1}^{\top} \delta \boldsymbol{\phi}_{i k}+\mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t \\ &\delta \mathbf{v}_{i k+1}=\delta \mathbf{v}_{i k}-\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t+\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t \\ &\delta \mathbf{p}_{i k+1}=\delta \mathbf{p}_{i k}+\delta \mathbf{v}_{i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2} \end{aligned} \tag{A.6} ​δϕik+1​=ΔR~kk+1⊤​δϕik​+Jrk​ηkgd​Δtδvik+1​=δvik​−ΔR~ik​(a~k​−bia​)∧δϕik​Δt+ΔR~ik​ηkad​Δtδpik+1​=δpik​+δvik​Δt−21​ΔR~ik​(a~k​−bia​)∧δϕik​Δt2+21​ΔR~ik​ηkad​Δt2​(A.6)
对 k=i,⋯,jk=i,\cdots,jk=i,⋯,j ，初始条件为 δϕii=δvii=δpii=03\delta \boldsymbol{\phi}_{ii} = \delta \bold{v}_{ii} = \delta \bold{p}_{ii}=\bold{0}_3δϕii​=δvii​=δpii​=03​ 。

预积分噪声向量的递推公式。对于上式，可以写成：
[δϕik+1δvik+1δpik+1]=[ΔR~kk+1⊤03×303×3−ΔR~ik(a~k−bia)∧ΔtI3×303×3−12ΔR~ik(a~k−bia)∧Δt2I3×3ΔtI3×3][δϕikδvikδpik]+[JrkΔt03×303×3ΔR~ikΔt03×312ΔR~ikΔt2][ηkgdηkad](A.7)\left[\begin{array}{c}\delta \boldsymbol{\phi}_{i k+1} \\ \delta \mathbf{v}_{i k+1} \\ \delta \mathbf{p}_{i k+1}\end{array}\right]=\left[\begin{array}{ccc}\Delta \tilde{\mathrm{R}}_{k k+1}^{\top} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ -\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t & \mathbf{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ -\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t^{2} & \mathbf{I}_{3 \times 3} \Delta t & \mathbf{I}_{3 \times 3}\end{array}\right]\left[\begin{array}{c}\delta \boldsymbol{\phi}_{i k} \\ \delta \mathbf{v}_{i k} \\ \delta \mathbf{p}_{i k}\end{array}\right]+\left[\begin{array}{cc}\mathrm{J}_{r}^{k} \Delta t & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \Delta \tilde{\mathrm{R}}_{i k} \Delta t \\ \mathbf{0}_{3 \times 3} & \frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k} \Delta t^{2}\end{array}\right]\left[\begin{array}{c}\boldsymbol{\eta}_{k}^{g d} \\ \boldsymbol{\eta}_{k}^{a d}\end{array}\right] \tag{A.7} ⎣⎡​δϕik+1​δvik+1​δpik+1​​⎦⎤​=⎣⎡​ΔR~kk+1⊤​−ΔR~ik​(a~k​−bia​)∧Δt−21​ΔR~ik​(a~k​−bia​)∧Δt2​03×3​I3×3​I3×3​Δt​03×3​03×3​I3×3​​⎦⎤​⎣⎡​δϕik​δvik​δpik​​⎦⎤​+⎣⎡​Jrk​Δt03×3​03×3​​03×3​ΔR~ik​Δt21​ΔR~ik​Δt2​⎦⎤​[ηkgd​ηkad​​](A.7)
写成：
ηik+1Δ=AηikΔ+Bηkd(A.8)\boldsymbol{\eta}_{i k+1}^{\Delta}=\mathbf{A} \boldsymbol{\eta}_{i k}^{\Delta}+\mathbf{B} \boldsymbol{\eta}_{k}^{d} \tag{A.8} ηik+1Δ​=AηikΔ​+Bηkd​(A.8)
其中 ηkd≡[ηkgd,ηkad]\boldsymbol{\eta}_{k}^{d} \equiv [\boldsymbol{\eta}_{k}^{gd} , \boldsymbol{\eta}_{k}^{ad}]ηkd​≡[ηkgd​,ηkad​] 。

从线性模型(A.8)和给定原始IMU测量噪声 ηkd\boldsymbol{\eta}_{k}^{d}ηkd​ 的协方差 Ση∈R6×6\bold{\Sigma}_{\boldsymbol{\eta}} \in \mathbb{R}^{6 \times 6}Ση​∈R6×6 ，现在可以迭代计算协方差:
Σik+1=AΣik+1A⊤+BΣηB⊤(A.9)\boldsymbol{\Sigma}_{i k+1}=\mathbf{A} \boldsymbol{\Sigma}_{i k+1} \mathbf{A}^{\top}+\mathbf{B} \boldsymbol{\Sigma}_{\boldsymbol{\eta}} \mathbf{B}^{\top} \tag{A.9} Σik+1​=AΣik+1​A⊤+BΣη​B⊤(A.9)
初始条件为：Σii=09×9\bold{\Sigma}_{ii} = \bold{0}_{9\times 9}Σii​=09×9​ 。

预积分量的地推递推公式。注意，协方差可以迭代计算的事实是很方便的，因为这意味着可以在积分一个新的测量之后更新协方差。 对于预积分测量本身也可以进行相同的迭代计算:
ΔR~ik+1=ΔR~ikExp⁡((ω~k−big)Δt)Δv~ik+1=Δv~ik+ΔR~ik(a~k−bia)ΔtΔp~ik+1=Δp~ik+Δv~ikΔt+12ΔR~ik(a~k−bia)Δt2(A.10)\Delta \tilde{\mathrm{R}}_{i k+1}=\Delta \tilde{\mathrm{R}}_{i k} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ \Delta \tilde{\mathbf{v}}_{i k+1}=\Delta \tilde{\mathbf{v}}_{i k}+\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t \\ \Delta \tilde{\mathbf{p}}_{i k+1}=\Delta \tilde{\mathbf{p}}_{i k}+\Delta \tilde{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \tag{A.10} ΔR~ik+1​=ΔR~ik​Exp((ω~k​−big​)Δt)Δv~ik+1​=Δv~ik​+ΔR~ik​(a~k​−bia​)ΔtΔp~​ik+1​=Δp~​ik​+Δv~ik​Δt+21​ΔR~ik​(a~k​−bia​)Δt2(A.10)

C.Bias更新

在上一节中假设用于计算预积分测量值的偏差 bi\bold{b}_ibi​ 已经给出。 然而另一种可能的是，bias估计在优化过程中发生了变化。一种解决方案是，当bias改变时，重新计算增量测量值；然而这在计算上是昂贵的。 反之，给定bias更新 b←bˉ+δb\bold{b} \leftarrow \bar{\bold{b}} + \delta \bold{b}b←bˉ+δb ，可以使用一阶展开来更新增量测量值:

ΔR~ij(big)≃ΔR~ij(b‾ig)Exp⁡(∂ΔR‾ij∂bgδbg)Δv~ij(big,bia)≃Δv~ij(b‾ig,b‾ia)+∂Δv‾ij∂bgδbig+∂Δv‾ij∂baδbiaΔp~ij(big,bia)≃Δp~ij(b‾ig,b‾ia)+∂Δp‾ij∂bgδbig+∂Δp‾ij∂baδbia(36)\begin{aligned} \Delta \tilde{\mathbf{R}}_{i j}\left(\mathbf{b}_{i}^{g}\right) & \simeq \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}\right) \\ \Delta \tilde{\mathbf{v}}_{i j}\left(\mathbf{b}_{i}^{g}, \mathbf{b}_{i}^{a}\right) & \simeq \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}_{i}^{a} \\ \Delta \tilde{\mathbf{p}}_{i j}\left(\mathbf{b}_{i}^{g}, \mathbf{b}_{i}^{a}\right) & \simeq \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}_{i}^{a} \end{aligned} \tag{36} ΔR~ij​(big​)Δv~ij​(big​,bia​)Δp~​ij​(big​,bia​)​≃ΔR~ij​(big​)Exp(∂bg∂ΔRij​​δbg)≃Δv~ij​(big​,bia​)+∂bg∂Δvij​​δbig​+∂ba∂Δvij​​δbia​≃Δp~​ij​(big​,bia​)+∂bg∂Δp​ij​​δbig​+∂ba∂Δp​ij​​δbia​​(36)
这类似于[26]中的bias修正，但直接作用于SO(3)。 雅可比 {∂ΔRˉij∂bg,∂Δvˉij∂bg,⋯}\{ \frac{\partial \Delta \bar{\bold{R}}_{ij} }{\partial \bold{b}^{g}} , \frac{\partial \Delta \bar{\bold{v}}_{ij} }{\partial \bold{b}^{g}},\cdots \}{∂bg∂ΔRˉij​​,∂bg∂Δvˉij​​,⋯} （在 bˉi\bar{\bold{b}}_{i}bˉi​ 处计算）描述测量值如何因bias估计的变化而变化。 雅可比矩阵的推导与在V-A节中使用的推导非常相似，它将测量值表示为一个大值加上一个小扰动; 因此我们省略了完整的推导，它可以在补充材料[29]中找到。 注意，雅可比矩阵保持不变，可以在预积分过程中预计算。

补充材料：预积分相对于Bias的导数

在本节中提供[4]的V-C节中提出的一阶bias矫正的完整推导。 先回顾一下预积分测量的表达式 :
ΔR~ij=∏k=ij−1Exp⁡((ω~k−big)Δt)Δv~ij=∑k=ij−1ΔR~ik(a~k−bia)ΔtΔp~ij=∑k=ij−132ΔR~ik(a~k−bia)Δt2(A.11)\begin{aligned} \Delta \tilde{\mathrm{R}}_{i j} &=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ \Delta \tilde{\mathbf{v}}_{i j} &=\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t \\ \Delta \tilde{\mathbf{p}}_{i j} &=\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \end{aligned} \tag{A.11} ΔR~ij​Δv~ij​Δp~​ij​​=k=i∏j−1​Exp((ω~k​−big​)Δt)=k=i∑j−1​ΔR~ik​(a~k​−bia​)Δt=k=i∑j−1​23​ΔR~ik​(a~k​−bia​)Δt2​(A.11)
假设已经计算了在给定bias估计 bˉi≡[bˉigbˉig]\bar{\bold{b}}_{i} \equiv [ \bar{\bold{b}}_{i}^{g} ~~~ \bar{\bold{b}}_{i}^{g}]bˉi​≡[bˉig​   bˉig​] 处的预积分变量， 将相应的预积分测量表示为 ΔR~ij(b‾i),Δv~ij(b‾i),Δp~ij(b‾i)\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right), \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}\right), \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)ΔR~ij​(bi​),Δv~ij​(bi​),Δp~​ij​(bi​) 。这里推导当bias估计更新时，如何去更新 ΔR~ij(b‾i),Δv~ij(b‾i),Δp~ij(b‾i)\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right), \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}\right), \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)ΔR~ij​(bi​),Δv~ij​(bi​),Δp~​ij​(bi​) 。

考虑这种情况，我们得到了bias的新的估计 b^i←b‾i+δbi\hat{\mathbf{b}}_{i} \leftarrow \overline{\mathbf{b}}_{i}+\delta \mathbf{b}_{i}b^i​←bi​+δbi​ ，其中 δbi\delta \mathbf{b}_{i}δbi​ 是一个关于之前估计 b‾i\overline{\mathbf{b}}_{i}bi​ 的、小的矫正。在(A.11)使用中新的bias估计，可以得到更新的预积分：
ΔR~ij(b^i)=∏k=ij−1Exp⁡((ω~k−b^ig)Δt)=∏k=ij−1Exp⁡((ω~k−b‾ig−δbig)Δt)Δv~ij(b^i)=∑k=ij−1ΔR~ik(b^i)(a~k−b^ia)Δt=∑k=ij−1ΔR~ik(b^i)(a~k−b‾ia−δbia)ΔtΔp~ij(b^i)=∑k=ij−132ΔR~ik(a~k−b^ia)Δt2=∑k=ij−132ΔR~ik(b^i)(a~k−b‾ia−δbia)Δt2(A.12)\begin{array}{l} \Delta \tilde{\mathrm{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) =\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\hat{\mathbf{b}}_{i}^{g}\right) \Delta t\right)=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}-\delta \mathbf{b}_{i}^{g}\right) \Delta t\right) \\ \Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) =\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i k}\left(\hat{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right) \Delta t=\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i k}\left(\hat{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t \\ \Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) =\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right) \Delta t^{2}=\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\hat{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t^{2} \end{array} \tag{A.12} ΔR~ij​(b^i​)=∏k=ij−1​Exp((ω~k​−b^ig​)Δt)=∏k=ij−1​Exp((ω~k​−big​−δbig​)Δt)Δv~ij​(b^i​)=∑k=ij−1​ΔR~ik​(b^i​)(a~k​−b^ia​)Δt=∑k=ij−1​ΔR~ik​(b^i​)(a~k​−bia​−δbia​)ΔtΔp~​ij​(b^i​)=∑k=ij−1​23​ΔR~ik​(a~k​−b^ia​)Δt2=∑k=ij−1​23​ΔR~ik​(b^i​)(a~k​−bia​−δbia​)Δt2​(A.12)
一种简单的解决方案是按照(A.12)中规定的新bias估计重新计算预积分值。但是在本节中将展示如何在不重复积分的情况下更新预积分量。

从预积分旋转测量 ΔR~ij(b‾i)\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)ΔR~ij​(bi​) 开始推导。假设bias矫正量很小，因此对乘积中的每一项使用一阶近似(7):
ΔR~ij(b^i)≃∏k=ij−1[Exp⁡((ω~k−b‾ig)Δt)Exp⁡(−JrkδbigdΔt)](A.13)\Delta \tilde{\mathrm{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) \simeq \prod_{k=i}^{j-1}\left[\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}\right) \Delta t\right) \operatorname{Exp}\left(-\mathrm{J}_{r}^{k} \delta \mathbf{b}_{i}^{g d} \Delta t\right)\right] \tag{A.13} ΔR~ij​(b^i​)≃k=i∏j−1​[Exp((ω~k​−big​)Δt)Exp(−Jrk​δbigd​Δt)](A.13)
其中定义 Jrk≐Jr(ω~k−b‾ig)\mathrm{J}_{r}^{k} \doteq \mathrm{J}_{r}\left(\tilde{\boldsymbol{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}\right)Jrk​≐Jr​(ω~k​−big​) 其中 Jr\mathrm{J}_{r}Jr​ 是式(8)中给出的SO(3)的右雅可比矩阵。重新排列了乘积中的项，使用式(11) 将包括 δb\delta \mathbf{b}δb 在内的项“移动”到最后:
ΔR~ij(b^i)=ΔR~ij(b‾i)∏k=ij−1[Exp⁡(−ΔR~k+1j(b‾i)⊤JrkδbigΔt)](A.13)\Delta \tilde{\mathrm{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)=\Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \prod_{k=i}^{j-1}\left[\operatorname{Exp}\left(-\Delta \tilde{\mathrm{R}}_{k+1 j}\left(\overline{\mathbf{b}}_{i}\right)^{\top} \mathbf{J}_{r}^{k} \delta \mathbf{b}_{i}^{g} \Delta t\right)\right] \tag{A.13} ΔR~ij​(b^i​)=ΔR~ij​(bi​)k=i∏j−1​[Exp(−ΔR~k+1j​(bi​)⊤Jrk​δbig​Δt)](A.13)
其中根据定义，有 ΔR~ij(b‾i)=∏k=ij−1[Exp⁡((ω~k−b‾ig)Δt)]\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)=\prod_{k=i}^{j-1}\left[\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}\right) \Delta t\right)\right]ΔR~ij​(bi​)=∏k=ij−1​[Exp((ω~k​−big​)Δt)] 。

重复应用一阶近似式(9) (因为 δbig\delta \mathbf{b}_{i}^{g}δbig​ 很小，因此右雅可比矩阵接近恒等) 得到:
ΔR~ij(b^i)≃ΔR~ij(b‾i)Exp⁡(∑k=ij−1[−ΔR~k+1j(b‾i)⊤JrkΔt]δbig)=ΔR~ij(b‾i)Exp⁡(∂ΔR‾ij∂bgδbig)(A.14)\Delta \tilde{\mathbf{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) \simeq \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \operatorname{Exp}\left(\sum_{k=i}^{j-1}\left[-\Delta \tilde{\mathrm{R}}_{k+1 j}\left(\overline{\mathbf{b}}_{i}\right)^{\top} \mathrm{J}_{r}^{k} \Delta t\right] \delta \mathbf{b}_{i}^{g}\right)\\ =\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right) \tag{A.14} ΔR~ij​(b^i​)≃ΔR~ij​(bi​)Exp(k=i∑j−1​[−ΔR~k+1j​(bi​)⊤Jrk​Δt]δbig​)=ΔR~ij​(bi​)Exp(∂bg∂ΔRij​​δbig​)(A.14)
对应于eq.(36)。雅可比 $\frac{\partial \Delta \overline{\mathrm{R}}{i j}}{\partial \mathbf{b}^{g}} $ 可以在预积分过程中预先计算。这可以与1.1节进行类比，因为 $\frac{\partial \Delta \overline{\mathrm{R}}{i j}}{\partial \mathbf{b}^{g}} $ 的结构本质上与式(A.2)中乘噪声的结构相同。使用(A.14)，可以更新之前的预积分测量 ΔR~ij(b‾i)\Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)ΔR~ij​(bi​) ，得到 ΔR~ij(b^i)\Delta \tilde{\mathrm{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)ΔR~ij​(b^i​) 。

现在推导预积分中的速度项 Δv~ij(b^i)\Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)Δv~ij​(b^i​) 。将 ΔR~ij(b^i)\Delta \tilde{\mathrm{R}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)ΔR~ij​(b^i​) 代回式(A.12)：
Δv~ij(b^i)=∑k=ij−1ΔR~ij(b‾i)Exp⁡(∂ΔR‾ij∂bgδbig)(a~k−b‾ia−δbia)Δt(A.15)\Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)=\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t \tag{A.15} Δv~ij​(b^i​)=k=i∑j−1​ΔR~ij​(bi​)Exp(∂bg∂ΔRij​​δbig​)(a~k​−bia​−δbia​)Δt(A.15)
考虑到矫正 δbig\delta \mathbf{b}_{i}^{g}δbig​ 很小，使用一阶近似(4):
Δv~ij(b^i)≃∑k=ij−1ΔR~ij(b‾i)(I+(∂ΔR‾ij∂bgδbig)∧)(a~k−b‾ia−δbia)Δt(A.16)\Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) \simeq \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t \tag{A.16} Δv~ij​(b^i​)≃k=i∑j−1​ΔR~ij​(bi​)(I+(∂bg∂ΔRij​​δbig​)∧)(a~k​−bia​−δbia​)Δt(A.16)
拓展之前的表达式，丢掉高阶项：
Δv~ij(b^i)≃∑k=ij−1ΔR~ij(b‾i)(a~k−b‾ia)Δt+∑k=ij−1ΔR~ij(b‾i)(−δbia)Δt+∑k=ij−1ΔR~ij(b‾i)(∂ΔR‾ij∂bgδbig)∧(a~k−b‾ia)Δt(A.17)\Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) \simeq \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t\\ +\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(-\delta \mathbf{b}_{i}^{a}\right) \Delta t+\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t \tag{A.17} Δv~ij​(b^i​)≃k=i∑j−1​ΔR~ij​(bi​)(a~k​−bia​)Δt+k=i∑j−1​ΔR~ij​(bi​)(−δbia​)Δt+k=i∑j−1​ΔR~ij​(bi​)(∂bg∂ΔRij​​δbig​)∧(a~k​−bia​)Δt(A.17)
根据 Δv~ij(b‾i)=∑k=ij−1ΔR~ij(b‾i)(a~k−b‾ia)Δt\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)=\sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta tΔv~ij​(bi​)=∑k=ij−1​ΔR~ij​(bi​)(a~k​−bia​)Δt ，使用属性(2)：
Δv~ij(b^i)=Δv~ij(b‾i)+∑k=ij−1−ΔR~ij(b‾i)Δtδbia+∑k=ij−1−ΔR~ij(b‾i)(a~k−b‾ia)∧∂ΔR‾ij∂bgΔtδbig=Δv~ij(b‾i)+∂Δv‾ij∂baδbia+∂Δv‾ij∂bgδbig(A.18)\begin{aligned} \Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) &=\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)+\sum_{k=i}^{j-1}-\Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \Delta t \delta \mathbf{b}_{i}^{a}+\sum_{k=i}^{j-1}-\Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \Delta t \delta \mathbf{b}_{i}^{g} \\ &=\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}_{i}^{a}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g} \end{aligned} \tag{A.18} Δv~ij​(b^i​)​=Δv~ij​(bi​)+k=i∑j−1​−ΔR~ij​(bi​)Δtδbia​+k=i∑j−1​−ΔR~ij​(bi​)(a~k​−bia​)∧∂bg∂ΔRij​​Δtδbig​=Δv~ij​(bi​)+∂ba∂Δvij​​δbia​+∂bg∂Δvij​​δbig​​(A.18)
对应于本文式(36)中的第二个表达式。

最后对 Δp~ij(b^i)\Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}\right)Δp~​ij​(b^i​) 进行相同的推导：
Δp~ij(b^i)=∑k=ij−132ΔR~ik(b^i)(a~k−b‾ia−δbia)Δt2=Eq.(A.14) ∑k=ij−132ΔR~ij(b‾i)Exp⁡(∂ΔR‾ij∂bgδbig)(a~k−b‾ia−δbia)Δt2≃Eq. (4) ∑k=ij−132ΔR~ij(b‾i)(I+(∂ΔR‾ij∂bgδbig)∧)(a~k−b‾ia−δbia)Δt2=Δp~ij(b‾i)+∑k=ij−1−32ΔR~ij(b‾i)δbiaΔt2+∑k=ij−132ΔR~ij(b‾i)(∂ΔR‾ij∂bgδbig)∧(a~k−b‾ia)Δt2=Eq. (2) Δp~ij(b‾i)+∑k=ij−1−32ΔR~ij(b‾i)Δt2δbia+∑k=ij−1−32ΔR~ij(b‾i)(a~k−b‾ia)∧∂ΔR‾ij∂bgΔt2δbig=Δp~ij(b‾ig,b‾ia)+∂Δp‾ij∂baδbia+∂Δp‾ij∂bgδbig\begin{align} \Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}\right) &=\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathbf{R}}_{i k}\left(\hat{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t^{2} \\ &\stackrel{\text { Eq.(A.14) }}{=} \sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t^{2} \\ &\stackrel{\text { Eq. (4) }} {\simeq} \sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t^{2} \\ &=\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)+\sum_{k=i}^{j-1}-\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \delta \mathbf{b}_{i}^{a} \Delta t^{2}+\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t^{2} \\ &\stackrel{\text { Eq. (2) }}{=} \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)+\sum_{k=i}^{j-1}-\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \Delta t^{2} \delta \mathbf{b}_{i}^{a}+\sum_{k=i}^{j-1}-\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \Delta t^{2} \delta \mathbf{b}_{i}^{g} \\ &=\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}_{i}^{a}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g} \end{align} Δp~​ij​(b^i​)​=k=i∑j−1​23​ΔR~ik​(b^i​)(a~k​−bia​−δbia​)Δt2= Eq.(A.14) k=i∑j−1​23​ΔR~ij​(bi​)Exp(∂bg∂ΔRij​​δbig​)(a~k​−bia​−δbia​)Δt2≃ Eq. (4) k=i∑j−1​23​ΔR~ij​(bi​)(I+(∂bg∂ΔRij​​δbig​)∧)(a~k​−bia​−δbia​)Δt2=Δp~​ij​(bi​)+k=i∑j−1​−23​ΔR~ij​(bi​)δbia​Δt2+k=i∑j−1​23​ΔR~ij​(bi​)(∂bg∂ΔRij​​δbig​)∧(a~k​−bia​)Δt2= Eq. (2) Δp~​ij​(bi​)+k=i∑j−1​−23​ΔR~ij​(bi​)Δt2δbia​+k=i∑j−1​−23​ΔR~ij​(bi​)(a~k​−bia​)∧∂bg∂ΔRij​​Δt2δbig​=Δp~​ij​(big​,bia​)+∂ba∂Δp​ij​​δbia​+∂bg∂Δp​ij​​δbig​​​
对应于式(36)中的最后一个表达式。

总而言之，用于后验bias更新的雅可比矩阵是(参见。(A.14)—(A.18)—(A.19)):
∂ΔR‾ij∂bg=−∑k=ij−1[ΔR~k+1j(b‾i)⊤JrkΔt]∂Δv‾ij∂ba=−∑k=ij−1ΔR~ij(b‾i)Δt∂Δv‾ij∂bg=−∑k=ij−1ΔR~ij(a~k−b‾ia)∧∂ΔR‾ij∂bgΔt∂Δp‾ij∂ba=−∑k=ij−132ΔR~ij(b‾i)Δt2∂Δp‾ij∂bg=−∑k=ij−132ΔR~ij(b‾i)(a~k−b‾ia)∧∂ΔR‾ij∂bgΔt2(A.20)\begin{aligned} \frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} &=-\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathrm{R}}_{k+1 j}\left(\overline{\mathbf{b}}_{i}\right)^{\top} \mathrm{J}_{r}^{k} \Delta t\right] \\ \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}} &=-\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \Delta t \\ \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}} &=-\sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i j}\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \Delta t \\ \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{a}} &=-\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right) \Delta t^{2} \\ \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}} &=-\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j}\left(\overline{\mathbf{b}}_{i}\right)\left(\tilde{\mathbf{a}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \Delta t^{2} \end{aligned} \tag{A.20} ∂bg∂ΔRij​​∂ba∂Δvij​​∂bg∂Δvij​​∂ba∂Δp​ij​​∂bg∂Δp​ij​​​=−k=i∑j−1​[ΔR~k+1j​(bi​)⊤Jrk​Δt]=−k=i∑j−1​ΔR~ij​(bi​)Δt=−k=i∑j−1​ΔR~ij​(a~k​−bia​)∧∂bg∂ΔRij​​Δt=−k=i∑j−1​23​ΔR~ij​(bi​)Δt2=−k=i∑j−1​23​ΔR~ij​(bi​)(a~k​−bia​)∧∂bg∂ΔRij​​Δt2​(A.20)
重复第1.1节的相同推导，可以表明(A.20)可以随着新的测量值的到来而增量计算。