【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预积分推导

优化与残差

在实际应用中，通常以 ${\mathbf{R}}_i$、${\mathbf{p}}_i$、${\mathbf{v}}_i$、${\mathbf{R}}_j$、${\mathbf{p}}_j$、${\mathbf{v}}_j$ 等为导航求解的目标，同时由于IMU的零偏也是不可忽视的，因此，全部的导航状态是 ${\mathbf{R}}_i$、${\mathbf{p}}_i$、${\mathbf{v}}_i$、${\mathbf{R}}_j$、${\mathbf{p}}_j$、${\mathbf{v}}_j$、$\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$ 。

注意下，这里的状态量不是零偏${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$，而是零偏的改变量$\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$。

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

即：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}& \triangleq \log \left[\Delta {\hat{\mathbf{R}}}_{ij}^T\Delta {\mathbf{R}}_{ij}\right]\\ & \approx \log \left\{{\left[\Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\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}}_{ij}}& \triangleq \Delta {\mathbf{v}}_{ij}-\Delta {\hat{\mathbf{v}}}_{ij}\\ & \approx {\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\left[\Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}& \triangleq \Delta {\mathbf{p}}_{ij}-\Delta {\hat{\mathbf{p}}}_{ij}\\ & \approx {\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\end{array}$$

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

$$\begin{array}{rl}& {\mathbf{R}}_i\leftarrow {\mathbf{R}}_i\cdot \mathrm{Exp}\left(\delta {\vec{\varphi }}_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 \mathrm{Exp}\left(\delta {\vec{\varphi }}_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{array}$$

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

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ 0& 1\end{array}\right]$$

给它一个右扰动：

$$\delta \mathbf{T}=\left[\begin{array}{cc}\delta \mathbf{R}& \delta \mathbf{p}\\ 0& 1\end{array}\right]$$

那么：

$$\begin{array}{rl}\mathbf{T}\cdot \delta \mathbf{T}& =\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{cc}\delta \mathbf{R}& \delta \mathbf{p}\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{R}\cdot \delta \mathbf{R}& \mathbf{p}+\mathbf{R}\cdot \delta \mathbf{p}\\ 0& 1\end{array}\right]\end{array}$$

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

${\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}$的雅可比

由于：

$${\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}=\log \left\{{\left[\Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g\right)\right]}^T\cdot {\mathbf{R}}_i^T{\mathbf{R}}_j\right\}$$

其中不包含 ${\mathbf{v}}_i$、${\mathbf{v}}_j$、${\mathbf{p}}_i$、${\mathbf{p}}_j$、$\delta {\mathbf{b}}_i^a$，因此关于这些状态的雅可比矩阵都是0。

对于${\mathbf{R}}_i$，右扰动模型：

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处利用$(AB{)}^T=B^T{A}^T$、$\mathrm{Exp}(\varphi {)}^T=\mathrm{Exp}(-\varphi )$的性质。

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

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

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

其中：

$${\mathbf{J}}_r^{-1}={\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}})$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i))}{\partial \delta {\varphi }_i}=-{\mathbf{J}}_r^{-1}{\mathbf{R}}_j^T{\mathbf{R}}_i$$

对于${\mathbf{R}}_j$，右扰动模型：

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近似性质。

其中：

$${\mathbf{J}}_r^{-1}={\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}})$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_j\mathrm{Exp}(\delta {\varphi }_j))}{\partial \delta {\varphi }_j}={\mathbf{J}}_r^{-1}$$

对于$\delta {\mathbf{b}}_i^g$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}(\delta {\mathbf{b}}_i^g+\widetilde{\delta {\mathbf{b}}_i^g})& =\log \left\{{\left[\Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\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}}}_{ij}\cdot \mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g\right)\mathrm{Exp}\left({\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\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}}}_{ij}\mathrm{Exp}\left({\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\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[\mathrm{Exp}\left(-{\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\widetilde{\delta {\mathbf{b}}_i^g}\right)\right]\cdot \Delta {\hat{\mathbf{R}}}_{ij}^T{\mathbf{R}}_i^T{\mathbf{R}}_j\right\}\\ & \stackrel{3}{=}\log \left\{\left[\mathrm{Exp}\left(-{\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\widetilde{\delta {\mathbf{b}}_i^g}\right)\right]\cdot \mathrm{Exp}\left(\log (\Delta {\hat{\mathbf{R}}}_{ij}^T{\mathbf{R}}_i^T{\mathbf{R}}_j)\right)\right\}\\ & =\log \left\{\left[\mathrm{Exp}\left(-{\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\widetilde{\delta {\mathbf{b}}_i^g}\right)\right]\cdot \mathrm{Exp}\left({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\right)\right\}\\ & \stackrel{4}{=}\log \left\{\mathrm{Exp}\left({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\right)\cdot \mathrm{Exp}\left[-\mathrm{Exp}\left(-{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\right){\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\widetilde{\delta {\mathbf{b}}_i^g}\right]\right\}\\ & \stackrel{5}{\approx }{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}-{\mathbf{J}}_r^{-1}\cdot \mathrm{Exp}\left(-{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\right){\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\widetilde{\delta {\mathbf{b}}_i^g}\end{array}$$

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

其中2处利用$(AB{)}^T=B^T{A}^T$、$\mathrm{Exp}(\varphi {)}^T=\mathrm{Exp}(-\varphi )$的性质。

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

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

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

$$\begin{array}{rl}{\mathbf{J}}_r^{-1}& ={\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}})\\ {\mathbf{J}}_r& ={\mathbf{J}}_r(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}(\delta {\mathbf{b}}_i^g))\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{b}}_i^g}=-{\mathbf{J}}_r^{-1}\cdot \mathrm{Exp}\left(-{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\right){\mathbf{J}}_r\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}$$

${\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}$的雅可比

由于：

$${\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}={\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\left[\Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]$$

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

对于$\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$，可以直接得到答案：

$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{b}}_i^g}& =-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{b}}_i^a}& =-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\end{array}$$

对于${\mathbf{v}}_i$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\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_{ij}\right)-\left[\Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}-{\mathbf{R}}_i^T\delta {\mathbf{v}}_i\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{v}}_i}=-{\mathbf{R}}_i^T$$

对于${\mathbf{v}}_j$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\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_{ij}\right)-\left[\Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}+{\mathbf{R}}_i^T\delta {\mathbf{v}}_j\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{v}}_j}={\mathbf{R}}_i^T$$

对于${\mathbf{R}}_i$，右扰动模型：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i))& =({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i){)}^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\left[\Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & =({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i){)}^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\Delta {\hat{\mathbf{v}}}_{ij}\\ & \stackrel{1}{=}\mathrm{Exp}(-\delta {\varphi }_i){\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\Delta {\hat{\mathbf{v}}}_{ij}\\ & \stackrel{2}{\approx }(\mathbf{I}-(\delta {\varphi }_i{)}^{\wedge }){\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\Delta {\hat{\mathbf{v}}}_{ij}\\ & ={\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)-\Delta {\hat{\mathbf{v}}}_{ij}-(\delta {\varphi }_i{)}^{\wedge }{\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\\ & \stackrel{3}{=}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}+{\left[{\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\right]}^{\wedge }\cdot \delta {\varphi }_i\end{array}$$

其中1处利用$(AB{)}^T=B^T{A}^T$、$\mathrm{Exp}(\varphi {)}^T=\mathrm{Exp}(-\varphi )$的性质。

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

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

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{R}}_i}={\left[{\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\right]}^{\wedge }$$

${\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}$的雅可比

由于：

$${\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}={\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]$$

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

对于$\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$，可以直接得到答案：

$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{b}}_i^g}& =-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{b}}_i^a}& =-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\end{array}$$

对于${\mathbf{p}}_j$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\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_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}+{\mathbf{R}}_i^T{\mathbf{R}}_j\delta {\mathbf{p}}_j\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{p}}_j}={\mathbf{R}}_i^T{\mathbf{R}}_j$$

对于${\mathbf{p}}_i$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\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_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}-{\mathbf{R}}_i^T{\mathbf{R}}_i\delta {\mathbf{p}}_i\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{p}}_i}=-{\mathbf{R}}_i^T{\mathbf{R}}_i=-\mathbf{I}$$

对于${\mathbf{v}}_i$：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\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_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}-{\mathbf{R}}_i^T\Delta t_{ij}\delta {\mathbf{v}}_i\end{array}$$

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{v}}_i}=-{\mathbf{R}}_i^T\Delta t_{ij}$$

对于${\mathbf{R}}_i$，右扰动模型：

$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i))& =({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i){)}^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\left[\Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\delta {\mathbf{b}}_i^a\right]\\ & =({\mathbf{R}}_i\mathrm{Exp}(\delta {\varphi }_i){)}^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\Delta {\hat{\mathbf{p}}}_{ij}\\ & \stackrel{1}{=}\mathrm{Exp}(-\delta {\varphi }_i){\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\Delta {\hat{\mathbf{p}}}_{ij}\\ & \stackrel{2}{\approx }(\mathbf{I}-\delta {\varphi }_i^{\wedge }){\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\Delta {\hat{\mathbf{p}}}_{ij}\\ & ={\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)-\Delta {\hat{\mathbf{p}}}_{ij}-\delta {\varphi }_i^{\wedge }{\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)\\ & \stackrel{3}{=}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}+{\left[{\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)\right]}^{\wedge }\delta {\varphi }_i\end{array}$$

其中1处利用$(AB{)}^T=B^T{A}^T$、$\mathrm{Exp}(\varphi {)}^T=\mathrm{Exp}(-\varphi )$的性质。

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

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

那么：

$$\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{R}}_i}={\left[{\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right)\right]}^{\wedge }$$

结束。

总结

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

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

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

$$残{差}_{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以及其预积分