【SLAM】IMU预积分的理解、手把手推导（44）

【SLAM】IMU预积分的理解、手把手推导（4/4）

由于篇幅设置，IMU预积分分为4篇完成：

• 【SLAM】IMU预积分的理解、手把手推导（1/4）：概要介绍
• 【SLAM】IMU预积分的理解、手把手推导（2/4）：噪声分离、分布形式、递推形式
• 【SLAM】IMU预积分的理解、手把手推导（3/4）：零偏更新后的速算
• 【SLAM】IMU预积分的理解、手把手推导（4/4）：残差的雅可比、总结

在优化过程中，残差计算和残差对状态量的雅可比矩阵是迭代的核心内容。本文对IMU预积分优化过程中，残差和对应的雅可比矩阵的计算进行推导。同时对整个IMU预积分过程进行总结。

IMU预积分推导

优化与残差

在实际应用中，通常以 R i \mathbf{R}_{i} Ri​、 p i \mathbf{p}_{i} pi​、 v i \mathbf{v}_{i} vi​、 R j \mathbf{R}_{j} Rj​、 p j \mathbf{p}_{j} pj​、 v j \mathbf{v}_{j} vj​ 等为导航求解的目标，同时由于IMU的零偏也是不可忽视的，因此，全部的导航状态是 R i \mathbf{R}_{i} Ri​、 p i \mathbf{p}_{i} pi​、 v i \mathbf{v}_{i} vi​、 R j \mathbf{R}_{j} Rj​、 p j \mathbf{p}_{j} pj​、 v j \mathbf{v}_{j} vj​、 δ b i g \delta \mathbf{b}_{i}^{g} δbig​、 δ b i a \delta \mathbf{b}_{i}^{a} δbia​ 。

注意下，这里的状态量不是零偏 b i g \mathbf{b}_{i}^{g} big​、 b i a \mathbf{b}_{i}^{a} bia​，而是零偏的改变量 δ b i g \delta \mathbf{b}_{i}^{g} δbig​、 δ b i a \delta \mathbf{b}_{i}^{a} δbia​。

残差定义 r Δ R i j \mathbf{r}_{\Delta \mathbf{R}_{i j}} rΔRij​​、 r Δ v i j \mathbf{r}_{\Delta \mathbf{v}_{i j}} rΔvij​​、 r Δ p i j \mathbf{r}_{\Delta \mathbf{p}_{i j}} rΔpij​​如下：其中第一部分是PVQ增量的估计值，需要通过非IMU的方式获得，例如点云或视觉到Map的匹配，这部分被视为PVQ增量真值。第二部分是PVQ增量的测量值，这里采用前文推导得到的，零偏更新后获得的修正后的测量值。

即：

r Δ R i j ≜ log ⁡ [ Δ R ^ i j T Δ R i j ] ≈ log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j } r Δ v i j ≜ Δ v i j − Δ v ^ i j ≈ R i T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] r Δ p i j ≜ Δ p i j − Δ p ^ i j ≈ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] \begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{i j}} & \triangleq \log \left[\Delta \hat{\mathbf{R}}_{ij} ^{T} \Delta \mathbf{R}_{i j}\right] \\ & \approx \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\}\\ \mathbf{r}_{\Delta \mathbf{v}_{i j}} & \triangleq \Delta \mathbf{v}_{i j}-\Delta \hat{\mathbf{v}}_{ij}\\ & \approx \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ \mathbf{r}_{\Delta \mathbf{p}_{i j}} & \triangleq \Delta \mathbf{p}_{i j}-\Delta \hat{\mathbf{p}}_{ij} \\& \approx \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \end{aligned} rΔRij​​rΔvij​​rΔpij​​​≜log[ΔR^ijT​ΔRij​]≈log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​⎭ ⎬ ⎫​≜Δvij​−Δv^ij​≈RiT​(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]≜Δpij​−Δp^​ij​≈RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]​

有了残差，接下来就变成了一个非线性最小二乘问题，通过迭代求解增量的方式，不断更新状态变量，使损失函数下降：

R i ← R i ⋅ Exp ⁡ ( δ ϕ ⃗ i ) p i ← p i + R i ⋅ δ p i v i ← v i + δ v i R j ← R j ⋅ Exp ⁡ ( δ ϕ ⃗ j ) p j ← p j + R j ⋅ δ p j v j ← v j + δ v j δ b i g ← δ b i g + δ b i g ~ δ b i a ← δ b i a + δ b i a ~ \begin{aligned} &\mathbf{R}_{i} \leftarrow \mathbf{R}_{i} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right) \\ &\mathbf{p}_{i} \leftarrow \mathbf{p}_{i}+\mathbf{R}_{i} \cdot \delta \mathbf{p}_{i}\\ &\mathbf{v}_{i} \leftarrow \mathbf{v}_{i}+\delta \mathbf{v}_{i} \\ &\mathbf{R}_{j} \leftarrow \mathbf{R}_{j} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\\ &\mathbf{p}_{j} \leftarrow \mathbf{p}_{j}+\mathbf{R}_{j} \cdot \delta \mathbf{p}_{j}\\ &\mathbf{v}_{j} \leftarrow \mathbf{v}_{j}+\delta \mathbf{v}_{j} \\ &\delta \mathbf{b}_{i}^{g} \leftarrow \delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}}\\ &\delta \mathbf{b}_{i}^{a} \leftarrow \delta \mathbf{b}_{i}^{a}+\widetilde{\delta \mathbf{b}_{i}^{a}} \end{aligned} ​Ri​←Ri​⋅Exp(δϕ ​i​)pi​←pi​+Ri​⋅δpi​vi​←vi​+δvi​Rj​←Rj​⋅Exp(δϕ ​j​)pj​←pj​+Rj​⋅δpj​vj​←vj​+δvj​δbig​←δbig​+δbig​ ​δbia​←δbia​+δbia​ ​​

需要额外注意下的是，这里的 p \mathbf p p的迭代方法是 p ← p + R ⋅ δ p \mathbf p \leftarrow \mathbf p + \mathbf{R}\cdot \delta\mathbf{p} p←p+R⋅δp，而不是 p ← p + δ p \mathbf p \leftarrow \mathbf p + \delta\mathbf{p} p←p+δp。怎么理解？可以把旋转、平移想象成一个大矩阵 T \mathbf{T} T：

T = [ R p 0 1 ] \mathbf{T}=\begin{bmatrix}\mathbf{R} & \mathbf{p} \\ 0 & 1\end{bmatrix} T=[R0​p1​]

给它一个右扰动：

δ T = [ δ R δ p 0 1 ] \delta \mathbf{T}=\begin{bmatrix}\delta \mathbf{R} & \delta \mathbf{p} \\ 0 & 1\end{bmatrix} δT=[δR0​δp1​]

那么：

T ⋅ δ T = [ R p 0 1 ] ⋅ [ δ R δ p 0 1 ] = [ R ⋅ δ R p + R ⋅ δ p 0 1 ] \begin{aligned}\mathbf{T} \cdot \delta \mathbf{T}&= \begin{bmatrix}\mathbf{R} & \mathbf{p} \\ 0 & 1\end{bmatrix} \cdot \begin{bmatrix}\delta \mathbf{R} & \delta \mathbf{p} \\ 0 & 1\end{bmatrix} \\ &= \begin{bmatrix}\mathbf{R}\cdot\delta \mathbf{R} & \mathbf{p} + \mathbf{R}\cdot\delta \mathbf{p} \\ 0 & 1\end{bmatrix}\end{aligned} T⋅δT​=[R0​p1​]⋅[δR0​δp1​]=[R⋅δR0​p+R⋅δp1​]​

在利用各类方法进行非线性最小二乘计算时，需要提供残差关于这些状态变量的 Jacobian。对于姿态来说，本文使用的是采用李群李代数来描述位姿，并使用扰动模型来计算 Jacobian。

r Δ R i j \mathbf{r}_{\Delta \mathbf{R}_{i j}} rΔRij​​的雅可比

由于：

r Δ R i j = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j } \mathbf{r}_{\Delta \mathbf{R}_{i j}} = \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\} rΔRij​​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​⎭ ⎬ ⎫​

其中不包含 v i \mathbf{v}_i vi​、 v j \mathbf{v}_j vj​、 p i \mathbf{p}_i pi​、 p j \mathbf{p}_j pj​、 δ b i a \delta\mathbf{b}_i^a δbia​，因此关于这些状态的雅可比矩阵都是0。

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ R i j ( R i Exp ⁡ ( δ ϕ i ) ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ ( R i Exp ⁡ ( δ ϕ i ) ) T R j } = 1 log ⁡ [ ( Δ R ^ i j ) T ⋅ Exp ⁡ ( − δ ϕ i ) R i T R j ] = 2 log ⁡ [ ( Δ R ^ i j ) T ⋅ R i T R j Exp ⁡ ( − R j T R i δ ϕ i ) ] = 3 log ⁡ { Exp ⁡ [ log ⁡ ( ( Δ R ^ i j ) T R i T R j ) ] ⋅ Exp ⁡ ( − R j T R i δ ϕ i ) } = 4 log ⁡ ( ( Δ R ^ i j ) T R i T R j ) − J r − 1 R j T R i δ ϕ i = r Δ R i j − J r − 1 R j T R i δ ϕ i \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_i\operatorname{Exp}(\delta \phi_i)) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T} \mathbf{R}_{j}\right\} \\ & \stackrel{1}= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \operatorname{Exp}(-\delta \phi_i) \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right] \\ &\stackrel{2}= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j} \operatorname{Exp}(-\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i) \right] \\ &\stackrel{3}= \log \{ \operatorname{Exp}\left[\log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})\right] \cdot \operatorname{Exp}(-\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i) \} \\ &\stackrel{4}= \log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i \\ &= \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i \end{aligned} rΔRij​​(Ri​Exp(δϕi​))​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅(Ri​Exp(δϕi​))TRj​⎭ ⎬ ⎫​=1log[(ΔR^ij​)T⋅Exp(−δϕi​)RiT​Rj​]=2log[(ΔR^ij​)T⋅RiT​Rj​Exp(−RjT​Ri​δϕi​)]=3log{Exp[log((ΔR^ij​)TRiT​Rj​)]⋅Exp(−RjT​Ri​δϕi​)}=4log((ΔR^ij​)TRiT​Rj​)−Jr−1​RjT​Ri​δϕi​=rΔRij​​−Jr−1​RjT​Ri​δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用Adjoint性质，将所有的 R i T R j \mathbf{R}_i^T\mathbf{R}_j RiT​Rj​ 换到左侧。

其中3处利用 A = Exp ⁡ ( log ⁡ ( A ) ) A=\operatorname{Exp}(\log(A)) A=Exp(log(A))的性质。

其中4处使用了BCH近似性质。

其中：

J r − 1 = J r − 1 ( r Δ R i j ) \mathbf{J}_r^{-1}=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) Jr−1​=Jr−1​(rΔRij​​)

那么：

∂ r Δ R i j ( R i Exp ⁡ ( δ ϕ i ) ) ∂ δ ϕ i = − J r − 1 R j T R i \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_i\operatorname{Exp}(\delta \phi_i))}{\partial \delta \phi_i}=-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i} ∂δϕi​∂rΔRij​​(Ri​Exp(δϕi​))​=−Jr−1​RjT​Ri​

对于 R j \mathbf{R}_j Rj​，右扰动模型：

r Δ R i j ( R j Exp ⁡ ( δ ϕ j ) ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j Exp ⁡ ( δ ϕ j ) } = log ⁡ [ ( Δ R ^ i j ) T ⋅ R i T R j Exp ⁡ ( δ ϕ j ) ] = 1 log ⁡ { Exp ⁡ [ log ⁡ ( ( Δ R ^ i j ) T R i T R j ) ] ⋅ Exp ⁡ ( δ ϕ j ) } = 2 log ⁡ ( ( Δ R ^ i j ) T R i T R j ) + J r − 1 δ ϕ j = r Δ R i j + J r − 1 δ ϕ j \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_j\operatorname{Exp}(\delta \phi_j)) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\operatorname{Exp}(\delta \phi_j)\right\} \\ &= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\operatorname{Exp}(\delta \phi_j)\right] \\ &\stackrel{1}= \log \{ \operatorname{Exp}\left[\log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})\right] \cdot \operatorname{Exp}(\delta \phi_j) \} \\ &\stackrel{2}= \log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})+\mathbf{J}_r^{-1}\delta \phi_j \\ &= \mathbf{r}_{\Delta \mathbf{R}_{i j}}+\mathbf{J}_r^{-1}\delta \phi_j \end{aligned} rΔRij​​(Rj​Exp(δϕj​))​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​Exp(δϕj​)⎭ ⎬ ⎫​=log[(ΔR^ij​)T⋅RiT​Rj​Exp(δϕj​)]=1log{Exp[log((ΔR^ij​)TRiT​Rj​)]⋅Exp(δϕj​)}=2log((ΔR^ij​)TRiT​Rj​)+Jr−1​δϕj​=rΔRij​​+Jr−1​δϕj​​

其中1、2处使用对数指数性质以及BCH近似性质。

其中：

J r − 1 = J r − 1 ( r Δ R i j ) \mathbf{J}_r^{-1}=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) Jr−1​=Jr−1​(rΔRij​​)

那么：

∂ r Δ R i j ( R j Exp ⁡ ( δ ϕ j ) ) ∂ δ ϕ j = J r − 1 \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_j\operatorname{Exp}(\delta \phi_j))}{\partial \delta \phi_j}=\mathbf{J}_r^{-1} ∂δϕj​∂rΔRij​​(Rj​Exp(δϕj​))​=Jr−1​

对于 δ b i g \delta\mathbf{b}_i^g δbig​：

r Δ R i j ( δ b i g + δ b i g ~ ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g ( δ b i g + δ b i g ~ ) ) ] T ⋅ R i T R j } = 1 log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) Exp ⁡ ( J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] T ⋅ R i T R j } = log ⁡ { [ Δ R ^ i j Exp ⁡ ( J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] T ⋅ Δ R i T R j } = 2 log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Δ R ^ i j T R i T R j } = 3 log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Exp ⁡ ( log ⁡ ( Δ R ^ i j T R i T R j ) ) } = log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Exp ⁡ ( r Δ R i j ) } = 4 log ⁡ { Exp ⁡ ( r Δ R i j ) ⋅ Exp ⁡ [ − Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ] } ≈ 5 r Δ R i j − J r − 1 ⋅ Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g δ b i g ~ \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\delta\mathbf{b}_i^g+\widetilde{\delta \mathbf{b}_{i}^{g}}) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} (\delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}})\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{1}= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \operatorname{Exp}\left(\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &= \log \left\{\left[\Delta \hat{\mathbf{R}}_{i j} \operatorname{Exp}\left(\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \cdot \Delta \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{2}= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \Delta \hat{\mathbf{R}}_{i j}^T \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{3}= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \operatorname{Exp} \left(\log(\Delta \hat{\mathbf{R}}_{i j}^T \mathbf{R}_{i}^T \mathbf{R}_{j})\right)\right\} \\ &= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \operatorname{Exp} \left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right)\right\} \\ &\stackrel{4} = \log \left\{\operatorname{Exp} \left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \cdot \operatorname{Exp}\left[ - \operatorname{Exp}\left( -\mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right] \right\} \\ &\stackrel{5}\approx \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_r^{-1}\cdot \operatorname{Exp}\left( -\mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}} \end{aligned} rΔRij​​(δbig​+δbig​ ​)​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​(δbig​+δbig​ ​))]T⋅RiT​Rj​⎭ ⎬ ⎫​=1log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)Exp(Jr​∂bg∂ΔR~ij​​δbig​ ​)]T⋅RiT​Rj​⎭ ⎬ ⎫​=log⎩ ⎨ ⎧​[ΔR^ij​Exp(Jr​∂bg∂ΔR~ij​​δbig​ ​)]T⋅ΔRiT​Rj​⎭ ⎬ ⎫​=2log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅ΔR^ijT​RiT​Rj​}=3log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅Exp(log(ΔR^ijT​RiT​Rj​))}=log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅Exp(rΔRij​​)}=4log{Exp(rΔRij​​)⋅Exp[−Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​δbig​ ​]}≈5rΔRij​​−Jr−1​⋅Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​δbig​ ​​

其中1、5处使用了BCH近似性质。

其中2处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中3处利用 A = Exp ⁡ ( log ⁡ ( A ) ) A=\operatorname{Exp}(\log(A)) A=Exp(log(A))的性质。

其中4处使用了利用Adjoint性质。

其中，两个 J r − 1 \mathbf{J}_r^{-1} Jr−1​、 J r \mathbf{J}_r Jr​表示的是不同的含义，需要注意区分：

J r − 1 = J r − 1 ( r Δ R i j ) J r = J r ( ∂ Δ R ~ i j ∂ b g ( δ b i g ) ) \begin{aligned} \mathbf{J}_r^{-1} &=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) \\ \mathbf{J}_r &= \mathbf{J}_r(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} (\delta \mathbf{b}_{i}^{g}))\end{aligned} Jr−1​Jr​​=Jr−1​(rΔRij​​)=Jr​(∂bg∂ΔR~ij​​(δbig​))​

那么：

∂ r Δ R i j ∂ δ b i g = − J r − 1 ⋅ Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta\mathbf{b}_i^g}=-\mathbf{J}_r^{-1}\cdot \operatorname{Exp}\left( - \mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} ∂δbig​∂rΔRij​​​=−Jr−1​⋅Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​

r Δ v i j \mathbf{r}_{\Delta \mathbf{v}_{i j}} rΔvij​​的雅可比

由于：

r Δ v i j = R i T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] \mathbf{r}_{\Delta \mathbf{v}_{i j}} = \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] rΔvij​​=RiT​(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]

其中不包含 p i \mathbf{p}_i pi​、 p j \mathbf{p}_j pj​、 R j \mathbf{R}_j Rj​，因此关于这些状态的雅可比矩阵都是0。

对于 δ b i g \delta \mathbf{b}_i^g δbig​、 δ b i a \delta \mathbf{b}_i^a δbia​，可以直接得到答案：

∂ r Δ v i j ∂ δ b i g = − ∂ Δ v ~ i j ∂ b g ∂ r Δ v i j ∂ δ b i a = − ∂ Δ v ~ i j ∂ b a \begin{aligned} \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{b}_i^g} &= -\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{b}_i^a} &= -\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \end{aligned} ∂δbig​∂rΔvij​​​∂δbia​∂rΔvij​​​​=−∂bg∂Δv~ij​​=−∂ba∂Δv~ij​​​

对于 v i \mathbf{v}_i vi​：

r Δ v i j ( v i + δ v i ) = R i T ( v j − v i − δ v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = r Δ v i j − R i T δ v i \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{v}_i + \delta \mathbf{v}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\delta \mathbf{v}_i-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= \mathbf{r}_{\Delta \mathbf{v}_{i j}}-\mathbf{R}_i^T\delta \mathbf{v}_i\end{aligned} rΔvij​​(vi​+δvi​)​=RiT​(vj​−vi​−δvi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=rΔvij​​−RiT​δvi​​

那么：

∂ r Δ v i j ∂ v i = − R i T \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_i}=-\mathbf{R}_i^T ∂vi​∂rΔvij​​​=−RiT​

对于 v j \mathbf{v}_j vj​：

r Δ v i j ( v j + δ v j ) = R i T ( v j + δ v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = r Δ v i j + R i T δ v j \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{v}_j + \delta \mathbf{v}_j) &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}+\delta \mathbf{v}_j-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= \mathbf{r}_{\Delta \mathbf{v}_{i j}}+\mathbf{R}_i^T\delta \mathbf{v}_j\end{aligned} rΔvij​​(vj​+δvj​)​=RiT​(vj​+δvj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=rΔvij​​+RiT​δvj​​

那么：

∂ r Δ v i j ∂ v j = R i T \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_j}=\mathbf{R}_i^T ∂vj​∂rΔvij​​​=RiT​

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ v i j ( R i Exp ⁡ ( δ ϕ i ) ) = ( R i Exp ⁡ ( δ ϕ i ) ) T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = ( R i Exp ⁡ ( δ ϕ i ) ) T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j = 1 Exp ⁡ ( − δ ϕ i ) R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j ≈ 2 ( I − ( δ ϕ i ) ∧ ) R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j = R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j − ( δ ϕ i ) ∧ R i T ( v j − v i − g ⋅ Δ t i j ) = 3 r Δ v i j + [ R i T ( v j − v i − g ⋅ Δ t i j ) ] ∧ ⋅ δ ϕ i \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{R}_i \operatorname{Exp}(\delta \phi_i)) &= (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &\stackrel{1}= \operatorname{Exp}(-\delta \phi_i)\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &\stackrel{2}\approx (\mathbf{I}-(\delta \phi_i)^{\wedge })\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} - (\delta \phi_i)^{\wedge }\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right) \\ &\stackrel{3}= \mathbf{r}_{\Delta \mathbf{v}_{i j}} + \left[\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge }\cdot \delta \phi_i \end{aligned} rΔvij​​(Ri​Exp(δϕi​))​=(Ri​Exp(δϕi​))T(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=(Ri​Exp(δϕi​))T(vj​−vi​−g⋅Δtij​)−Δv^ij​=1Exp(−δϕi​)RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​≈2(I−(δϕi​)∧)RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​=RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​−(δϕi​)∧RiT​(vj​−vi​−g⋅Δtij​)=3rΔvij​​+[RiT​(vj​−vi​−g⋅Δtij​)]∧⋅δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用了 Exp ⁡ \operatorname{Exp} Exp的近似展开形式。

其中3处使用了性质： a ∧ ⋅ b = − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}=-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b=−b∧⋅a。

那么：

∂ r Δ v i j ∂ R i = [ R i T ( v j − v i − g ⋅ Δ t i j ) ] ∧ \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{R}_i}=\left[\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge } ∂Ri​∂rΔvij​​​=[RiT​(vj​−vi​−g⋅Δtij​)]∧

r Δ p i j \mathbf{r}_{\Delta \mathbf{p}_{i j}} rΔpij​​的雅可比

由于：

r Δ p i j = R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] \mathbf{r}_{\Delta \mathbf{p}_{i j}} = \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] rΔpij​​=RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]

其中不包含 v j \mathbf{v}_j vj​、 R j \mathbf{R}_j Rj​，因此关于这些状态的雅可比矩阵都是0。

对于 δ b i g \delta \mathbf{b}_i^g δbig​、 δ b i a \delta \mathbf{b}_i^a δbia​，可以直接得到答案：

∂ r Δ p i j ∂ δ b i g = − ∂ Δ p ~ i j ∂ b g ∂ r Δ p i j ∂ δ b i a = − ∂ Δ p ~ i j ∂ b a \begin{aligned} \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_i^g} &= -\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_i^a} &= -\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \end{aligned} ∂δbig​∂rΔpij​​​∂δbia​∂rΔpij​​​​=−∂bg∂Δp~​ij​​=−∂ba∂Δp~​ij​​​

对于 p j \mathbf{p}_j pj​：

r Δ p i j ( p j + R j δ p j ) = R i T ( p j + R j δ p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j + R i T R j δ p j \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{p}_j+\mathbf{R}_j\delta \mathbf{p}_j) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} + \mathbf{R}_j\delta \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}+\mathbf{R}_i^T \mathbf{R}_j\delta\mathbf{p}_j\end{aligned} rΔpij​​(pj​+Rj​δpj​)​=RiT​(pj​+Rj​δpj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​+RiT​Rj​δpj​​

那么：

∂ r Δ p i j ∂ p j = R i T R j \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{p}_j}=\mathbf{R}_i^T\mathbf{R}_j ∂pj​∂rΔpij​​​=RiT​Rj​

对于 p i \mathbf{p}_i pi​：

r Δ p i j ( p i + R i δ p i ) = R i T ( p j − p i − R i δ p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j − R i T R i δ p i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{p}_i+\mathbf{R}_i\delta \mathbf{p}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} -\mathbf{p}_{i}-\mathbf{R}_i\delta \mathbf{p}_i-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{R}_i^T \mathbf{R}_i\delta\mathbf{p}_i\end{aligned} rΔpij​​(pi​+Ri​δpi​)​=RiT​(pj​−pi​−Ri​δpi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​−RiT​Ri​δpi​​

那么：

∂ r Δ p i j ∂ p i = − R i T R i = − I \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{p}_i}=-\mathbf{R}_i^T\mathbf{R}_i=-\mathbf{I} ∂pi​∂rΔpij​​​=−RiT​Ri​=−I

对于 v i \mathbf{v}_i vi​：

r Δ p i j ( v i + δ v i ) = R i T ( p j − p i − ( v i + δ v i ) ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j − R i T Δ t i j δ v i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{v}_i+\delta \mathbf{v}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} -\mathbf{p}_{i}-(\mathbf{v}_{i}+\delta \mathbf{v}_i) \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{R}_i^T\Delta t_{ij}\delta \mathbf{v}_i \end{aligned} rΔpij​​(vi​+δvi​)​=RiT​(pj​−pi​−(vi​+δvi​)⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​−RiT​Δtij​δvi​​

那么：

∂ r Δ p i j ∂ v i = − R i T Δ t i j \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{v}_i}=-\mathbf{R}_i^T\Delta t_{ij} ∂vi​∂rΔpij​​​=−RiT​Δtij​

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ p i j ( R i Exp ⁡ ( δ ϕ i ) ) = ( R i Exp ⁡ ( δ ϕ i ) ) T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = ( R i Exp ⁡ ( δ ϕ i ) ) T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j = 1 Exp ⁡ ( − δ ϕ i ) R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j ≈ 2 ( I − δ ϕ i ∧ ) R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j = R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j − δ ϕ i ∧ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) = 3 r Δ p i j + [ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) ] ∧ δ ϕ i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{R}_i \operatorname{Exp}(\delta \phi_i)) &= (\mathbf{R}_i \operatorname{Exp}(\delta \phi_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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= (\mathbf{R}_i \operatorname{Exp}(\delta \phi_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)-\Delta \hat{\mathbf{p}}_{i j} \\ &\stackrel{1}= \operatorname{Exp}(-\delta \phi_i)\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)-\Delta \hat{\mathbf{p}}_{i j} \\ &\stackrel{2}\approx (\mathbf{I}-\delta \phi_i^{\wedge})\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)-\Delta \hat{\mathbf{p}}_{i j} \\ &= \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)-\Delta \hat{\mathbf{p}}_{i j}-\delta \phi_i^{\wedge}\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) \\ &\stackrel{3}= \mathbf{r}_{\Delta \mathbf{p}_{i j}}+\left[\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)\right]^{\wedge}\delta \phi_i\end{aligned} rΔpij​​(Ri​Exp(δϕi​))​=(Ri​Exp(δϕi​))T(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=(Ri​Exp(δϕi​))T(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​=1Exp(−δϕi​)RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​≈2(I−δϕi∧​)RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​=RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​−δϕi∧​RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)=3rΔpij​​+[RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)]∧δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用了 Exp ⁡ \operatorname{Exp} Exp的近似展开形式。

其中3处使用了性质： a ∧ ⋅ b = − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}=-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b=−b∧⋅a。

那么：

∂ r Δ p i j ∂ R i = [ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) ] ∧ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{R}_i}=\left[\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)\right]^{\wedge} ∂Ri​∂rΔpij​​​=[RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)]∧

结束。

总结

先梳理一下预积分的作用：

为了消除起始状态对于积分的影响，所有的预积分都是针对于PVQ增量上进行的。即，只在一个区间内的增量上进行研究。

一个优化问题的核心就是残差的设计：

残 差 i j = P V Q 增量估计 值 i j − P V Q 增量测量 值 i j 残差_{ij}=PVQ增量估计值_{ij}-PVQ增量测量值_{ij} 残差ij​=PVQ增量估计值ij​−PVQ增量测量值ij​

其中，增量估计值由非IMU方式获取，增量测量值由IMU提供。比如在VIO方案中，估计值需要视觉在两个关键帧之间通过视觉SLAM方法计算出来的，测量值需要对两个关键帧之间的IMU结果进行积分得到。

此时面临三个问题：

1. PVQ增量测量值的计算：对IMU结果进行积分很耗时，但这个积分和IMU零偏相关，而零偏又属于优化量。这就导致每次优化循环中，都要重新通过积分计算增量测量值

2. 想要进行优化，肯定需要进行雅可比矩阵的计算：残差对状态量的雅可比矩阵

3. 在优化过程中，需要进行信息矩阵的设定来调节比例：增量测量值噪声的信息矩阵

再梳理一下预积分的步骤：

1. 为了消除起始状态对积分的影响，获取PVQ增量真值的表达式；

2. 利用PVQ增量真值=PVQ增量测量值-PVQ增量噪声，对PVQ增量真值表达式进行化简，获得PVQ增量测量值和PVQ增量噪声的表达式；（测量值用于第4步，噪声值用于第3步）

3. 分析PVQ增量噪声的分布形式、递推形式，确定其协方差矩阵的递推形式；（解决问题3）

4. 探寻零偏更新后PVQ增量测量值的速算，确定零偏相对于PVQ增量测量值的雅可比矩阵；（解决问题1）

5. 推导优化状态量相对于残差的雅可比矩阵；（解决问题2）

简而言之：

IMU预积分，解决了初始状态更新，以及加速度计、陀螺仪零偏变化时所导致的积分重新计算问题，同时对噪声的信息矩阵进行分析，用于调节残差的比例。

其主要思路是计算两个关键帧之间的状态增量，当初始状态变化，就在原来增量基础上加上初始状态的变化量；当零偏变化时，则通过求取预积分量关于零偏的雅各比，实现预积分的一阶线性近似更新。

相关阅读

• IMU预积分的理解和推导
• 简明预积分推导
• 如何理解IMU以及其预积分

【SLAM】IMU预积分的理解、手把手推导（4/4）

由于篇幅设置，IMU预积分分为4篇完成：

• 【SLAM】IMU预积分的理解、手把手推导（1/4）：概要介绍
• 【SLAM】IMU预积分的理解、手把手推导（2/4）：噪声分离、分布形式、递推形式
• 【SLAM】IMU预积分的理解、手把手推导（3/4）：零偏更新后的速算
• 【SLAM】IMU预积分的理解、手把手推导（4/4）：残差的雅可比、总结

在优化过程中，残差计算和残差对状态量的雅可比矩阵是迭代的核心内容。本文对IMU预积分优化过程中，残差和对应的雅可比矩阵的计算进行推导。同时对整个IMU预积分过程进行总结。

IMU预积分推导

优化与残差

在实际应用中，通常以 R i \mathbf{R}_{i} Ri​、 p i \mathbf{p}_{i} pi​、 v i \mathbf{v}_{i} vi​、 R j \mathbf{R}_{j} Rj​、 p j \mathbf{p}_{j} pj​、 v j \mathbf{v}_{j} vj​ 等为导航求解的目标，同时由于IMU的零偏也是不可忽视的，因此，全部的导航状态是 R i \mathbf{R}_{i} Ri​、 p i \mathbf{p}_{i} pi​、 v i \mathbf{v}_{i} vi​、 R j \mathbf{R}_{j} Rj​、 p j \mathbf{p}_{j} pj​、 v j \mathbf{v}_{j} vj​、 δ b i g \delta \mathbf{b}_{i}^{g} δbig​、 δ b i a \delta \mathbf{b}_{i}^{a} δbia​ 。

注意下，这里的状态量不是零偏 b i g \mathbf{b}_{i}^{g} big​、 b i a \mathbf{b}_{i}^{a} bia​，而是零偏的改变量 δ b i g \delta \mathbf{b}_{i}^{g} δbig​、 δ b i a \delta \mathbf{b}_{i}^{a} δbia​。

残差定义 r Δ R i j \mathbf{r}_{\Delta \mathbf{R}_{i j}} rΔRij​​、 r Δ v i j \mathbf{r}_{\Delta \mathbf{v}_{i j}} rΔvij​​、 r Δ p i j \mathbf{r}_{\Delta \mathbf{p}_{i j}} rΔpij​​如下：其中第一部分是PVQ增量的估计值，需要通过非IMU的方式获得，例如点云或视觉到Map的匹配，这部分被视为PVQ增量真值。第二部分是PVQ增量的测量值，这里采用前文推导得到的，零偏更新后获得的修正后的测量值。

即：

r Δ R i j ≜ log ⁡ [ Δ R ^ i j T Δ R i j ] ≈ log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j } r Δ v i j ≜ Δ v i j − Δ v ^ i j ≈ R i T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] r Δ p i j ≜ Δ p i j − Δ p ^ i j ≈ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] \begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{i j}} & \triangleq \log \left[\Delta \hat{\mathbf{R}}_{ij} ^{T} \Delta \mathbf{R}_{i j}\right] \\ & \approx \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\}\\ \mathbf{r}_{\Delta \mathbf{v}_{i j}} & \triangleq \Delta \mathbf{v}_{i j}-\Delta \hat{\mathbf{v}}_{ij}\\ & \approx \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ \mathbf{r}_{\Delta \mathbf{p}_{i j}} & \triangleq \Delta \mathbf{p}_{i j}-\Delta \hat{\mathbf{p}}_{ij} \\& \approx \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \end{aligned} rΔRij​​rΔvij​​rΔpij​​​≜log[ΔR^ijT​ΔRij​]≈log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​⎭ ⎬ ⎫​≜Δvij​−Δv^ij​≈RiT​(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]≜Δpij​−Δp^​ij​≈RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]​

有了残差，接下来就变成了一个非线性最小二乘问题，通过迭代求解增量的方式，不断更新状态变量，使损失函数下降：

R i ← R i ⋅ Exp ⁡ ( δ ϕ ⃗ i ) p i ← p i + R i ⋅ δ p i v i ← v i + δ v i R j ← R j ⋅ Exp ⁡ ( δ ϕ ⃗ j ) p j ← p j + R j ⋅ δ p j v j ← v j + δ v j δ b i g ← δ b i g + δ b i g ~ δ b i a ← δ b i a + δ b i a ~ \begin{aligned} &\mathbf{R}_{i} \leftarrow \mathbf{R}_{i} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right) \\ &\mathbf{p}_{i} \leftarrow \mathbf{p}_{i}+\mathbf{R}_{i} \cdot \delta \mathbf{p}_{i}\\ &\mathbf{v}_{i} \leftarrow \mathbf{v}_{i}+\delta \mathbf{v}_{i} \\ &\mathbf{R}_{j} \leftarrow \mathbf{R}_{j} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\\ &\mathbf{p}_{j} \leftarrow \mathbf{p}_{j}+\mathbf{R}_{j} \cdot \delta \mathbf{p}_{j}\\ &\mathbf{v}_{j} \leftarrow \mathbf{v}_{j}+\delta \mathbf{v}_{j} \\ &\delta \mathbf{b}_{i}^{g} \leftarrow \delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}}\\ &\delta \mathbf{b}_{i}^{a} \leftarrow \delta \mathbf{b}_{i}^{a}+\widetilde{\delta \mathbf{b}_{i}^{a}} \end{aligned} ​Ri​←Ri​⋅Exp(δϕ ​i​)pi​←pi​+Ri​⋅δpi​vi​←vi​+δvi​Rj​←Rj​⋅Exp(δϕ ​j​)pj​←pj​+Rj​⋅δpj​vj​←vj​+δvj​δbig​←δbig​+δbig​ ​δbia​←δbia​+δbia​ ​​

需要额外注意下的是，这里的 p \mathbf p p的迭代方法是 p ← p + R ⋅ δ p \mathbf p \leftarrow \mathbf p + \mathbf{R}\cdot \delta\mathbf{p} p←p+R⋅δp，而不是 p ← p + δ p \mathbf p \leftarrow \mathbf p + \delta\mathbf{p} p←p+δp。怎么理解？可以把旋转、平移想象成一个大矩阵 T \mathbf{T} T：

T = [ R p 0 1 ] \mathbf{T}=\begin{bmatrix}\mathbf{R} & \mathbf{p} \\ 0 & 1\end{bmatrix} T=[R0​p1​]

给它一个右扰动：

δ T = [ δ R δ p 0 1 ] \delta \mathbf{T}=\begin{bmatrix}\delta \mathbf{R} & \delta \mathbf{p} \\ 0 & 1\end{bmatrix} δT=[δR0​δp1​]

那么：

T ⋅ δ T = [ R p 0 1 ] ⋅ [ δ R δ p 0 1 ] = [ R ⋅ δ R p + R ⋅ δ p 0 1 ] \begin{aligned}\mathbf{T} \cdot \delta \mathbf{T}&= \begin{bmatrix}\mathbf{R} & \mathbf{p} \\ 0 & 1\end{bmatrix} \cdot \begin{bmatrix}\delta \mathbf{R} & \delta \mathbf{p} \\ 0 & 1\end{bmatrix} \\ &= \begin{bmatrix}\mathbf{R}\cdot\delta \mathbf{R} & \mathbf{p} + \mathbf{R}\cdot\delta \mathbf{p} \\ 0 & 1\end{bmatrix}\end{aligned} T⋅δT​=[R0​p1​]⋅[δR0​δp1​]=[R⋅δR0​p+R⋅δp1​]​

在利用各类方法进行非线性最小二乘计算时，需要提供残差关于这些状态变量的 Jacobian。对于姿态来说，本文使用的是采用李群李代数来描述位姿，并使用扰动模型来计算 Jacobian。

r Δ R i j \mathbf{r}_{\Delta \mathbf{R}_{i j}} rΔRij​​的雅可比

由于：

r Δ R i j = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j } \mathbf{r}_{\Delta \mathbf{R}_{i j}} = \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\} rΔRij​​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​⎭ ⎬ ⎫​

其中不包含 v i \mathbf{v}_i vi​、 v j \mathbf{v}_j vj​、 p i \mathbf{p}_i pi​、 p j \mathbf{p}_j pj​、 δ b i a \delta\mathbf{b}_i^a δbia​，因此关于这些状态的雅可比矩阵都是0。

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ R i j ( R i Exp ⁡ ( δ ϕ i ) ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ ( R i Exp ⁡ ( δ ϕ i ) ) T R j } = 1 log ⁡ [ ( Δ R ^ i j ) T ⋅ Exp ⁡ ( − δ ϕ i ) R i T R j ] = 2 log ⁡ [ ( Δ R ^ i j ) T ⋅ R i T R j Exp ⁡ ( − R j T R i δ ϕ i ) ] = 3 log ⁡ { Exp ⁡ [ log ⁡ ( ( Δ R ^ i j ) T R i T R j ) ] ⋅ Exp ⁡ ( − R j T R i δ ϕ i ) } = 4 log ⁡ ( ( Δ R ^ i j ) T R i T R j ) − J r − 1 R j T R i δ ϕ i = r Δ R i j − J r − 1 R j T R i δ ϕ i \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_i\operatorname{Exp}(\delta \phi_i)) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T} \mathbf{R}_{j}\right\} \\ & \stackrel{1}= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \operatorname{Exp}(-\delta \phi_i) \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right] \\ &\stackrel{2}= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j} \operatorname{Exp}(-\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i) \right] \\ &\stackrel{3}= \log \{ \operatorname{Exp}\left[\log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})\right] \cdot \operatorname{Exp}(-\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i) \} \\ &\stackrel{4}= \log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i \\ &= \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i}\delta \phi_i \end{aligned} rΔRij​​(Ri​Exp(δϕi​))​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅(Ri​Exp(δϕi​))TRj​⎭ ⎬ ⎫​=1log[(ΔR^ij​)T⋅Exp(−δϕi​)RiT​Rj​]=2log[(ΔR^ij​)T⋅RiT​Rj​Exp(−RjT​Ri​δϕi​)]=3log{Exp[log((ΔR^ij​)TRiT​Rj​)]⋅Exp(−RjT​Ri​δϕi​)}=4log((ΔR^ij​)TRiT​Rj​)−Jr−1​RjT​Ri​δϕi​=rΔRij​​−Jr−1​RjT​Ri​δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用Adjoint性质，将所有的 R i T R j \mathbf{R}_i^T\mathbf{R}_j RiT​Rj​ 换到左侧。

其中3处利用 A = Exp ⁡ ( log ⁡ ( A ) ) A=\operatorname{Exp}(\log(A)) A=Exp(log(A))的性质。

其中4处使用了BCH近似性质。

其中：

J r − 1 = J r − 1 ( r Δ R i j ) \mathbf{J}_r^{-1}=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) Jr−1​=Jr−1​(rΔRij​​)

那么：

∂ r Δ R i j ( R i Exp ⁡ ( δ ϕ i ) ) ∂ δ ϕ i = − J r − 1 R j T R i \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_i\operatorname{Exp}(\delta \phi_i))}{\partial \delta \phi_i}=-\mathbf{J}_r^{-1}\mathbf{R}_{j}^T\mathbf{R}_{i} ∂δϕi​∂rΔRij​​(Ri​Exp(δϕi​))​=−Jr−1​RjT​Ri​

对于 R j \mathbf{R}_j Rj​，右扰动模型：

r Δ R i j ( R j Exp ⁡ ( δ ϕ j ) ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) ] T ⋅ R i T R j Exp ⁡ ( δ ϕ j ) } = log ⁡ [ ( Δ R ^ i j ) T ⋅ R i T R j Exp ⁡ ( δ ϕ j ) ] = 1 log ⁡ { Exp ⁡ [ log ⁡ ( ( Δ R ^ i j ) T R i T R j ) ] ⋅ Exp ⁡ ( δ ϕ j ) } = 2 log ⁡ ( ( Δ R ^ i j ) T R i T R j ) + J r − 1 δ ϕ j = r Δ R i j + J r − 1 δ ϕ j \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_j\operatorname{Exp}(\delta \phi_j)) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\operatorname{Exp}(\delta \phi_j)\right\} \\ &= \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\operatorname{Exp}(\delta \phi_j)\right] \\ &\stackrel{1}= \log \{ \operatorname{Exp}\left[\log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})\right] \cdot \operatorname{Exp}(\delta \phi_j) \} \\ &\stackrel{2}= \log(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j})+\mathbf{J}_r^{-1}\delta \phi_j \\ &= \mathbf{r}_{\Delta \mathbf{R}_{i j}}+\mathbf{J}_r^{-1}\delta \phi_j \end{aligned} rΔRij​​(Rj​Exp(δϕj​))​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)]T⋅RiT​Rj​Exp(δϕj​)⎭ ⎬ ⎫​=log[(ΔR^ij​)T⋅RiT​Rj​Exp(δϕj​)]=1log{Exp[log((ΔR^ij​)TRiT​Rj​)]⋅Exp(δϕj​)}=2log((ΔR^ij​)TRiT​Rj​)+Jr−1​δϕj​=rΔRij​​+Jr−1​δϕj​​

其中1、2处使用对数指数性质以及BCH近似性质。

其中：

J r − 1 = J r − 1 ( r Δ R i j ) \mathbf{J}_r^{-1}=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) Jr−1​=Jr−1​(rΔRij​​)

那么：

∂ r Δ R i j ( R j Exp ⁡ ( δ ϕ j ) ) ∂ δ ϕ j = J r − 1 \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}(\mathbf{R}_j\operatorname{Exp}(\delta \phi_j))}{\partial \delta \phi_j}=\mathbf{J}_r^{-1} ∂δϕj​∂rΔRij​​(Rj​Exp(δϕj​))​=Jr−1​

对于 δ b i g \delta\mathbf{b}_i^g δbig​：

r Δ R i j ( δ b i g + δ b i g ~ ) = log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g ( δ b i g + δ b i g ~ ) ) ] T ⋅ R i T R j } = 1 log ⁡ { [ Δ R ~ i j ⋅ Exp ⁡ ( ∂ Δ R ~ i j ∂ b g δ b i g ) Exp ⁡ ( J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] T ⋅ R i T R j } = log ⁡ { [ Δ R ^ i j Exp ⁡ ( J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] T ⋅ Δ R i T R j } = 2 log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Δ R ^ i j T R i T R j } = 3 log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Exp ⁡ ( log ⁡ ( Δ R ^ i j T R i T R j ) ) } = log ⁡ { [ Exp ⁡ ( − J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ) ] ⋅ Exp ⁡ ( r Δ R i j ) } = 4 log ⁡ { Exp ⁡ ( r Δ R i j ) ⋅ Exp ⁡ [ − Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g δ b i g ~ ] } ≈ 5 r Δ R i j − J r − 1 ⋅ Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g δ b i g ~ \begin{aligned}\mathbf{r}_{\Delta \mathbf{R}_{i j}}(\delta\mathbf{b}_i^g+\widetilde{\delta \mathbf{b}_{i}^{g}}) &= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} (\delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}})\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{1}= \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \operatorname{Exp}\left(\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \cdot \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &= \log \left\{\left[\Delta \hat{\mathbf{R}}_{i j} \operatorname{Exp}\left(\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \cdot \Delta \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{2}= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \Delta \hat{\mathbf{R}}_{i j}^T \mathbf{R}_{i}^T \mathbf{R}_{j}\right\} \\ &\stackrel{3}= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \operatorname{Exp} \left(\log(\Delta \hat{\mathbf{R}}_{i j}^T \mathbf{R}_{i}^T \mathbf{R}_{j})\right)\right\} \\ &= \log \left\{\left[ \operatorname{Exp}\left(-\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right] \cdot \operatorname{Exp} \left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right)\right\} \\ &\stackrel{4} = \log \left\{\operatorname{Exp} \left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \cdot \operatorname{Exp}\left[ - \operatorname{Exp}\left( -\mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}}\right] \right\} \\ &\stackrel{5}\approx \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_r^{-1}\cdot \operatorname{Exp}\left( -\mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}}\widetilde{\delta \mathbf{b}_{i}^{g}} \end{aligned} rΔRij​​(δbig​+δbig​ ​)​=log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​(δbig​+δbig​ ​))]T⋅RiT​Rj​⎭ ⎬ ⎫​=1log⎩ ⎨ ⎧​[ΔR~ij​⋅Exp(∂bg∂ΔR~ij​​δbig​)Exp(Jr​∂bg∂ΔR~ij​​δbig​ ​)]T⋅RiT​Rj​⎭ ⎬ ⎫​=log⎩ ⎨ ⎧​[ΔR^ij​Exp(Jr​∂bg∂ΔR~ij​​δbig​ ​)]T⋅ΔRiT​Rj​⎭ ⎬ ⎫​=2log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅ΔR^ijT​RiT​Rj​}=3log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅Exp(log(ΔR^ijT​RiT​Rj​))}=log{[Exp(−Jr​∂bg∂ΔR~ij​​δbig​ ​)]⋅Exp(rΔRij​​)}=4log{Exp(rΔRij​​)⋅Exp[−Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​δbig​ ​]}≈5rΔRij​​−Jr−1​⋅Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​δbig​ ​​

其中1、5处使用了BCH近似性质。

其中2处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中3处利用 A = Exp ⁡ ( log ⁡ ( A ) ) A=\operatorname{Exp}(\log(A)) A=Exp(log(A))的性质。

其中4处使用了利用Adjoint性质。

其中，两个 J r − 1 \mathbf{J}_r^{-1} Jr−1​、 J r \mathbf{J}_r Jr​表示的是不同的含义，需要注意区分：

J r − 1 = J r − 1 ( r Δ R i j ) J r = J r ( ∂ Δ R ~ i j ∂ b g ( δ b i g ) ) \begin{aligned} \mathbf{J}_r^{-1} &=\mathbf{J}_r^{-1}(\mathbf{r}_{\Delta \mathbf{R}_{i j}}) \\ \mathbf{J}_r &= \mathbf{J}_r(\frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} (\delta \mathbf{b}_{i}^{g}))\end{aligned} Jr−1​Jr​​=Jr−1​(rΔRij​​)=Jr​(∂bg∂ΔR~ij​​(δbig​))​

那么：

∂ r Δ R i j ∂ δ b i g = − J r − 1 ⋅ Exp ⁡ ( − r Δ R i j ) J r ∂ Δ R ~ i j ∂ b g \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta\mathbf{b}_i^g}=-\mathbf{J}_r^{-1}\cdot \operatorname{Exp}\left( - \mathbf{r}_{\Delta \mathbf{R}_{i j}} \right)\mathbf{J}_r \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial {\mathbf{b}}^{g}} ∂δbig​∂rΔRij​​​=−Jr−1​⋅Exp(−rΔRij​​)Jr​∂bg∂ΔR~ij​​

r Δ v i j \mathbf{r}_{\Delta \mathbf{v}_{i j}} rΔvij​​的雅可比

由于：

r Δ v i j = R i T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] \mathbf{r}_{\Delta \mathbf{v}_{i j}} = \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] rΔvij​​=RiT​(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]

其中不包含 p i \mathbf{p}_i pi​、 p j \mathbf{p}_j pj​、 R j \mathbf{R}_j Rj​，因此关于这些状态的雅可比矩阵都是0。

对于 δ b i g \delta \mathbf{b}_i^g δbig​、 δ b i a \delta \mathbf{b}_i^a δbia​，可以直接得到答案：

∂ r Δ v i j ∂ δ b i g = − ∂ Δ v ~ i j ∂ b g ∂ r Δ v i j ∂ δ b i a = − ∂ Δ v ~ i j ∂ b a \begin{aligned} \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{b}_i^g} &= -\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{b}_i^a} &= -\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \end{aligned} ∂δbig​∂rΔvij​​​∂δbia​∂rΔvij​​​​=−∂bg∂Δv~ij​​=−∂ba∂Δv~ij​​​

对于 v i \mathbf{v}_i vi​：

r Δ v i j ( v i + δ v i ) = R i T ( v j − v i − δ v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = r Δ v i j − R i T δ v i \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{v}_i + \delta \mathbf{v}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\delta \mathbf{v}_i-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= \mathbf{r}_{\Delta \mathbf{v}_{i j}}-\mathbf{R}_i^T\delta \mathbf{v}_i\end{aligned} rΔvij​​(vi​+δvi​)​=RiT​(vj​−vi​−δvi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=rΔvij​​−RiT​δvi​​

那么：

∂ r Δ v i j ∂ v i = − R i T \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_i}=-\mathbf{R}_i^T ∂vi​∂rΔvij​​​=−RiT​

对于 v j \mathbf{v}_j vj​：

r Δ v i j ( v j + δ v j ) = R i T ( v j + δ v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = r Δ v i j + R i T δ v j \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{v}_j + \delta \mathbf{v}_j) &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}+\delta \mathbf{v}_j-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= \mathbf{r}_{\Delta \mathbf{v}_{i j}}+\mathbf{R}_i^T\delta \mathbf{v}_j\end{aligned} rΔvij​​(vj​+δvj​)​=RiT​(vj​+δvj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=rΔvij​​+RiT​δvj​​

那么：

∂ r Δ v i j ∂ v j = R i T \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_j}=\mathbf{R}_i^T ∂vj​∂rΔvij​​​=RiT​

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ v i j ( R i Exp ⁡ ( δ ϕ i ) ) = ( R i Exp ⁡ ( δ ϕ i ) ) T ( v j − v i − g ⋅ Δ t i j ) − [ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a ] = ( R i Exp ⁡ ( δ ϕ i ) ) T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j = 1 Exp ⁡ ( − δ ϕ i ) R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j ≈ 2 ( I − ( δ ϕ i ) ∧ ) R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j = R i T ( v j − v i − g ⋅ Δ t i j ) − Δ v ^ i j − ( δ ϕ i ) ∧ R i T ( v j − v i − g ⋅ Δ t i j ) = 3 r Δ v i j + [ R i T ( v j − v i − g ⋅ Δ t i j ) ] ∧ ⋅ δ ϕ i \begin{aligned}\mathbf{r}_{\Delta \mathbf{v}_{i j}}(\mathbf{R}_i \operatorname{Exp}(\delta \phi_i)) &= (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \right] \\ &= (\mathbf{R}_{i}\operatorname{Exp}(\delta \phi_i))^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &\stackrel{1}= \operatorname{Exp}(-\delta \phi_i)\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &\stackrel{2}\approx (\mathbf{I}-(\delta \phi_i)^{\wedge })\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ &= \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} - (\delta \phi_i)^{\wedge }\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right) \\ &\stackrel{3}= \mathbf{r}_{\Delta \mathbf{v}_{i j}} + \left[\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge }\cdot \delta \phi_i \end{aligned} rΔvij​​(Ri​Exp(δϕi​))​=(Ri​Exp(δϕi​))T(vj​−vi​−g⋅Δtij​)−[Δv~ij​+∂bg∂Δv~ij​​δbig​+∂ba∂Δv~ij​​δbia​]=(Ri​Exp(δϕi​))T(vj​−vi​−g⋅Δtij​)−Δv^ij​=1Exp(−δϕi​)RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​≈2(I−(δϕi​)∧)RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​=RiT​(vj​−vi​−g⋅Δtij​)−Δv^ij​−(δϕi​)∧RiT​(vj​−vi​−g⋅Δtij​)=3rΔvij​​+[RiT​(vj​−vi​−g⋅Δtij​)]∧⋅δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用了 Exp ⁡ \operatorname{Exp} Exp的近似展开形式。

其中3处使用了性质： a ∧ ⋅ b = − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}=-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b=−b∧⋅a。

那么：

∂ r Δ v i j ∂ R i = [ R i T ( v j − v i − g ⋅ Δ t i j ) ] ∧ \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{R}_i}=\left[\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge } ∂Ri​∂rΔvij​​​=[RiT​(vj​−vi​−g⋅Δtij​)]∧

r Δ p i j \mathbf{r}_{\Delta \mathbf{p}_{i j}} rΔpij​​的雅可比

由于：

r Δ p i j = R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] \mathbf{r}_{\Delta \mathbf{p}_{i j}} = \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] rΔpij​​=RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]

其中不包含 v j \mathbf{v}_j vj​、 R j \mathbf{R}_j Rj​，因此关于这些状态的雅可比矩阵都是0。

对于 δ b i g \delta \mathbf{b}_i^g δbig​、 δ b i a \delta \mathbf{b}_i^a δbia​，可以直接得到答案：

∂ r Δ p i j ∂ δ b i g = − ∂ Δ p ~ i j ∂ b g ∂ r Δ p i j ∂ δ b i a = − ∂ Δ p ~ i j ∂ b a \begin{aligned} \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_i^g} &= -\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_i^a} &= -\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \end{aligned} ∂δbig​∂rΔpij​​​∂δbia​∂rΔpij​​​​=−∂bg∂Δp~​ij​​=−∂ba∂Δp~​ij​​​

对于 p j \mathbf{p}_j pj​：

r Δ p i j ( p j + R j δ p j ) = R i T ( p j + R j δ p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j + R i T R j δ p j \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{p}_j+\mathbf{R}_j\delta \mathbf{p}_j) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} + \mathbf{R}_j\delta \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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}+\mathbf{R}_i^T \mathbf{R}_j\delta\mathbf{p}_j\end{aligned} rΔpij​​(pj​+Rj​δpj​)​=RiT​(pj​+Rj​δpj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​+RiT​Rj​δpj​​

那么：

∂ r Δ p i j ∂ p j = R i T R j \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{p}_j}=\mathbf{R}_i^T\mathbf{R}_j ∂pj​∂rΔpij​​​=RiT​Rj​

对于 p i \mathbf{p}_i pi​：

r Δ p i j ( p i + R i δ p i ) = R i T ( p j − p i − R i δ p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j − R i T R i δ p i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{p}_i+\mathbf{R}_i\delta \mathbf{p}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} -\mathbf{p}_{i}-\mathbf{R}_i\delta \mathbf{p}_i-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{R}_i^T \mathbf{R}_i\delta\mathbf{p}_i\end{aligned} rΔpij​​(pi​+Ri​δpi​)​=RiT​(pj​−pi​−Ri​δpi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​−RiT​Ri​δpi​​

那么：

∂ r Δ p i j ∂ p i = − R i T R i = − I \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{p}_i}=-\mathbf{R}_i^T\mathbf{R}_i=-\mathbf{I} ∂pi​∂rΔpij​​​=−RiT​Ri​=−I

对于 v i \mathbf{v}_i vi​：

r Δ p i j ( v i + δ v i ) = R i T ( p j − p i − ( v i + δ v i ) ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = r Δ p i j − R i T Δ t i j δ v i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{v}_i+\delta \mathbf{v}_i) &= \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j} -\mathbf{p}_{i}-(\mathbf{v}_{i}+\delta \mathbf{v}_i) \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= \mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{R}_i^T\Delta t_{ij}\delta \mathbf{v}_i \end{aligned} rΔpij​​(vi​+δvi​)​=RiT​(pj​−pi​−(vi​+δvi​)⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=rΔpij​​−RiT​Δtij​δvi​​

那么：

∂ r Δ p i j ∂ v i = − R i T Δ t i j \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{v}_i}=-\mathbf{R}_i^T\Delta t_{ij} ∂vi​∂rΔpij​​​=−RiT​Δtij​

对于 R i \mathbf{R}_i Ri​，右扰动模型：

r Δ p i j ( R i Exp ⁡ ( δ ϕ i ) ) = ( R i Exp ⁡ ( δ ϕ i ) ) T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − [ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a ] = ( R i Exp ⁡ ( δ ϕ i ) ) T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j = 1 Exp ⁡ ( − δ ϕ i ) R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j ≈ 2 ( I − δ ϕ i ∧ ) R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j = R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) − Δ p ^ i j − δ ϕ i ∧ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) = 3 r Δ p i j + [ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) ] ∧ δ ϕ i \begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}(\mathbf{R}_i \operatorname{Exp}(\delta \phi_i)) &= (\mathbf{R}_i \operatorname{Exp}(\delta \phi_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)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial {\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ &= (\mathbf{R}_i \operatorname{Exp}(\delta \phi_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)-\Delta \hat{\mathbf{p}}_{i j} \\ &\stackrel{1}= \operatorname{Exp}(-\delta \phi_i)\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)-\Delta \hat{\mathbf{p}}_{i j} \\ &\stackrel{2}\approx (\mathbf{I}-\delta \phi_i^{\wedge})\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)-\Delta \hat{\mathbf{p}}_{i j} \\ &= \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)-\Delta \hat{\mathbf{p}}_{i j}-\delta \phi_i^{\wedge}\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) \\ &\stackrel{3}= \mathbf{r}_{\Delta \mathbf{p}_{i j}}+\left[\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)\right]^{\wedge}\delta \phi_i\end{aligned} rΔpij​​(Ri​Exp(δϕi​))​=(Ri​Exp(δϕi​))T(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−[Δp~​ij​+∂bg∂Δp~​ij​​δbig​+∂ba∂Δp~​ij​​δbia​]=(Ri​Exp(δϕi​))T(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​=1Exp(−δϕi​)RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​≈2(I−δϕi∧​)RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​=RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)−Δp^​ij​−δϕi∧​RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)=3rΔpij​​+[RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)]∧δϕi​​

其中1处利用 ( A B ) T = B T A T (AB)^T=B^TA^T (AB)T=BTAT、 Exp ⁡ ( ϕ ) T = Exp ⁡ ( − ϕ ) \operatorname{Exp}(\phi)^T=\operatorname{Exp}(-\phi) Exp(ϕ)T=Exp(−ϕ)的性质。

其中2处利用了 Exp ⁡ \operatorname{Exp} Exp的近似展开形式。

其中3处使用了性质： a ∧ ⋅ b = − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}=-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b=−b∧⋅a。

那么：

∂ r Δ p i j ∂ R i = [ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) ] ∧ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{R}_i}=\left[\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)\right]^{\wedge} ∂Ri​∂rΔpij​​​=[RiT​(pj​−pi​−vi​⋅Δtij​−21​g⋅Δtij2​)]∧

结束。

总结

先梳理一下预积分的作用：

为了消除起始状态对于积分的影响，所有的预积分都是针对于PVQ增量上进行的。即，只在一个区间内的增量上进行研究。

一个优化问题的核心就是残差的设计：

残 差 i j = P V Q 增量估计 值 i j − P V Q 增量测量 值 i j 残差_{ij}=PVQ增量估计值_{ij}-PVQ增量测量值_{ij} 残差ij​=PVQ增量估计值ij​−PVQ增量测量值ij​

其中，增量估计值由非IMU方式获取，增量测量值由IMU提供。比如在VIO方案中，估计值需要视觉在两个关键帧之间通过视觉SLAM方法计算出来的，测量值需要对两个关键帧之间的IMU结果进行积分得到。

此时面临三个问题：

1. PVQ增量测量值的计算：对IMU结果进行积分很耗时，但这个积分和IMU零偏相关，而零偏又属于优化量。这就导致每次优化循环中，都要重新通过积分计算增量测量值

2. 想要进行优化，肯定需要进行雅可比矩阵的计算：残差对状态量的雅可比矩阵

3. 在优化过程中，需要进行信息矩阵的设定来调节比例：增量测量值噪声的信息矩阵

再梳理一下预积分的步骤：

1. 为了消除起始状态对积分的影响，获取PVQ增量真值的表达式；

2. 利用PVQ增量真值=PVQ增量测量值-PVQ增量噪声，对PVQ增量真值表达式进行化简，获得PVQ增量测量值和PVQ增量噪声的表达式；（测量值用于第4步，噪声值用于第3步）

3. 分析PVQ增量噪声的分布形式、递推形式，确定其协方差矩阵的递推形式；（解决问题3）

4. 探寻零偏更新后PVQ增量测量值的速算，确定零偏相对于PVQ增量测量值的雅可比矩阵；（解决问题1）

5. 推导优化状态量相对于残差的雅可比矩阵；（解决问题2）

简而言之：

IMU预积分，解决了初始状态更新，以及加速度计、陀螺仪零偏变化时所导致的积分重新计算问题，同时对噪声的信息矩阵进行分析，用于调节残差的比例。

其主要思路是计算两个关键帧之间的状态增量，当初始状态变化，就在原来增量基础上加上初始状态的变化量；当零偏变化时，则通过求取预积分量关于零偏的雅各比，实现预积分的一阶线性近似更新。

相关阅读

• IMU预积分的理解和推导
• 简明预积分推导
• 如何理解IMU以及其预积分