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

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

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

上一篇文章对IMU预积分噪声进行分离，并对其递推形式进行推导，最终引出了其信息矩阵的递推形式。而本文将会对零偏改变时IMU预积分的速算进行推导。这是IMU预积分节省计算量的关键。

IMU预积分推导

零偏更新时的PVQ增量测量值更新

前面的计算和推导，都是在假设积分区间内陀螺和加计的零偏恒定的基础上推导的。当零偏发生变化时，若仍按照前述公式，PVQ增量测量值需要整个重新计算一遍，这将非常的耗费算力。为了解决这个问题，提出了利用线性化来进行零偏变化时的一阶近似更新方法。

具体来说，我们把PVQ增量观测值看成 ${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$ 的函数，那么，当 ${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$ 更新了 $\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$ 之后，PVQ增量观测值应作如下的修正：

$$\begin{array}{rl}& \Delta {\tilde{\mathbf{R}}}_{ij}\left({\mathbf{b}}_i^g+\delta {\mathbf{b}}_i^g\right)\approx \Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_i^g)\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)\\ & \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_i^g+\delta {\mathbf{b}}_i^g,{\mathbf{b}}_i^a+\delta {\mathbf{b}}_i^a)\approx \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_i^g,{\mathbf{b}}_i^a)+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a\\ & \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_i^g+\delta {\mathbf{b}}_i^g,{\mathbf{b}}_i^a+\delta {\mathbf{b}}_i^a)\approx \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_i^g,{\mathbf{b}}_i^a)+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a\end{array}$$

需要注意的是：这里的$\Delta {\tilde{\mathbf{R}}}_{ij}$括号里面表示的是零偏更新前、更新后的PVQ增量测量值的标记，并不表示乘法运算！

为了更清晰地表示，将零偏更新前的仍保留原本的写法$\Delta {\tilde{\mathbf{R}}}_{ij}$，零偏更新后的表示为$\Delta {\hat{\mathbf{R}}}_{ij}$。

即：

$$\begin{array}{rl}& \Delta {\hat{\mathbf{R}}}_{ij}\approx \Delta {\tilde{\mathbf{R}}}_{ij}\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)\\ & \Delta {\hat{\mathbf{v}}}_{ij}\approx \Delta {\tilde{\mathbf{v}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a\\ & \Delta {\hat{\mathbf{p}}}_{ij}\approx \Delta {\tilde{\mathbf{p}}}_{ij}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a\end{array}$$

下面分别对这些雅可比进行分析。

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

在前文$\Delta {\mathbf{R}}_{ij}$真值分离中，最终得到：

$$\Delta {\tilde{\mathbf{R}}}_{ij}=\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g\right)\Delta t\right)$$

那么，当 ${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$ 更新了 $\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$ 后：

$$\begin{array}{rl}\Delta {\hat{\mathbf{R}}}_{ij}& =\prod _{k=i}^{j-1}\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_k-({\mathbf{b}}_i^g+\delta {\mathbf{b}}_i^g))\Delta t\right)\\ & =\prod _{k=i}^{j-1}\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g)\Delta t-\delta {\mathbf{b}}_i^g\Delta t\right)\\ & \stackrel{1}{=}\prod _{k=i}^{j-1}\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g)\Delta t\right)\mathrm{Exp}(-{\mathbf{J}}_r^k\delta {\mathbf{b}}_i^g\Delta t)\\ & \stackrel{2}{=}\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_i-{\mathbf{b}}_i^g)\Delta t\right)\mathrm{Exp}(-{\mathbf{J}}_r^i\delta {\mathbf{b}}_i^g\Delta t)\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_{i+1}-{\mathbf{b}}_i^g)\Delta t\right)\mathrm{Exp}(-{\mathbf{J}}_r^{i+1}\delta {\mathbf{b}}_i^g\Delta t)\dots \\ & =\Delta {\tilde{\mathbf{R}}}_{i,i+1}\mathrm{Exp}(-{\mathbf{J}}_r^i\delta {\mathbf{b}}_i^g\Delta t)\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}\mathrm{Exp}(-{\mathbf{J}}_r^{i+1}\delta {\mathbf{b}}_i^g\Delta t)\Delta {\tilde{\mathbf{R}}}_{i+2,i+3}\dots \\ & \stackrel{3}{=}\Delta {\tilde{\mathbf{R}}}_{i,i+1}\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}^{\mathrm{T}}{\mathbf{J}}_{r,i}\delta {\mathbf{b}}_i^g\Delta t)\mathrm{Exp}(-{\mathbf{J}}_r^{i+1}\delta {\mathbf{b}}_i^g\Delta t)\Delta {\tilde{\mathbf{R}}}_{i+2,i+3}\dots \\ & \stackrel{4}{=}\Delta {\tilde{\mathbf{R}}}_{ij}\prod _{k=i}^{j-1}\mathrm{Exp}\left(-\Delta {\tilde{\mathbf{R}}}_{k+1,j}^{\mathrm{T}}{\mathbf{J}}_r^k\delta {\mathbf{b}}_i^g\Delta t\right)\\ & \approx \Delta {\tilde{\mathbf{R}}}_{ij}\mathrm{Exp}\left(-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\delta {\mathbf{b}}_i^g\right)\end{array}$$

其中1处使用了BCH近似性质，其中2处将 $\prod$展开，3和4处利用Adjoint性质。这和前文$\Delta {\mathbf{R}}_{ij}$真值分离中使用的方法几乎一致。

其中：

$${\mathbf{J}}_r^k={\mathbf{J}}_r\left(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g)\Delta t\right)$$

对比可知：

$$\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)=\mathrm{Exp}\left(-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\delta {\mathbf{b}}_i^g\right)$$

即（雅可比矩阵也要改成递推形式）：

$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_i^g}& =-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\\ & =-\sum _{k=i}^{j-2}\left(\Delta {\tilde{\mathbf{R}}}_{k+1,j}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\right)-\Delta {\tilde{\mathbf{R}}}_{j,j}^{\mathrm{T}}{\mathbf{J}}_r^{j-1}\Delta t\\ & \stackrel{1}{=}-\sum _{k=i}^{j-2}\left((\Delta {\tilde{\mathbf{R}}}_{k+1,j-1}\Delta {\tilde{\mathbf{R}}}_{j-1,j}{)}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\right)-{\mathbf{J}}_r^{j-1}\Delta t\\ & =-\Delta {\tilde{\mathbf{R}}}_{j,j-1}\sum _{k=i}^{j-2}\left(\Delta {\tilde{\mathbf{R}}}_{k+1,j-1}^{\mathrm{T}}{\mathbf{J}}_r^k\Delta t\right)-{\mathbf{J}}_r^{j-1}\Delta t\\ & =\Delta {\tilde{\mathbf{R}}}_{j,j-1}\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}-{\mathbf{J}}_r^{j-1}\Delta t\end{array}$$

其中1处利用了 $\Delta {\tilde{\mathbf{R}}}_{jj}^T=\mathbf{I}$ 以及 $\Delta {\tilde{\mathbf{R}}}_{lm}\Delta {\tilde{\mathbf{R}}}_{mn}=\Delta {\tilde{\mathbf{R}}}_{ln}$ 的性质，推导过程中进行了一些变形。这和前文$\delta {\vec{\varphi }}_{ij-1}\to \delta {\vec{\varphi }}_{ij}$递推中使用的方法几乎一致。

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

在前文$\Delta {\mathbf{v}}_{ij}$真值分离中，最终得到：

$$\Delta {\tilde{\mathbf{v}}}_{ij}=\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)\cdot \Delta t\right]$$

那么，当 ${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$ 更新了 $\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$ 后：

$$\begin{array}{rl}\Delta {\hat{\mathbf{v}}}_{ij}& =\sum _{k=i}^{j-1}\left[\Delta {\hat{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-\delta {\mathbf{b}}_i^a)\Delta t\right]\\ & =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-\delta {\mathbf{b}}_i^a)\Delta t\\ & \stackrel{1}{=}\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\left(\mathbf{I}+{\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)}^{\wedge }\right)({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-\delta {\mathbf{b}}_i^a)\Delta t\\ & \stackrel{2}{\approx }\Delta {\tilde{\mathbf{v}}}_{ij}-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t\delta {\mathbf{b}}_i^a-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t\delta {\mathbf{b}}_i^g\end{array}$$

其中1处使用了$\mathrm{Exp}()$的小量近似，其中2处忽略高阶小项，并使用了外积的交换律。这和前文$\Delta {\mathbf{v}}_{ij}$真值分离中使用的方法几乎一致。

对比可知：

$$\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a=-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t\delta {\mathbf{b}}_i^a-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t\delta {\mathbf{b}}_i^g$$

即（雅可比矩阵也要改成递推形式）：

$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^g}& =-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t\\ & \stackrel{1}{=}\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}-\left(\Delta {\tilde{\mathbf{R}}}_{ij-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}\Delta t\right)\\ \frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_i^a}& =-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t\\ & \stackrel{2}{=}\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij-1}}{\partial {\mathbf{b}}_i^a}-\Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t\end{array}$$

其中1、2处直接进行加项拆分即可完成推导。这和前文$\delta {\mathbf{v}}_{ij-1}\to \delta {\mathbf{v}}_{ij}$递推中使用的方法几乎一致。

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

在前文$\Delta {\mathbf{p}}_{ij}$真值分离中，最终得到：

$$\Delta {\tilde{\mathbf{p}}}_{ij}=\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)\Delta t^2\right]$$

那么，当 ${\mathbf{b}}_i^g$、${\mathbf{b}}_i^a$ 更新了 $\delta {\mathbf{b}}_i^g$、$\delta {\mathbf{b}}_i^a$ 后：

$$\begin{array}{rl}\Delta {\hat{\mathbf{p}}}_{ij}& =\sum _{k=i}^{j-1}\left[\Delta {\hat{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\hat{\mathbf{R}}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-\delta {\mathbf{b}}_i^a\right)\Delta t^2\right]\\ & \stackrel{1}{=}\sum _{k=i}^{j-1}[\left(\Delta {\tilde{\mathbf{v}}}_{ik}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\left(\mathbf{I}+{\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g\right)}^{\wedge }\right)({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-\delta {\mathbf{b}}_i^a)\Delta t^2]\\ & \stackrel{2}{=}\Delta {\tilde{\mathbf{p}}}_{ij}+\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^a}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\right]\delta {\mathbf{b}}_i^a+\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t^2\right]\delta {\mathbf{b}}_i^g\end{array}$$

其中1处使用了$\mathrm{Exp}()$的小量近似，其中2处忽略高阶小项，并使用了外积的交换律。这和前文$\Delta {\mathbf{p}}_{ij}$真值分离中使用的方法几乎一致。

对比可知：

$$\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^a}\delta {\mathbf{b}}_i^a=\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^a}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\right]\delta {\mathbf{b}}_i^a+\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t^2\right]\delta {\mathbf{b}}_i^g$$

即（雅可比矩阵也要改成递推形式）：

$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^g}& =\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_i^g}\Delta t^2\right]\\ & \stackrel{1}{=}\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}+\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}{\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij-1}}{\partial {\mathbf{b}}_i^g}\Delta t^2\right]\\ \frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_i^a}& =\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}_i^a}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\right]\\ & \stackrel{2}{=}\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij-1}}{\partial {\mathbf{b}}_i^a}+\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij-1}}{\partial {\mathbf{b}}_i^a}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t^2\right]\end{array}$$

其中1、2处直接进行加项拆分即可完成推导。这和前文$\delta {\mathbf{p}}_{ij-1}\to \delta {\mathbf{p}}_{ij}$递推中使用的方法几乎一致。

小结

利用线性化的方法，对于$i$、$j$两帧之间的PVQ增量测量值我们只需要做一次就可以了（即$\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_i^g)$、$\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_i^g,{\mathbf{b}}_i^a)$、$\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_i^g,{\mathbf{b}}_i^a)$ ），通过测量值函数对零偏的偏导数（即雅可比）和零偏更新量 $\delta {\mathbf{b}}_i^g$ 、 $\delta {\mathbf{b}}_i^a$ 就可以近似的计算出修正量，获得新测量值的近似值，而不需要重新积分。

如果优化过程中起始位姿发生了变化，则雅可比也相应更新。而零偏更新量 $\delta {\mathbf{b}}_i^g$ 、 $\delta {\mathbf{b}}_i^a$ 本身就是待优化的变量之一，自然也是相应更新。从而测量值的修正量实现了自动更新。

以上就是IMU预积分避免重新积分，降低运算量的关键。

相关阅读

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