IMU预积分公式汇总

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

IMU观测模型

Bω~WB(t)_{\mathrm{B}} \tilde{\omega}_{\mathrm{WB}}(t)Bω~WB(t)是陀螺仪观测值，Ba~(t)_{\mathbf{B}} \tilde{\mathbf{a}}(t)Ba~(t)是加速度计观测值，wg_\mathrm{w} \mathrm{g}wg是重力加速度，bg(t)\mathbf{b}^{g}(t)bg(t)和ba(t)\mathbf{b}^{a}(t)ba(t)分别是陀螺仪和加速度计的零偏，ηg(t)\boldsymbol{\eta}^{g}(t)ηg(t)和ηg(t)\boldsymbol{\eta}^{g}(t)ηg(t)分别是陀螺仪和加速度计的观测噪声。

IMU预积分模型

连续时间下：

离散时间下：

带入IMU观测模型后：

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

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

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

分离出状态噪声：

状态噪声向量为：[δϕij⊤,δvij⊤,δpij⊤]⊤[\delta \phi_{i j}^{\top}, \delta \mathbf{v}_{i j}^{\top}, \delta \mathbf{p}_{i j}^{\top} ]^{\top}[δϕij⊤,δvij⊤,δpij⊤]⊤

计算协方差阵

迭代求解状态噪声：

通过噪声向量构建协方差阵：
ηikΔ≐[δϕik,δvik,δpik]ηkd≐[ηkgdηkad]\boldsymbol{\eta}_{i k}^{\Delta} \doteq\left[\delta \boldsymbol{\phi}_{i k}, \delta \mathbf{v}_{i k}, \delta \mathbf{p}_{i k}\right]\\ \boldsymbol{\eta}_{k}^{d} \doteq \left[ \begin{array}{ll}{\boldsymbol{\eta}_{k}^{g d}} & {\boldsymbol{\eta}_{k}^{a d}}\end{array}\right] ηikΔ≐[δϕik,δvik,δpik]ηkd≐[ηkgdηkad]
其中，ηikΔ\boldsymbol{\eta}_{i k}^{\Delta}ηikΔ为状态噪声，ηkd\boldsymbol{\eta}_{k}^{d}ηkd为IMU观测噪声

将式(59)-(61)写为矩阵形式：
ηijΔ=Aj−1ηij−1Δ+Bj−1ηj−1d\boldsymbol{\eta}_{i j}^{\Delta}=\mathbf{A}_{j-1} \boldsymbol{\eta}_{i j-1}^{\Delta}+\mathbf{B}_{j-1} \boldsymbol{\eta}_{j-1}^{d} ηijΔ=Aj−1ηij−1Δ+Bj−1ηj−1d
则协方差阵为：
Σij=Aj−1Σij−1Aj−1⊤+Bj−1ΣηBj−1⊤\boldsymbol{\Sigma}_{i j}=\mathbf{A}_{j-1} \boldsymbol{\Sigma}_{i j-1} \mathbf{A}_{j-1}^{\top}+\mathbf{B}_{j-1} \boldsymbol{\Sigma}_{\eta} \mathbf{B}_{j-1}^{\top} Σij=Aj−1Σij−1Aj−1⊤+Bj−1ΣηBj−1⊤

融合零偏更新

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

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

其中，

计算残差雅可比

相关参数：δϕi,δpi,δvi,δϕj,δpj,δvj,δ~big,δ~bia\delta \boldsymbol{\phi}_{i}, \delta \mathbf{p}_{i}, \delta \mathbf{v}_{i}, \delta \boldsymbol{\phi}_{j}, \delta \mathbf{p}_{j}, \delta \mathbf{v}_{j}, \tilde{\delta} \mathbf{b}_{i}^{g}, \tilde{\delta} \mathbf{b}_{i}^{a}δϕi,δpi,δvi,δϕj,δpj,δvj,δ~big,δ~bia

位置残差雅可比：

速度残差雅可比：

姿态残差雅可比：