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

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

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

本章开始将会开始对IMU预积分进行详细的推导。本次的推导采用李群李代数的形式来表示旋转，对应的是ORB-SLAM3的方案。比如VINS，采用的四元数方案，可能从推导过程上看有一些不同。

本文主要通过PVQ增量真值=PVQ增量测量值-PVQ增量噪声，将增量噪声进行分离操作，并探究其分布形式和递推形式。

IMU预积分推导

先说一下李群李代数的定义吧：

$$\dot{\mathbf{R}}={\varphi }^{\wedge }\mathbf{R}$$

$$\mathbf{R}=\mathrm{Exp}(\varphi )=\exp ({\varphi }^{\wedge })$$

其中，$\mathbf{R}$为李群SO(3)，$\varphi$为对应的李代数so(3)，两者之间相差一个指数对数的关系。如果是$\mathrm{Exp}$，括号里面是李代数向量，如果是$\exp$，括号里面是李代数向量对应的反对称矩阵。

有时候，会有用$\vec{\varphi }$的表示，其实与$\varphi$的含义是一致的。

PVQ增量真值

假设$i$帧的Q、V、P分别是$R_i$、$v_i$、$p_i$，则可以利用从$k=i$到$k=j-1$帧的所有IMU测量值，直接更新得到$j$帧的$R_j$、$v_j$、$p_j$，详细如下：

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

其中，${\tilde{\mathbf{\omega }}}_k$是第$k$帧的角速度的测量值，${\mathbf{b}}_k^g$是第$k$帧的角速度零偏，${\mathbf{\eta }}_k^g$是第$k$帧的角速度测量噪声。

$${\mathbf{v}}_j={\mathbf{v}}_i+\mathbf{g}\cdot \Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_k^a-{\eta }_k^a\right)\cdot \Delta t$$

其中，$g$是重力加速度，${\tilde{\mathbf{a}}}_k$是第$k$帧的加速度的测量值，${\mathbf{b}}_k^a$是第$k$帧的加速度零偏，${\mathbf{\eta }}_k^a$是第$k$帧的加速度测量噪声。加速度的测量值耦合了重力加速度，因此需要加上一个$\mathbf{g}\cdot \Delta t_{ij}$进行抵消。

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

加速度的测量值耦合了重力加速度，因此需要加上一个$\frac{1}{2}\mathbf{g}\cdot \Delta t^2$进行抵消。

同时：

$$\Delta t_{ij}=\sum _{k=i}^{j-1}\Delta t=(j-i)\Delta t$$

为了避免每次更新初始的$R_i$、$v_i$、$p_i$都要重新积分求解$R_j$、$v_j$、$p_j$，引出PVQ增量真值（即预积分）的值。详细如下，这里应用了正交矩阵（旋转矩阵）的转置等于正交矩阵的逆的性质：

$$\begin{array}{rl}\Delta {\mathbf{R}}_{ij}& \triangleq {\mathbf{R}}_i^T{\mathbf{R}}_j\\ & =\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g-{\mathbf{\eta }}_k^g\right)\cdot \Delta t\right)\end{array}$$

$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& \triangleq {\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)\\ & =\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_k^a-{\eta }_k^a\right)\cdot \Delta t\end{array}$$

$$\begin{array}{rl}\Delta {\mathbf{p}}_{ij}& \triangleq {\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)\\ & =\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\cdot \Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_k^a-{\mathbf{\eta }}_k^a\right)\cdot \Delta t^2\right]\end{array}$$

注意：上面公式中的$\Delta {\mathbf{v}}_{ij}$、$\Delta {\mathbf{p}}_{ij}$并不是通常意义上的速度和位置变化量，而是根据IMU加速度计的测量值计算出来的所谓的位移和速度增量，由于IMU加速度测量值耦合了重力加速度，因此对应的IMU预积分真值也必须含有一个重力加速度的分量，否则无法解释速度的变化量为什么还要减去$\mathbf{g}\cdot \Delta t_{ij}$，位置的变化量为什么还要减去$\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2$。

由积分引出预积分，预积分里面的每一项与起始状态无关，可以认为都是相对量，这个好处在于计算预积分时不需要考虑起始状态，值得注意的是关于速度与位置的预积分里面都包含了重力。这部分相对量就是PVQ增量真值。

即PVQ增量真值的核心就是，消除起始状态对积分的影响，同时保留重力的影响。

上面将PVQ增量真值表达成，传感器增量测量值减去它的零偏与噪声，其中零偏可以作为状态量去得出，但是噪声是没有办法得出的。通常的办法就是通过计算噪声的方式来将其过滤掉，当然无论是优化还是滤波都逃不过一个重要的矩阵——信息矩阵（协方差矩阵的逆）。由于假设了传感器噪声（${\eta }_k^g$、${\eta }_k^a$）是高斯白噪声，所以传感器噪声的方差对PVQ增量噪声的方差（$\delta {\vec{\varphi }}_{ij}$、$\delta {\mathbf{v}}_{ij}$、$\delta {\mathbf{p}}_{ij}$）的影响可以通过高斯分布推理过来的。即推导出PVQ增量噪声关于传感器噪声的式子，还要推出其协方差之间的关系。

PVQ增量噪声分离

由于：

$$PVQ增量真值=PVQ增量测量值-PVQ增量噪声$$

下面分别对$\Delta {\mathbf{R}}_{ij}$、$\Delta {\mathbf{v}}_{ij}$、$\Delta {\mathbf{p}}_{ij}$进行整理，尝试将噪声项${\mathbf{\eta }}_k^g$、${\mathbf{\eta }}_k^a$，从PVQ增量测量值中分离出来，使PVQ增量真值具有上述的形式，以便于后续推导出噪声的递推公式。

假设在预积分的区间内，两帧间的零偏是相等的，即${\mathbf{b}}_i^g={\mathbf{b}}_{i+1}^g=\cdots ={\mathbf{b}}_j^g$以及${\mathbf{b}}_i^a={\mathbf{b}}_{i+1}^a=\cdots ={\mathbf{b}}_j^a$。

$\Delta {\mathbf{R}}_{ij}$真值分离

对于$\Delta {\mathbf{R}}_{ij}$，则有：

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

其中1处使用了BCH近似性质：当 $\delta \vec{\varphi }$是小量时：

$$\mathrm{Exp}(\vec{\varphi }+\delta \vec{\varphi })\approx \mathrm{Exp}(\vec{\varphi })\cdot \mathrm{Exp}\left({\mathbf{J}}_r(\vec{\varphi })\cdot \delta \vec{\varphi }\right)$$

$$\mathrm{Exp}\left(\vec{\varphi }+{\mathbf{J}}_r^{-1}(\vec{\varphi })\cdot \delta \vec{\varphi }\right)\approx \mathrm{Exp}(\vec{\varphi })\cdot \mathrm{Exp}(\delta \vec{\varphi })$$

其中2处将 $\prod$展开，3和4处利用Adjoint性质，将所有的 $\Delta \tilde{\mathbf{R}}$ 换到最左侧，这里需要注意$\mathrm{Exp}\left(\right)$ 内部的 $\Delta \tilde{\mathbf{R}}$的下标和数量：

$$\mathrm{Exp}(\vec{\varphi })\cdot \mathbf{R}=\mathbf{R}\cdot \mathrm{Exp}\left({\mathbf{R}}^T\vec{\varphi }\right)$$

令：

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

$$\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)$$

$$\mathrm{Exp}\left(-\delta {\vec{\varphi }}_{ij}\right)=\prod _{k=i}^{j-1}\mathrm{Exp}\left(-\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r^k\cdot {\eta }_k^g\Delta t\right)$$

则有：

$$\Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\vec{\varphi }}_{ij}\right)$$

$\Delta {\tilde{\mathbf{R}}}_{ij}$即PVQ增量测量值，它由陀螺仪测量值和对陀螺仪零偏的估计得到，而 $\delta {\vec{\varphi }}_{ij}$或 $\mathrm{Exp}\left(\delta {\vec{\varphi }}_{ij}\right)$ 即测量噪声。

$\Delta {\mathbf{v}}_{ij}$真值分离

将$\Delta {\mathbf{R}}_{ij}$真值整理好的式子，带入$\Delta {\mathbf{v}}_{ij}$真值式子中，进行整理：

$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& =\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\eta }_k^a\right)\cdot \Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \mathrm{Exp}\left(-\delta {\vec{\varphi }}_{ik}\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\eta }_k^a\right)\cdot \Delta t\\ & \stackrel{1}{\approx }\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left(\mathbf{I}-\delta {\vec{\varphi }}_{ik}^{\wedge }\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\mathbf{\eta }}_k^a\right)\cdot \Delta t\\ & \stackrel{2}{\approx }\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left(\mathbf{I}-\delta {\vec{\varphi }}_{ik}^{\wedge }\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)\cdot \Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^a\Delta t\right]\\ & \stackrel{3}{=}\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+\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ik}\cdot \Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^a\Delta t\right]\\ & =\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]+\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ik}\cdot \Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^a\Delta t\right]\end{array}$$

其中1处使用了当$\vec{\varphi }$是小量时，$\exp \left({\vec{\varphi }}^{\wedge }\right)\approx \mathbf{I}+{\vec{\varphi }}^{\wedge }$，或者$\mathrm{Exp}(\vec{\varphi })\approx \mathbf{I}+{\vec{\varphi }}^{\wedge }$。

其中2处忽略高阶小项$\delta {\vec{\varphi }}_{ik}^{\wedge }{\eta }_k^a$。

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

令：

$$\begin{array}{rl}\Delta {\tilde{\mathbf{v}}}_{ij}& \triangleq \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]\\ \delta {\mathbf{v}}_{ij}& \triangleq \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^a\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ik}\cdot \Delta t\right]\end{array}$$

则有：

$$\Delta {\mathbf{v}}_{ij}\triangleq \Delta {\tilde{\mathbf{v}}}_{ij}-\delta {\mathbf{v}}_{ij}$$

${\tilde{\mathbf{v}}}_{ij}$即速度增量测量值，它由IMU测量值和对零偏的估计或猜测计算得到，而$\delta {\mathbf{v}}_{ij}$即其测量噪声。

$\Delta {\mathbf{p}}_{ij}$真值分离

将$\Delta {\mathbf{R}}_{ij}$真值、$\Delta {\mathbf{v}}_{ij}$真值整理好的式子，带入$\Delta {\mathbf{p}}_{ij}$真值式子中，进行整理：

$$\begin{array}{rl}\Delta {\mathbf{p}}_{ij}& =\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\cdot \Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\mathbf{\eta }}_k^a\right)\cdot \Delta t^2\right]\\ & \approx \sum _{k=i}^{j-1}\left[\left(\Delta {\tilde{\mathbf{v}}}_{ik}-\delta {\mathbf{v}}_{ik}\right)\cdot \Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \mathrm{Exp}\left(-\delta {\vec{\varphi }}_{ik}\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\mathbf{\eta }}_k^a\right)\cdot \Delta t^2\right]\\ & \stackrel{1}{\approx }\sum _{k=i}^{j-1}\left[\left(\Delta {\tilde{\mathbf{v}}}_{ik}-\delta {\mathbf{v}}_{ik}\right)\cdot \Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left(\mathbf{I}-\delta {\vec{\varphi }}_{ik}^{\wedge }\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a-{\mathbf{\eta }}_k^a\right)\cdot \Delta t^2\right]\\ & \stackrel{2}{\approx }\sum _{k=i}^{j-1}\left[\left(\Delta {\tilde{\mathbf{v}}}_{ik}-\delta {\mathbf{v}}_{ik}\right)\cdot \Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left(\mathbf{I}-\delta {\vec{\varphi }}_{ik}^{\wedge }\right)\cdot \left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)\cdot \Delta t^2-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_k^a\Delta t^2\right]\\ & \stackrel{3}{=}\sum _{k=i}^{j-1}[\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+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\delta {\vec{\varphi }}_{ik}\Delta t^2-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_k^a\Delta t^2-\delta {\mathbf{v}}_{ik}\Delta t]\end{array}$$

其中1处使用了当$\vec{\varphi }$是小量时，$\exp \left({\vec{\varphi }}^{\wedge }\right)\approx \mathbf{I}+{\vec{\varphi }}^{\wedge }$，或者$\mathrm{Exp}(\vec{\varphi })\approx \mathbf{I}+{\vec{\varphi }}^{\wedge }$。

其中2处忽略高阶小项$\delta {\vec{\varphi }}_{ik}^{\wedge }{\eta }_k^a$。

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

令：

$$\begin{array}{rl}& \Delta {\tilde{\mathbf{p}}}_{ij}\triangleq \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]\\ & \delta {\mathbf{p}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\delta {\mathbf{v}}_{ik}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\delta {\vec{\varphi }}_{ik}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_k^a\Delta t^2\right]\end{array}$$

则有：

$$\Delta {\mathbf{p}}_{ij}\triangleq \Delta {\tilde{\mathbf{p}}}_{ij}-\delta {\mathbf{p}}_{ij}$$

$\Delta {\tilde{\mathbf{p}}}_{ij}$即位置增量测量值，它由IMU测量值和对零偏的估计得到，而$\delta {\mathbf{p}}_{ij}$即其测量噪声。

小结

上面得到PVQ增量真值和测量值的关系如下：

$$\begin{array}{rl}& \Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\vec{\varphi }}_{ij}\right)\\ & \Delta {\mathbf{v}}_{ij}\triangleq \Delta {\tilde{\mathbf{v}}}_{ij}-\delta {\mathbf{v}}_{ij}\\ & \Delta {\mathbf{p}}_{ij}\triangleq \Delta {\tilde{\mathbf{p}}}_{ij}-\delta {\mathbf{p}}_{ij}\end{array}$$

我们也将PVQ增量噪声，称为IMU预积分测量噪声；PVQ增量测量值，称为IMU预积分测量值。

PVQ增量噪声的分布形式

下面对PVQ增量测量噪声进行分析，证明其符合高斯分布（目的是给出其协方差的计算表达式），令POV增量噪声为：

$${\eta }_{ij}^{\Delta }\triangleq {\left[\begin{array}{lll}\delta {\vec{\varphi }}_{ij}^T& \delta {\mathbf{v}}_{ij}^T& \delta {\mathbf{p}}_{ij}^T\end{array}\right]}^T$$

我们希望其满足高斯分布，即 ${\mathbf{\eta }}_{ij}^{\Delta }\sim N\left(0_{9\times 1},{\mathbf{\Sigma }}_{ij}\right)$ 。由于 ${\mathbf{\eta }}_{ij}^{\Delta }$是 $\delta {\vec{\varphi }}_{ij}^T$、$\delta {\mathbf{v}}_{ij}^T$、$\delta {\mathbf{p}}_{ij}^T$ 的线性组合，下面分别分析这三个噪声项的分布形式。

$\delta {\vec{\varphi }}_{ij}^T$的分布形式

根据上面分离出的噪声$\delta {\vec{\varphi }}_{ij}$：

$$\delta {\vec{\varphi }}_{ij}=-\log \left(\prod _{k=i}^{j-1}\mathrm{Exp}\left(-\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r^k\cdot {\eta }_k^g\Delta t\right)\right)$$

令：

$${\mathbf{\xi }}_k=\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r^k\cdot {\eta }_k^g\Delta t$$

由于 ${\eta }_k^g$ 是小量，因此 ${\mathbf{\xi }}_k$也是小量，于是 ${\mathbf{J}}_r\left({\xi }_k\right)\approx \mathbf{I}$、${\mathbf{J}}_r^{-1}\left({\xi }_k\right)\approx \mathbf{I}$，并利用BCH公式的近似形式：

$$\log (\mathrm{Exp}(\vec{\varphi })\cdot \mathrm{Exp}(\delta \vec{\varphi }))=\vec{\varphi }+{\mathbf{J}}_r^{-1}(\vec{\varphi })\cdot \delta \vec{\varphi }$$

因此：

$$\begin{array}{rl}\delta {\vec{\varphi }}_{ij}& =-\log \left(\prod _{k=i}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\\ & =-\log \left(\mathrm{Exp}\left(-{\xi }_i\right)\prod _{k=i+1}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\\ & \approx -\left(-{\xi }_i+\mathbf{I}\cdot \log \left(\prod _{k=i+1}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\right)\\ & ={\xi }_i-\log \left(\prod _{k=i+1}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\\ & ={\xi }_i-\log \left(\mathrm{Exp}\left(-{\xi }_{i+1}\right)\prod _{k=i+2}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\\ & \approx {\xi }_i+{\xi }_{i+1}-\log \left(\prod _{k=i+2}^{j-1}\mathrm{Exp}\left(-{\xi }_k\right)\right)\\ & \approx \sum _{k=i}^{j-1}{\xi }_k\end{array}$$

即：

$$\delta {\vec{\varphi }}_{ij}\approx \sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1j}^T{\mathbf{J}}_r^k{\eta }_k^g\Delta t$$

由于 $\Delta {\tilde{\mathbf{R}}}_{k+1j}^T$、${\mathbf{J}}_r^k$、$\Delta t$都是已知量，而 ${\eta }_k^g$ 是零均值高斯噪声，因此 $\delta {\vec{\varphi }}_{ij}$ （的一阶近似）也是零均值高斯噪声。

$\delta {\mathbf{v}}_{ij}^T$ 的分布形式

由于 $\delta {\vec{\varphi }}_{ij}$ 近似拥有了高斯噪声的形式，且 ${\mathbf{\eta }}_k^a$ 也是零均值高斯噪声，根据 $\delta {\mathbf{v}}_{ij}^T$ 的表达式可知其也拥有高斯分布的形式。

$\delta {\mathbf{p}}_{ij}^T$ 的分布形式

类似于 $\delta {\mathbf{v}}_{ij}^T$，$\delta {\mathbf{p}}_{ij}^T$也拥有高斯分布的形式。

PVQ增量噪声递推

下面推导预积分测量噪声的递推形式，即 ${\eta }_{ij-1}^{\Delta }\to {\eta }_{ij}^{\Delta }$，及其协方差 ${\mathbf{\Sigma }}_{ij}$ 的递推形式 ${\mathbf{\Sigma }}_{ij-1}\to {\mathbf{\Sigma }}_{ij}$ 。

下面依次推导 $\delta {\vec{\varphi }}_{ij-1}\to \delta {\vec{\varphi }}_{ij}$、$\delta {\mathbf{v}}_{ij-1}\to \delta {\mathbf{v}}_{ij}$、$\delta {\mathbf{p}}_{ij-1}\to \delta {\mathbf{p}}_{ij}$。

$\delta {\vec{\varphi }}_{ij-1}\to \delta {\vec{\varphi }}_{ij}$递推

$$\begin{array}{rl}\delta {\vec{\varphi }}_{ij}& =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1j}^T{\mathbf{J}}_r^k{\eta }_k^g\Delta t\\ & =\sum _{k=i}^{j-2}\Delta {\tilde{\mathbf{R}}}_{k+1j}^T{\mathbf{J}}_r^k{\mathbf{\eta }}_k^g\Delta t+\Delta {\tilde{\mathbf{R}}}_{jj}^T{\mathbf{J}}_r^{j-1}{\mathbf{\eta }}_{j-1}^g\Delta t\\ & \stackrel{1}{=}\sum _{k=i}^{j-2}{\left(\Delta {\tilde{\mathbf{R}}}_{k+1j-1}\Delta {\tilde{\mathbf{R}}}_{j-1j}\right)}^T{\mathbf{J}}_r^k{\mathbf{\eta }}_k^g\Delta t+{\mathbf{J}}_r^{j-1}{\mathbf{\eta }}_{j-1}^g\Delta t\\ & =\Delta {\tilde{\mathbf{R}}}_{jj-1}\sum _{k=i}^{j-2}\Delta {\tilde{\mathbf{R}}}_{k+1j-1}^T{\mathbf{J}}_r^k{\mathbf{\eta }}_k^g\Delta t+{\mathbf{J}}_r^{j-1}{\mathbf{\eta }}_{j-1}^g\Delta t\\ & =\Delta {\tilde{\mathbf{R}}}_{jj-1}\delta {\vec{\varphi }}_{ij-1}+{\mathbf{J}}_r^{j-1}{\mathbf{\eta }}_{j-1}^g\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 {\mathbf{v}}_{ij-1}\to \delta {\mathbf{v}}_{ij}$递推

$$\begin{array}{rl}\delta {\mathbf{v}}_{ij}=& \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_k^{\alpha d}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ik}\cdot \Delta t\right]\\ =& \sum _{k=i}^{j-2}\left[\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_k^a\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ik}\cdot \Delta t\right]+\Delta {\tilde{\mathbf{R}}}_{ij-1}{\mathbf{\eta }}_{j-1}^a\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ij-1}\cdot \Delta t\\ =& \delta {\mathbf{v}}_{ij-1}+\Delta {\tilde{\mathbf{R}}}_{ij-1}{\mathbf{\eta }}_{j-1}^a\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\vec{\varphi }}_{ij-1}\cdot \Delta t\end{array}$$

直接进行加项拆分即可完成推导。

$\delta {\mathbf{p}}_{ij-1}\to \delta {\mathbf{p}}_{ij}$递推

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

直接进行加项拆分即可完成推导。

小结

已知定义：

$${\eta }_{ij}^{\Delta }\triangleq {\left[\begin{array}{lll}\delta {\vec{\varphi }}_{ij}^T& \delta {\mathbf{v}}_{ij}^T& \delta {\mathbf{p}}_{ij}^T\end{array}\right]}^T$$

令：

$${\mathbf{\eta }}_k^{}={\left[{\left({\mathbf{\eta }}_k^g\right)}^T{\left({\eta }_k^a\right)}^T\right]}^T$$

综上可得 ${\mathbf{\eta }}_{ij}^{\Delta }$ 的递推形式如下：

$$\begin{array}{rl}{\mathbf{\eta }}_{ij}^{\Delta }=& \left[\begin{array}{ccc}\Delta {\tilde{\mathbf{R}}}_{jj-1}& 0& 0\\ -\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t& \mathbf{I}& 0\\ -\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t^2& \Delta t\mathbf{I}& \mathbf{I}\end{array}\right]{\eta }_{ij-1}^{\Delta }+\left[\begin{array}{cc}{\mathbf{J}}_r^{j-1}\Delta t& 0\\ 0& \Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t\\ 0& \frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t^2\end{array}\right]{\mathbf{\eta }}_{j-1}^{}\end{array}$$

令：

$$\begin{array}{rl}& {\mathbf{A}}_{j-1}=\left[\begin{array}{ccc}\Delta {\tilde{\mathbf{R}}}_{jj-1}& 0& 0\\ -\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t& \mathbf{I}& 0\\ -\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t^2& \Delta t\mathbf{I}& \mathbf{I}\end{array}\right]\\ & {\mathbf{B}}_{j-1}=\left[\begin{array}{cc}{\mathbf{J}}_r^{j-1}\Delta t& 0\\ 0& \Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t\\ 0& \frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t^2\end{array}\right]\end{array}$$

则有PVQ增量噪声的递推形式：

$${\eta }_{ij}^{\Delta }={\mathbf{A}}_{j-1}{\eta }_{ij-1}^{\Delta }+{\mathbf{B}}_{j-1}{\eta }_{j-1}^{}$$

PVQ增量噪声的协方差矩阵就有了如下的递推计算形式：

$${\mathbf{\Sigma }}_{ij}={\mathbf{A}}_{j-1}{\mathbf{\Sigma }}_{ij-1}{\mathbf{A}}_{j-1}^T+{\mathbf{B}}_{j-1}{\mathbf{\Sigma }}_{\mathbf{\eta }}{\mathbf{B}}_{j-1}^T$$

从形式上看，PVQ增量噪声的协方差的递推形式类似于卡尔曼滤波中的状态变量协方差的预测方程，其中的 $\mathbf{Q}$ 就相当于 ${\Sigma }_{\eta }$ ，在每个递推周期都固定的加上这样一个常量噪声，表示从当前状态转移到下一个状态的过程中，存在各种噪声，总是会引入新的误差：

$$\overline{\mathbf{P}}={\mathbf{FPF}}^{\top }+\mathbf{Q}$$

PVQ增量噪声的协方差矩阵（即噪声分布）将用来计算信息矩阵，在优化框架中起到平衡权重的作用。在实际应用中首先要求协方差矩阵的逆矩阵，相当于取了协方差的倒数，方差越大权重越小，反之权重越大，然后再将逆矩阵转成信息矩阵，与残差相乘，起到调节残差比例的作用。

关于噪声的内容到此为止，接下来讨论零偏的问题。

相关阅读

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