IMU预积分推导（一）

IMU Pre Integration

IMU预积分推导系列文章
IMU预积分推导（一）
IMU预积分推导（二）
IMU预积分推导（三）

1、预积分的引入

1.1、IMU积分

第$i$时刻，系统的状态量为IMU相对世界坐标系的旋转矩阵，速度，位置和偏置
$${\mathbf{x}}_i=[{\mathbf{R}}_i,{\mathbf{v}}_i,{\mathbf{p}}_i,{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}{]}^T$$
在一次积分的基础上，对IMU数据进行二次积分，从$i$时刻一直递推到与其相差较多时间步的$j$时刻，即
$$\begin{array}{rl}{\mathbf{R}}_j& ={\mathbf{R}}_i\prod _{k=i}^{j-1}\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,k}-{\mathbf{\eta }}_{gd,k})\Delta t)\\ {\mathbf{v}}_j& ={\mathbf{v}}_i+\mathbf{g}\Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_k({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t\\ {\mathbf{p}}_j& ={\mathbf{p}}_i+\sum _{k=i}^{j-1}{\mathbf{v}}_k\Delta t+\frac{1}{2}\sum _{k=i}^{j-1}\mathbf{g}\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{\mathbf{R}}_k({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t^2\end{array}\text{\hspace{1em}(1a)(1b)(1c)}$$
其中$\Delta t_{ij}={\sum }_{k=i}^{j-1}\Delta t$。

1.2、预积分定义

直接积分的缺点是，它描述的过程和状态量有关，即如果第$i$时刻的状态量发生改变，则$i+1,\cdots ,j-1$时刻的状态量也会发生改变，这时需要重新计算积分，这对于需要频繁进行优化，且对实时性要求高的系统来说是不可接受的。可以定义相对状态量，使其计算与第$i$时刻状态量无关，该相对量称为预积分
$$\begin{array}{rl}\Delta {\mathbf{R}}_{ij}& \stackrel{\mathrm{def}}{=}{\mathbf{R}}_i^T{\mathbf{R}}_j=\prod _{k=i}^{j-1}\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,k}-{\mathbf{\eta }}_{gd,k})\Delta t)\\ \Delta {\mathbf{v}}_{ij}& \stackrel{\mathrm{def}}{=}{\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})=\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t\\ \Delta {\mathbf{p}}_{ij}& \stackrel{\mathrm{def}}{=}{\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\Delta t_{ij}^2\right)=\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t^2\right]\end{array}\text{\hspace{1em}(2a)(2b)(2c)}$$
下面假设在相邻两个图像关键帧之间零偏是固定不变的，即满足
$$\begin{array}{rl}{\mathbf{b}}_{g,k}& ={\mathbf{b}}_{g,i}={\mathbf{b}}_{g,i+1}=\cdots ={\mathbf{b}}_{g,j-1}\\ {\mathbf{b}}_{a,k}& ={\mathbf{b}}_{a,i}={\mathbf{b}}_{a,i+1}=\cdots ={\mathbf{b}}_{a,j-1}\end{array}$$

故在两帧之间，预积分状态$\Delta {\mathbf{x}}_{ij}=[\Delta {\mathbf{R}}_{ij},\Delta {\mathbf{v}}_{ij},\Delta {\mathbf{p}}_{ij}]$

2、噪声分离

现在希望将陀螺仪测量噪声${\mathbf{\eta }}_{ad,k}$和加速度计测量噪声${\mathbf{\eta }}_{gd,k}$分离出去。

定义仅由IMU数据计算得到的与噪声无关的预积分测量值$\Delta {\tilde{\mathbf{x}}}_{ij}=[\Delta {\tilde{\mathbf{R}}}_{ij},\Delta {\tilde{\mathbf{v}}}_{ij},\Delta {\tilde{\mathbf{p}}}_{ij}]$，其中$\Delta {\tilde{\mathbf{R}}}_{ij},\Delta {\tilde{\mathbf{v}}}_{ij},\Delta {\tilde{\mathbf{p}}}_{ij}$分别是预积分旋转测量值，预积分速度测量值，预积分位移测量值，三者形式如下
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ij}& \stackrel{\mathrm{def}}{=}\prod _{k=i}^{j-1}\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,k})\Delta t)\\ \Delta {\tilde{\mathbf{v}}}_{ij}& \stackrel{\mathrm{def}}{=}\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k})\Delta t\\ \Delta {\tilde{\mathbf{p}}}_{ij}& \stackrel{\mathrm{def}}{=}\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k})\Delta t^2\right]\end{array}\text{\hspace{1em}(3a)(3b)(3c)}$$
记预积分误差为$\delta (\Delta {\mathbf{x}}_{ij})=[\delta (\Delta {\mathbf{\varphi }}_{ij}{)}^T,\delta (\Delta {\mathbf{v}}_{ij}{)}^T,\delta (\Delta {\mathbf{p}}_{ij}{)}^T{]}^T$，其中$\delta (\Delta {\mathbf{\varphi }}_{ij}),\delta (\Delta {\mathbf{v}}_{ij}),\delta (\Delta {\mathbf{p}}_{ij})$分别是预积分旋转误差，预积分速度误差，预积分位移误差。为简便起见，后文将上述三个值分别简记为$\delta {\mathbf{\varphi }}_{ij},\delta {\mathbf{v}}_{ij},\delta {\mathbf{p}}_{ij}$。预积分状态变量真值，预积分测量值，预积分误差之间应该满足$\Delta {\mathbf{x}}_{ij}=\Delta {\tilde{\mathbf{x}}}_{ij}\oplus (-\delta (\Delta {\mathbf{x}}_{ij}))$，该关系也是预积分误差的定义，其中$\ominus$表示广义减法，即
$$\begin{array}{rl}\Delta {\mathbf{R}}_{ij}& =\Delta {\tilde{\mathbf{R}}}_{ij}\mathrm{Exp}(-\delta {\mathbf{\varphi }}_{ij})\\ \Delta {\mathbf{v}}_{ij}& =\Delta {\tilde{\mathbf{v}}}_{ij}-\delta {\mathbf{v}}_{ij}\\ \Delta {\mathbf{p}}_{ij}& =\Delta {\tilde{\mathbf{p}}}_{ij}-\delta {\mathbf{p}}_{ij}\end{array}\text{\hspace{1em}(4a)(4b)(4c)}$$
特别注意，$(1),(2),(3),(4)$其中求和号和求积号应该按照§2来理解，$(1)$中的下标可以用任意满足$i⩽k⩽j$的$k$代替，$(2),(3),(4)$中的下标可以用任意满足$i⩽m⩽n⩽j$的$m,n$代替，计算方式不变。

下面推导满足上式的预积分误差表达式

2.1、预积分旋转误差表达式
$$\begin{array}{rl}\Delta {\mathbf{R}}_{ij}& =\prod _{k=i}^{j-1}\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,i}-{\mathbf{\eta }}_{gd,k})\Delta t)\\ & \approx \prod _{k=i}^{j-1}[\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,i})\Delta t)\mathrm{Exp}(-{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t)]\\ & =\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_i-{\mathbf{b}}_{g,i})\Delta t)\mathrm{Exp}(-{\mathbf{J}}_{r,i}{\mathbf{\eta }}_{gd,i}\Delta t)\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_{i+1}-{\mathbf{b}}_{g,i+1})\Delta t)\mathrm{Exp}(-{\mathbf{J}}_{r,i+1}{\mathbf{\eta }}_{gd,i+1}\Delta t)\cdots \\ & =\Delta {\tilde{\mathbf{R}}}_{i,i+1}\underset{\approx \Delta {\tilde{\mathbf{R}}}_{i+1,i+2}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}^T{\mathbf{J}}_{r,i}{\mathbf{\eta }}_{gd,i}\Delta t)}{\underbrace{\mathrm{Exp}(-{\mathbf{J}}_{r,i}{\mathbf{\eta }}_{gd,i}\Delta t)\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}}}\mathrm{Exp}(-{\mathbf{J}}_{r,i+1}{\mathbf{\eta }}_{gd,i+1}\Delta t)\cdots \\ & \approx \Delta {\tilde{\mathbf{R}}}_{i,i+2}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{i+1,i+2}^T{\mathbf{J}}_{r,i}{\mathbf{\eta }}_{gd,i}\Delta t)\mathrm{Exp}(-{\mathbf{J}}_{r,i+1}{\mathbf{\eta }}_{gd,i+1}\Delta t)\Delta {\tilde{\mathbf{R}}}_{i+2,i+3}\cdots \\ & =\Delta {\tilde{\mathbf{R}}}_{ij}\prod _{k=i}^{j-1}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t)\end{array}\text{\hspace{1em}(5)}$$
其中其中第一行推至第二行应利用了BCH近似，右雅可比采取简化写法，省略对应的向量，即记${\mathbf{J}}_{r,k}={\mathbf{J}}_r(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,i})\Delta t)$。第四行推导至第五行用到了$(A1)$

比较$(4a)$和$(5)$，可以得到
$$\delta {\mathbf{\varphi }}_{ij}=-\mathrm{Log}(\prod _{k=i}^{j-1}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t))\text{\hspace{1em}(6)}$$
2.2、预积分速度误差表达式
$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& =\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-{\mathbf{\eta }}_{ad,k})\Delta t\\ & =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}\mathrm{Exp}(-\delta {\mathbf{\varphi }}_{ik})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-{\mathbf{\eta }}_{ad,k})\Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}(\mathbf{I}-\delta {\mathbf{\varphi }}_{ik}^{\wedge })({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-{\mathbf{\eta }}_{ad,k})\Delta t\\ & \approx \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k})\Delta t+\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t\right]\\ & =\Delta {\tilde{\mathbf{v}}}_{ij}+\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t\right]\end{array}\text{\hspace{1em}(7)}$$
第二行推导至第三行是根据$(6)$，考虑到$\delta {\mathbf{\varphi }}_{ij}$比较小，利用矩阵指数函数的一阶近似化简。第三行推导至第四行运用了外积的反交换律，并舍去含$\delta {\mathbf{\varphi }}_{ij}^{\wedge }{\mathbf{\eta }}_{ad,k}$的高阶小项。

比较$(4b)$和$(7)$，可以得到
$$\delta {\mathbf{v}}_{ij}=\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ij}{\mathbf{\eta }}_{ad,k}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ij}\Delta t\right]\text{\hspace{1em}(8)}$$
2.3、预积分位移误差表达式
$$\begin{array}{rl}\Delta {\mathbf{p}}_{ij}& =\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-{\mathbf{\eta }}_{ad,k})\Delta t^2\right]\\ & =\sum _{k=i}^{j-1}\left[(\Delta {\tilde{\mathbf{v}}}_{ik}-\delta {\mathbf{v}}_{ik})\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}(\mathbf{I}-\delta {\mathbf{\varphi }}_{ik}^{\wedge })({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-{\mathbf{\eta }}_{ad,k})\Delta t^2\right]\\ & \approx \sum _{k=i}^{j-1}\left[(\Delta {\tilde{\mathbf{v}}}_{ik}-\delta {\mathbf{v}}_{ik})\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}(\mathbf{I}-\delta {\mathbf{\varphi }}_{ik}^{\wedge })({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i})\Delta t^2-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t^2\right]\\ & =\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i})\Delta t^2-\delta {\mathbf{v}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t^2-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t^2\right]\end{array}\text{\hspace{1em}(9)}$$
比较$(4c)$和$(9)$，可以得到
$$\delta {\mathbf{p}}_{ij}=\sum _{k=i}^{j-1}\left[\delta {\mathbf{v}}_{ik}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t^2\right]\text{\hspace{1em}(10)}$$

3、误差传播

为避免大量的重复计算，下面考虑预积分误差$\delta (\Delta {\mathbf{x}}_{i,j})$和$\delta (\Delta {\mathbf{x}}_{i,j-1})$之间的递推关系

3.1、预积分旋转误差传播

考虑到预积分旋转误差表达式$(5)$较为复杂，不方便递推，下面对其进行线性化。记${\mathbf{\xi }}_k=\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t$，则
$$\begin{array}{rl}\delta {\mathbf{\varphi }}_{ij}& =-\mathrm{Log}(\prod _{k=i}^{j-1}\mathrm{Exp}(-{\mathbf{\xi }}_k))\\ & =-\mathrm{Log}(\mathrm{Exp}(-{\mathbf{\xi }}_i)\mathrm{Exp}(-{\mathbf{\xi }}_{i+1})\cdots \mathrm{Exp}(-{\mathbf{\xi }}_{j-1}))\\ & \approx -\mathrm{Log}(\mathrm{Exp}(-{\mathbf{\xi }}_i-{\mathbf{J}}_r^{-1}(-{\mathbf{\xi }}_i){\mathbf{\xi }}_{i+1})\cdots \mathrm{Exp}(-{\mathbf{\xi }}_{j-1}))\\ & \approx -\mathrm{Log}(\mathrm{Exp}(-\sum _{k=i}^{j-1}{\mathbf{\xi }}_k))\\ & =\sum _{k=i}^{j-1}{\mathbf{\xi }}_k\end{array}$$
其中第三行推导至第四行是考虑到${\mathbf{\xi }}_k$为小量，其和仍为小量，因此每个右雅可比都接近单位阵。将${\mathbf{\xi }}_k$的定义代回，得到
$$\delta {\mathbf{\varphi }}_{ij}=\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t\text{\hspace{1em}(11)}$$
若$j>i$，则可将预积分旋转误差表达式$(11)$写成递推形式
$$\begin{array}{rl}\delta {\mathbf{\varphi }}_{ij}& =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t\\ & =\sum _{k=i}^{j-2}\underset{(\Delta {\tilde{\mathbf{R}}}_{k+1,j-1}\Delta {\tilde{\mathbf{R}}}_{j-1,j}{)}^T}{\underbrace{\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T}}{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t+\underset{\mathbf{I}}{\underbrace{\Delta {\tilde{\mathbf{R}}}_{j,j}^T}}{\mathbf{J}}_{r,j-1}{\mathbf{\eta }}_{gd,j-1}\Delta t\\ & =\Delta {\tilde{\mathbf{R}}}_{j-1,j}^T\sum _{k=i}^{j-2}\Delta {\tilde{\mathbf{R}}}_{k+1,j-1}^T{\mathbf{J}}_{r,k}{\mathbf{\eta }}_{gd,k}\Delta t+{\mathbf{J}}_{r,j-1}{\mathbf{\eta }}_{gd,j-1}\Delta t\\ & =\Delta {\tilde{\mathbf{R}}}_{j-1,j}^T\delta {\mathbf{\varphi }}_{i,j-1}+{\mathbf{J}}_{r,j-1}{\mathbf{\eta }}_{gd,j-1}\Delta t\end{array}\text{\hspace{1em}(12)}$$

3.2、预积分速度误差传播

若$j>i$，则可将预积分速度误差表达式$(8)$写成递推形式
$$\begin{array}{rl}\delta {\mathbf{v}}_{ij}& =\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ij}{\mathbf{\eta }}_{ad,k}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ij}\Delta t\right]\\ & =\sum _{k=i}^{j-2}\left[\Delta {\tilde{\mathbf{R}}}_{ij}{\mathbf{\eta }}_{ad,k}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ij}\Delta t\right]+\Delta {\tilde{\mathbf{R}}}_{i,j-1}{\mathbf{\eta }}_{ad,j-1}\Delta t-\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{i,j-1}\Delta t\\ & =\delta {\mathbf{v}}_{i,j-1}+\Delta {\tilde{\mathbf{R}}}_{i,j-1}{\mathbf{\eta }}_{ad,j-1}\Delta t-\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{i,j-1}\Delta t\end{array}\text{\hspace{1em}(13)}$$
3.3、预积分位移误差传播

若$j>i$，则可将预积分位移误差表达式$(1i_0)$写成递推形式
$$\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}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t^2\right]\\ & =\sum _{k=i}^{j-2}\left[\delta {\mathbf{v}}_{ik}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{ik}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\mathbf{\eta }}_{ad,k}\Delta t^2\right]+\delta {\mathbf{v}}_{i,j-1}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{i,j-1}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}{\mathbf{\eta }}_{ad,j-1}\Delta t^2\\ & =\delta {\mathbf{p}}_{i,j-1}+\delta {\mathbf{v}}_{i,j-1}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\delta {\mathbf{\varphi }}_{i,j-1}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}{\mathbf{\eta }}_{ad,j-1}\Delta t^2\end{array}\text{\hspace{1em}(14)}$$
3.4、预积分误差传播

定义IMU测量噪声为${\mathbf{\eta }}_{d,k}=[{\mathbf{\eta }}_{gd,k}^T,{\mathbf{\eta }}_{ad,k}^T{]}^T$

若$j>i$，则可根据$(12),(13),(14)$将预积分误差写成递推形式
$$\delta (\Delta {\mathbf{x}}_{ij})={\mathbf{A}}_{j-1}\delta (\Delta {\mathbf{x}}_{i,j-1})+{\mathbf{B}}_{j-1}{\mathbf{\eta }}_{d,j-1}\text{\hspace{1em}(15)}$$
其中系数矩阵为
$${\mathbf{A}}_{j-1}=\left[\begin{array}{ccc}\Delta {\tilde{\mathbf{R}}}_{j-1,j}^T& \mathbf{O}& \mathbf{O}\\ -\Delta {\tilde{\mathbf{R}}}_{ij}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\Delta t& \mathbf{I}& \mathbf{O}\\ -\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\Delta t^2& \Delta t\mathbf{I}& \mathbf{O}\end{array}\right]{\mathbf{B}}_{j-1}=\left[\begin{array}{cc}{\mathbf{J}}_{r,j-1}\Delta t& \mathbf{O}\\ \mathbf{O}& \Delta {\tilde{\mathbf{R}}}_{i,j-1}\Delta t\\ \mathbf{O}& \frac{1}{2}{\tilde{\mathbf{R}}}_{i,j-1}\Delta t^2\end{array}\right]$$
记$\mathbf{\Sigma }({\mathbf{\eta }}_{d,k})\in {\mathbb{R}}^{6\times 6}$是${\mathbf{\eta }}_{d,k}$的协方差矩阵，该矩阵在不同时刻恒定，且已给定，又记为$\mathbf{\Sigma }({\mathbf{\eta }}_d)$。
$$\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ij}))={\mathbf{A}}_{j-1}\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{i,j-1})){\mathbf{A}}_{j-1}^T+{\mathbf{B}}_{j-1}\mathbf{\Sigma }({\mathbf{\eta }}_d){\mathbf{B}}_{j-1}^T\text{\hspace{1em}(16)}$$
因为$\delta (\Delta {\mathbf{x}}_{ii})=0$，而常量的协方差矩阵是零矩阵，因此初始值$\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ii}))=\mathbf{O}$

根据$(15)$可知，在递推过程中预积分误差均值保持为$0$，故$\delta (\Delta {\mathbf{x}}_{ij})\sim \mathcal{N}(0,\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ij}))$