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

预积分噪声传播

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

旋转噪声递推式

由于噪声属于小量，所以指数映射的乘积等于指数求和后的指数映射，即$\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 {\vec{\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 {\vec{\varphi }}_{ij-1}\to \delta {\vec{\varphi }}_{ij}$：
$$\delta {\vec{\varphi }}_{ij}=\Delta {\tilde{\mathbf{R}}}_{jj-1}\delta {\vec{\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 {\vec{\varphi }}_{ij-1}\cdot \Delta t$$

位移噪声递推式

$\delta {\mathbf{p}}_{ij-1}\to \delta {\mathbf{p}}_{ij}$：
δ p i j = δ p i j − 1 + δ v i j − 1 Δ