IMU预积分公式汇总

本文中全部公式来自论文：On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
为了查看公式时更加方便，现把文中的有关IMU预积分的公式汇总如下：

IMU观测模型

${}_{\mathrm{B}}{\tilde{\omega }}_{\mathrm{W}\mathrm{B}}(t)$是陀螺仪观测值，${}_{\mathbf{B}}\tilde{\mathbf{a}}(t)$是加速度计观测值，${}_{\mathrm{w}}\mathrm{g}$是重力加速度，${\mathbf{b}}^g(t)$和${\mathbf{b}}^a(t)$分别是陀螺仪和加速度计的零偏，${\mathbf{\eta }}^g(t)$和${\mathbf{\eta }}^g(t)$分别是陀螺仪和加速度计的观测噪声。

IMU预积分模型

连续时间下：

离散时间下：

带入IMU观测模型后：

考虑两个关键帧之间的IMU预积分：

将上式转换为两帧之间的相对运动关系（包括相对旋转、速度和位置）：

假设两个关键帧之间的陀螺仪和加速度计的零偏保持恒定：

分离出状态噪声：

状态噪声向量为：$[\delta {\varphi }_{ij}^{\top },\delta {\mathbf{v}}_{ij}^{\top },\delta {\mathbf{p}}_{ij}^{\top }{]}^{\top }$

计算协方差阵

迭代求解状态噪声：



通过噪声向量构建协方差阵：
$$\begin{gathered} {\mathbf{\eta }}_{ik}^{\Delta }\doteq \left[\delta {\mathbf{\varphi }}_{ik},\delta {\mathbf{v}}_{ik},\delta {\mathbf{p}}_{ik}\right] \\ {\mathbf{\eta }}_k^d\doteq \left[\begin{array}{ll}{\mathbf{\eta }}_k^{gd}& {\mathbf{\eta }}_k^{ad}\end{array}\right] \end{gathered}$$
其中，${\mathbf{\eta }}_{ik}^{\Delta }$为状态噪声，${\mathbf{\eta }}_k^d$为IMU观测噪声

将式(59)-(61)写为矩阵形式：
$${\mathbf{\eta }}_{ij}^{\Delta }={\mathbf{A}}_{j-1}{\mathbf{\eta }}_{ij-1}^{\Delta }+{\mathbf{B}}_{j-1}{\mathbf{\eta }}_{j-1}^d$$
则协方差阵为：
$${\mathbf{\Sigma }}_{ij}={\mathbf{A}}_{j-1}{\mathbf{\Sigma }}_{ij-1}{\mathbf{A}}_{j-1}^{\top }+{\mathbf{B}}_{j-1}{\mathbf{\Sigma }}_{\eta }{\mathbf{B}}_{j-1}^{\top }$$

融合零偏更新

实际上，两个关键帧之间的陀螺仪和加速度计的零偏已经发生了变化$\delta \mathrm{b}$，在$j$时刻的零偏应该等于$\mathrm{b}=\overline{\mathrm{b}}+\delta \mathrm{b}$

所以，融合零偏更新后的相对运动关系为：

其中，

计算残差雅可比

相关参数：$\delta {\mathbf{\varphi }}_i,\delta {\mathbf{p}}_i,\delta {\mathbf{v}}_i,\delta {\mathbf{\varphi }}_j,\delta {\mathbf{p}}_j,\delta {\mathbf{v}}_j,\tilde{\delta }{\mathbf{b}}_i^g,\tilde{\delta }{\mathbf{b}}_i^a$

位置残差雅可比：

速度残差雅可比：

姿态残差雅可比：