《预积分总结与公式推导》学习笔记2

预积分噪声传播

上一篇博客的最后介绍了IMU预积分可以分解为预积分测量值减去预积分噪声值的形式，但是没有给出预积分噪声的传播方式（递推关系）。本节从这一点出发，首先定义预积分噪声为：
$${\mathbf{\eta }}_{ij}^{\Delta }\triangleq {\left[\begin{array}{ccc}\delta {\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)$。下面分别对三个噪声项进行分析。

旋转噪声递推式

由于噪声属于小量，所以指数映射的乘积等于指数求和后的指数映射，即$\mathrm{Exp}(\delta x_1)\mathrm{Exp}(\delta x_2)\cdots \mathrm{Exp}(\delta x_n)=\mathrm{Exp}(\delta x_1+\delta x_1+\cdots +\delta x_n)$，所以
$$\delta {\varphi }_{ij}=\sum _{k=i}^{j-1}\left(\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g\right)\Delta t\right)\cdot {\mathbf{\eta }}_k^{gd}\Delta t\right)$$ $\delta {\varphi }_{ij-1}\to \delta {\varphi }_{ij}$：
$$\delta {\varphi }_{ij}=\Delta {\tilde{\mathbf{R}}}_{jj-1}\delta {\varphi }_{ij-1}+{\mathbf{J}}_r\left({\tilde{\mathbf{\omega }}}_{j-1}-{\mathbf{b}}_{j-1}^g\right){\eta }_{j-1}^{gd}\Delta t$$

速度噪声递推式

$\delta {\mathbf{v}}_{ij-1}\to \delta {\mathbf{v}}_{ij}$：
$$\delta {\mathbf{v}}_{ij}=\delta {\mathbf{v}}_{ij-1}+\Delta {\tilde{\mathbf{R}}}_{ij-1}{\eta }_{j-1}^{ad}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{f}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\cdot \delta {\varphi }_{ij-1}\cdot \Delta t$$

位移噪声递推式

$\delta {\mathbf{p}}_{ij-1}\to \delta {\mathbf{p}}_{ij}$：
$$\delta {\mathbf{p}}_{ij}=\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{f}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\delta {\varphi }_{ij-1}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}{\eta }_{j-1}^{ad}\Delta t^2$$

预积分噪声递推式

将上述三部分携程矩阵和向量的乘积形式：
$${\eta }_{ij}^{\Delta }={\mathbf{A}}_{j-1}{\eta }_{ij-1}^{\Delta }+{\mathbf{B}}_{j-1}{\mathbf{\eta }}_{j-1}^d$$其中转移矩阵
$$\begin{gathered} {\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{f}}}_{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{f}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t^2& \Delta t\mathbf{l}& \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{gathered}$$只和$[i,j-1]$时间区间内的IMU测量，偏置有关。${\mathbf{\eta }}_{j-1}^d={\left[{\left({\mathbf{\eta }}_{j-1}^{gd}\right)}^T{\left({\eta }_{j-1}^{ad}\right)}^T\right]}^T$表示$j-1$时刻IMU的噪声向量，包括IMU角速度和加速度噪声。

从而，噪声的协方差矩阵递推公式如下：
$${\mathbf{\Sigma }}_{ij}={\mathbf{A}}_{j-1}{\mathbf{\Sigma }}_{ij-1}{\mathbf{A}}_{j-1}^T+{\mathbf{B}}_{j-1}{\mathbf{\Sigma }}_{\eta }{\mathbf{B}}_{j-1}^T$$

IMU偏置的微量更新对预积分测量值的影响

上述预积分计算中，假设偏置在积分区间内是恒定的。但是当偏置整体变化时（区间内偏置还是恒定不变，只是这个恒定值发生变化），如果重新计算预积分的话，将会十分耗时。因此进行对偏置更新的一阶线性化近似。

一阶线性化

假设偏置更新过程为
$$\begin{gathered} {\hat{\mathbf{b}}}_i^g\leftarrow {\overline{\mathbf{b}}}_i^g+\delta {\mathbf{b}}_i^g \\ {\hat{\mathbf{b}}}_i^a\leftarrow {\overline{\mathbf{b}}}_i^a+\delta {\mathbf{b}}_i^a \end{gathered}$$
偏置更新后，预积分测量值通过对偏置的一阶线性化进行更新：
$$\begin{gathered} \Delta {\tilde{\mathbf{R}}}_{ij}\left({\hat{\mathbf{b}}}_i^g\right)\approx \Delta {\tilde{\mathbf{R}}}_{ij}\left({\overline{\mathbf{b}}}_i^g\right)\cdot \mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}\left({\overline{\mathbf{b}}}_i^g\right)}{\partial {\overline{\mathbf{b}}}^g}\delta {\mathbf{b}}_i^g\right) \\ \Delta {\tilde{\mathbf{v}}}_{ij}\left({\hat{\mathbf{b}}}_i^g,{\hat{\mathbf{b}}}_i^a\right)\approx \Delta {\tilde{\mathbf{v}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)}{\partial {\overline{\mathbf{b}}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)}{\partial {\overline{\mathbf{b}}}^a}\delta {\mathbf{b}}_i^a \\ \Delta {\tilde{\mathbf{p}}}_{ij}\left({\hat{\mathbf{b}}}_i^g,{\hat{\mathbf{b}}}_i^a\right)\approx \Delta {\tilde{\mathbf{p}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)}{\partial {\overline{\mathbf{b}}}^g}\delta {\mathbf{b}}_i^g+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right)}{\partial {\overline{\mathbf{b}}}^a}\delta {\mathbf{b}}_i^a \end{gathered}$$

预积分测量值对偏置的雅克比

为了方便书写，简记
$$\begin{gathered} \Delta {\hat{\mathbf{R}}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\left({\hat{\mathbf{b}}}_i^g\right)\Delta {\overline{\mathbf{R}}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\left({\overline{\mathbf{b}}}_i^g\right) \\ \Delta {\hat{\mathbf{v}}}_{ij}\triangleq \Delta {\tilde{\mathbf{v}}}_{ij}\left({\hat{\mathbf{b}}}_i^g,{\hat{\mathbf{b}}}_i^a\right)\Delta {\overline{\mathbf{v}}}_{ij}\triangleq \Delta {\tilde{\mathbf{v}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right) \\ \Delta {\hat{\mathbf{p}}}_{ij}\doteq \Delta {\tilde{\mathbf{p}}}_{ij}\left({\hat{\mathbf{b}}}_i^g,{\hat{\mathbf{b}}}_i^a\right)\Delta {\overline{\mathbf{p}}}_{ij}\doteq \Delta {\tilde{\mathbf{p}}}_{ij}\left({\overline{\mathbf{b}}}_i^g,{\overline{\mathbf{b}}}_i^a\right) \end{gathered}$$

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

预积分旋转测量对偏置的雅克比

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

预积分速度测量对偏置的雅克比

$$\begin{gathered} \frac{\partial \Delta {\overline{\mathbf{v}}}_{ij}}{\partial {\overline{\mathbf{b}}}^g}=-\sum _{k=i}^{j-1}\left(\Delta {\overline{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{f}}}_k-{\overline{\mathbf{b}}}_i^a\right)}^{\wedge }\frac{\partial \Delta {\overline{\mathbf{R}}}_{ik}}{\partial {\overline{\mathbf{b}}}^g}\Delta t\right) \\ \frac{\partial \Delta {\overline{\mathbf{v}}}_{ij}}{\partial {\overline{\mathbf{b}}}^a}=-\sum _{k=i}^{j-1}\left(\Delta {\overline{\mathbf{R}}}_{ik}\Delta t\right) \end{gathered}$$

预积分位置测量对偏置的雅克比

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

预积分残差与雅克比

预积分残差定义

残差定义为理想值+偏置更新与测量值的差异：
$$\begin{gathered} {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}\triangleq \log \left[{\left(\Delta {\hat{\mathbf{R}}}_{ij}\right)}^T\Delta {\mathbf{R}}_{ij}\right] \\ {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}\triangleq \Delta {\mathbf{v}}_{ij}-\Delta {\hat{\mathbf{v}}}_{ij} \\ {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}\triangleq \Delta {\mathbf{p}}_{ij}-\Delta {\hat{\mathbf{p}}}_{ij} \end{gathered}$$

预积分残差对系统状态的雅克比

系统状态定义为$i$和$j$时刻的旋转，位置和速度，以及区间内的偏置更新：${\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$。根据残差定义，可以得到各残差对上述8项系统状态的雅克比矩阵，此处略去推导及结果展示，具体参考文献[1]。

参考文献

[1] 邱笑晨，预积分总结与公式推导，2018