IMU Preintegration

0. 为什么需要 IMU 预积分

要解释 IMU 预积分的话，首先需要先了解正常情况下 IMU 积分与所谓的 IMU 预积分的区别。这里直接给出 IMU 积分过程（其中涉及到的 IMU 测量模型与运动学模型先不展开）。IMU 积分是指根据 IMU 传感器获取得到的加速度 $a_t$ 和角速度 $w_t$ ，与给定 IMU bias $b _{a_t}, b _{g_t}$ ，从第 $i$ 时刻的 $p _{b_i}^w, v _{b_i}^w, q _{b_i}^w$ 基础上不断积分得到第 $j$ 时刻的 $p _{b_j}^w, v _{b_j}^w, q _{b_j}^w$ 。如下图所示，

IMU 积分公式如下： $$ \begin{align*} p _{b_j}^w &= p _{b_i}^w + v_i^w \Delta t_k + \iint _{t\in[i, j]} (q _{b_t}^w (a_t - b _{a_t}-n_a)-g^w)dt^2 \newline v _{b_j}^w &= v _{b_i}^w + \int _{t\in[i, j]} (q _{b_t}^w (a_t - b _{a_t}-n_a)-g^w)dt \newline q _{b_j}^w &= \int _{t\in[i, j]} q _{b_t}^w \otimes \begin{bmatrix} 0 \newline \frac{1}{2} (w_t - b _{g_t} - n_g) \end{bmatrix} dt\newline \end{align*} \tag{0.1}\label{0.1} $$ 观察积分公式我们发现 $p _{b_j}^w, v _{b_j}^w, q _{b_j}^w$ 中的积分项依赖于初值 $p^w _{b_i},v^w _{b_i},q _{b_i}^w$ 。 如果时刻 $b_i$ 的状态发生改变，时刻 $b_j$ 的状态必须重新计算。在基于优化的 Visual Inertial Odometry 算法中，状态每次迭代都会改变，如果每次迭代都重新积分，那么计算量会非常大。而预积分的目的就是想将这部分预先计算，且不受每次迭代更新重新计算，由此，我们需要想方法去掉基于 World 坐标系下的依赖项 $p^w _{b_i},v^w _{b_i},q _{b_i}^w$ 。

IMU 预积分技术最早由 T Lupton 于 12 年提出¹，以欧拉角为基础。C Forster 于 15 年²³⁴将其进一步拓展到李代数上，形成了一套优雅的理论体系。这套 IMU 预积分理论在开源因子图优化库 GTSAM 中进行实现，且被广泛应用于图优化框架的 VIO 中。其中包括 SVO2、ORB-SLAM3、VINS-Mono/Fusion、ICE-BA 等。由于这套理论的概念是在李群流行上的推导的，因此其中涉及到许多李群李代方面的知识，这部分知识可参考⁵⁶⁷（因此下文中使用到的一些公式、定义将直接给出，不再加以说明）。这套理论的后续发展有，Guoquan Huang 等⁸⁹¹⁰，在欧拉积分/二阶龙格库塔积分（Runge-Kutta 即，中值积分）基础上引入群空间闭式 (closed-form) 积分。

1. IMU 器件测量模型与运动学模型

IMU 通常包括一个三轴加速度计和一个三轴陀螺仪，能够测量传感器相对于惯性系的旋转速率和加速度。测量模型定义如下： $$ \begin{align*} {}^b\tilde{w}(t) &= {}^bw(t) + b_g(t) + \eta_g(t) \newline {}^b\tilde{a}(t) &= {}^w_bR^T ({}^wa(t) - {}^wg ) + b_a(t) + \eta_a(t) \end{align*} \tag{1.1}\label{1.1} $$ 其中 ${}^b\tilde{w}(t),{}^b\tilde{a}(t)$ 分别为陀螺仪与加速度计的测量值，$b$ 为随时间缓慢变化的 bias，$\eta$ 是白噪声。该模型利用了 Static World Assumption，由于 SLAM 的运行场景通常比较小（由此重力变化不大），且使用的 IMU 器件一般为 MEMS 器件（精度较低，陀螺仪静置时无法敏感到地球自转）。因此在上式 $\eqref{1.1}$ 测量模型中忽略了地球自转、重力矢量变化等影响因素，且假设运行区域水平面是一个平面。在此假设下的 VINS 中，世界坐标系 $w$ 被认为是一个惯性系，由此，下面的运动模型都是基于该假设进行推导。因此有运动模型的微分方程形式如下： $$ {}^w_b\dot{R} = {}^w_bR \ {}^bw^\wedge , \quad {}^w\dot{v} = {}^wa, \quad {}^w\dot{p} = {}^wv \tag{1.2}\label{1.2} $$ 根据上式，$t+\Delta t$ 时刻的状态可以通过积分得： $$ \begin{align*} {}^w_bR(t+\Delta t) &= {}^w_bR(t) \ \mathrm{Exp}\left(\int_t^{t+\Delta t} {}^bw(\tau) d\tau \right) \newline {}^wv(t+\Delta t) &= {}^wv(t) + \int_t^{t+\Delta t} {}^wa(\tau) d\tau \newline {}^wp(t+\Delta t) &= {}^wp(t) + \int_t^{t+\Delta t} {}^wv(\tau) d\tau + \iint_t^{t+\Delta t} {}^wa(\tau) d\tau \end{align*} \tag{1.3}\label{1.3} $$ 当假设在时间 $[t, t+\Delta t]$ 内 ${}^wa$ 和 ${}^bw$ 是恒定的，使用欧拉积分，可以得到上式的离散形式如下： $$ \begin{align*} {}^w_bR(t+\Delta t) &= {}^w_bR(t) \ \mathrm{Exp} ({}^bw(t) \Delta t ) \newline {}^wv(t+\Delta t) &= {}^wv(t) + {}^wa(t) \Delta t \newline {}^wp(t+\Delta t) &= {}^wp(t) + {}^wv(t) \Delta t + \frac{1}{2} {}^wa(t) \Delta t^2 \end{align*} \tag{1.4}\label{1.4} $$ 为了公式书写方便与符号简明，下面省略一些上下标记号，约定如下 $R \doteq {}^w_bR, w \doteq {}^bw, a \doteq {}^ba, v \doteq {}^wv, p \doteq {}^wp, g\doteq {}^wg$ 。将测量模型（式 $\eqref{1.1}$ ）带入离散运动模式（式 $\eqref{1.4}$ ）易得： $$ \begin{align*} R(t+\Delta t) &= R(t) \ \mathrm{Exp} \Big(\big(\tilde{w}(t) - b_g(t) - \eta _{gd}(t)\big) \Delta t \Big) \newline v(t+\Delta t) &= v(t) + R(t)\big(\tilde{a}(t) - b_a(t) - \eta _{ad}(t)\big) \Delta t + g \Delta t \newline p(t+\Delta t) &= p(t) + v(t)\Delta t + \frac{1}{2}g \Delta t^2 + \frac{1}{2}R(t)\big(\tilde{a}(t) - b_a(t) - \eta _{ad}(t)\big) \Delta t^2 \end{align*} \tag{1.5}\label{1.5} $$ 上述公式中，我们注意到噪声采用的 $\eta _{gd},\eta _{ad}$ ，其中下标 $d$ 表示为 discrete ，即使用了离散时间噪声，它们与连续噪声项 $\eta _{g},\eta _{a}$ 是不同的，离散噪声与连续噪声的协方差有如下关系（Nikolas Trawny and Stergios I. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation 2.1 有详细推导过程）： $$ \begin{align*} \mathrm{Cov}\big(\eta _{gd}(t) \big) &= \frac{1}{\Delta t} \mathrm{Cov}\big(\eta_g(t)\big) \newline \mathrm{Cov}\big(\eta _{ad}(t) \big) &= \frac{1}{\Delta t} \mathrm{Cov}\big(\eta_a(t)\big) \end{align*}\tag{1.6}\label{1.6} $$

2. IMU 预积分

下面我们推导如何将两个关键帧 $k = i$ 和 $k = j$ 之间的所有 IMU 测量数据进行积分，由 $k = i$ 时刻的 $R_i, v_i, p_i$ 直接更新得到 $k = j$ 时刻的 $R_j, v_j, p_j$ 。过程如下图：

假设 IMU 的采样频率不变，即时间间隔 $\Delta t$ 恒定，每个离散时刻由 $k = 0,1,2,\cdots$ 表示，由此将式 $\eqref{1.5}$ 简化符号如下：

$$ \begin{align*} R _{k+1} &= R_k \ \mathrm{Exp} \Big(\big(\tilde{w} _k - b _{g_k} - \eta _{{gd} _k}\big) \Delta t \Big) \newline v _{k+1} &= v_k + R_k\big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t + g \Delta t \newline p _{k+1} &= p_k + v_k\Delta t + \frac{1}{2}g \Delta t^2 + \frac{1}{2}R_k \big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t^2 \end{align*} \tag{2.1}\label{2.1} $$

在关键帧 $k = i$ 和 $k = j$ 之间迭代所有 $\Delta t$ 时间间隔的 IMU 积分有：

$$ \begin{align*} R_j &= R_i \prod _{k=i}^{j-1} \mathrm{Exp} \Big(\big(\tilde{w} _k - b _{g_k} - \eta _{{gd} _k}\big) \Delta t \Big) \newline v_j &= v_i + g \Delta t _{ij} + \sum _{k=i}^{j-1} R_k\big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t \newline p_j &= p_i + \sum _{k=i}^{j-1} v_k\Delta t + \frac{j-i}{2}g \Delta t^2 + \frac{1}{2} \sum _{k=i}^{j-1} R_k \big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t^2 \newline &= p_i + \sum _{k=i}^{j-1} \left( v_k\Delta t + \frac{1}{2}g \Delta t^2 + \frac{1}{2} R_k \big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t^2 \right) \newline \text{其中,} \ \ \Delta t _{ij} &= \sum _{k=i}^{j-1} \Delta t = (j-i)\Delta t \end{align*} \tag{2.2}\label{2.2} $$

为了避免每次更新初始的 $R_i$ ，$v_i$ 和 $p_i$ 都需要从头积分求解 $R_j$ ，$v_j$ 和 $p_j$ 。由此引出相对运动的预积分项（理想值）以避免依赖于 $R_i$ ，$v_i$ 和 $p_i$ ：

$$ \begin{align*} \Delta R _{ij} &\doteq R_i^T R_j = \prod _{k=i}^{j-1} \mathrm{Exp} \big((\tilde{w} _k - b _{g_k} - \eta _{gd_k}) \Delta t \big) \newline \Delta v _{ij} &\doteq R_i^T (v_j - v_i - g \Delta t _{ij}) = \sum _{k=i}^{j-1} \Delta R _{ik} \big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k} \big) \Delta t \newline \Delta p _{ij} &\doteq R_i^T (p_j - p_i - v_i \Delta t _{ij} - \frac{1}{2} g \Delta t _{ij}^2) = \sum _{k=i}^{j-1} \Big[ \Delta v _{ik} \Delta t + \frac{1}{2} \Delta R _{ik} (\tilde{a} _k - b _{a_k} - \eta _{{ad} _k}) \Delta t^2 \Big] \newline &\text{其中,} \ \Delta R _{ik} \doteq R_i^T R_k, \ \ \Delta v _{ik} \doteq R_i^T(v_k - v_i -g \Delta t _{ik}) \end{align*} \tag{2.3}\label{2.3} $$

需要注意的是虽然所称为 ‘‘delta’’ 量，但与 $\Delta R _{ij}$ 相比， $\Delta v _{ij}$ 和 $\Delta p _{ij}$ 都不对应于速度和位置的真实物理变化，而是以一种使式 $\eqref{2.3}$ 的右侧独立于时刻 $i$ 的状态量和重力效应的方式定义的。使得我们能从两个关键帧之间的惯性测量直接计算式 $\eqref{2.3}$ 的右侧。上述三个预积分公式中，前两个式子的推导是显而易见的。而对于 $\Delta p _{ij}$ 的推导如下（为了节省篇幅，令 $f_k = \tilde{a} _k - b _{a_k} - \eta _{{ad} _k}$），

$$ \begin{align*} p_j & - p_i - v_i \Delta t _{ij} - \frac{1}{2} g \Delta t _{ij}^2 \newline &= \sum _{k=i}^{j-1} \left( v_k\Delta t + \frac{1}{2}g \Delta t^2 + \frac{1}{2} R_k f_k \Delta t^2 \right) - \sum _{k=i}^{j-1} v_i \Delta t - \frac{(j-i)^2}{2} g \Delta t^2 \newline &= \sum _{k=i}^{j-1}(v_k - v_i)\Delta t + \underbrace{\Big[ \frac{j-i}{2} - \frac{(j-i)^2}{2} \Big]} _{\large \llap{\text{利用} \frac{j-i}{2} - \frac{(j-i)^2}{2} = - \frac{(j-i)[j-(i+1)]}{2} } \rlap{= \sum _{k=i}^{j-1} (k-i) \text{等差数列求和}}} g \Delta t^2 + \frac{1}{2} \sum _{k=i}^{j-1} R_k f_k \Delta t^2 \newline &= \sum _{k=i}^{j-1}(v_k - v_i)\Delta t + \sum _{k=i}^{j-1} (k-i) g \Delta t^2 + \frac{1}{2} \sum _{k=i}^{j-1} R_k f_k \Delta t^2 \newline &= \sum _{k=i}^{j-1} \left\lbrace \big[ v_k - v_i - (k-i)g\Delta t \big] \Delta t + \frac{1}{2} R_k f_k \Delta t^2 \right\rbrace \newline &= \sum _{k=i}^{j-1} \left[ \big( v_k - v_i - g\Delta t _{ik} \big) \Delta t + \frac{1}{2} R_k f_k \Delta t^2 \right] \newline &= \sum _{k=i}^{j-1} \left( R_i \Delta v _{ik} \Delta t + \frac{1}{2} R_k f_k \Delta t^2 \right) \newline \text{等}& \text{式左右两边同时左乘} R_i^T \newline R_i^T & \left( p_j - p_i - v_i \Delta t _{ij} - \frac{1}{2} g \Delta t _{ij}^2 \right) \newline &= \sum _{k=i}^{j-1} \left(R_i^T R_i \Delta v _{ik} \Delta t + \frac{1}{2}R_i^T R_k f_k \Delta t^2 \right) = \sum _{k=i}^{j-1} \left(\Delta v _{ik} \Delta t + \frac{1}{2}R _{ik} f_k \Delta t^2 \right) \newline &= \sum _{k=i}^{j-1} \left(\Delta v _{ik} \Delta t + \frac{1}{2}R _{ik} \big(\tilde{a} _k - b _{a_k} - \eta _{{ad} _k}\big) \Delta t^2 \right) \end{align*} \tag{2.3a}\label{2.3a} $$

我们注意到，即使采用式 $\eqref{2.3}$ 进行预积分计算，但是它依旧依赖于 bias 的估计。为了决绝这个问题，预积分算法将之分解与两个步骤：1. 首先假设两个关键帧之间的 bias 是一致的，即 $b _{g_i} = b _{g _{i+1}} = \cdots = b _{g _{j-1}},\quad b _{a_i} = b _{a _{i+1}} = \cdots = b _{a _{j-1}}$ 先进行预积分 IMU 测量值（[2.1 章节](#2.1 预积分 IMU 测量值)）；2. 当 bias 更新时，采用其他方式避免重复积分地更新预积分值（[2.3 章节](#2.3 伴随 bias 迭代更新的预积分测量更新)）。

2.1 预积分 IMU 测量值

式 $\eqref{2.3}$ 将关键帧 $i$ 和 $j$ 的状态（等式左侧）与测量值（等式右侧）联系起来。在这个意义上，可以被当初一个测量模型。不幸的是，它对测量噪声的依赖相当复杂，这使得 MAP 估计的直接应用也变得复杂。为了使得求解容易，需要将噪声项从惯性测量量中进行分离，使得预积分项变成 “$测量值 = 理想值 \oplus 白噪声$” 的形式。

2.1.1 $\Delta R _{ij}$ 项

$$ \begin{align*} \Delta R _{ij} &= \prod _{k=i}^{j-1} \mathrm{Exp} \big((\tilde{w} _k - b _{g_i} - \eta _{gd_k}) \Delta t \big) \newline \small{\text{利用 Exp}}& \small{(\phi+\delta\phi) \approx \mathrm{Exp}(\phi)\mathrm{Exp}(J_r(\phi)\delta\phi),\ \ \text{且令}\ J_r^k \doteq J_r((\tilde{w} _k - b _{g_i})\Delta t)} \newline &\simeq \prod _{k=i}^{j-1} \mathrm{Exp} \big((\tilde{w} _k - b _{g_i}) \Delta t \big) \mathrm{Exp} \big(-J_r^k \eta _{gd_i} \Delta t \big) \newline \small{\text{利用 Exp}}& \small{(\phi)\ R = R \ \mathrm{Exp}(R^T\phi)\ \text{将噪声项移至最后}} \newline &\simeq \Delta \tilde{R} _{ij} \prod _{k=i}^{j-1} \mathrm{Exp}(-\Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t) \newline & \doteq \Delta \tilde{R} _{ij} \mathrm{Exp}(-\delta \phi _{ij}) \end{align*} \tag{2.4}\label{2.4} $$

其中 $\Delta \tilde{R} _{ij} \doteq \prod _{k=i}^{j-1} \mathrm{Exp} \big((\tilde{w} _k - b _{g_i}) \Delta t \big)$ 即为预积分旋转测量值，它由陀螺仪测量值和对陀螺仪 bias 的估计计算得到。而 $\delta \phi _{ij}$ 为其测量噪声。

2.1.2 $\Delta v _{ij}$ 项

将式 $\eqref{2.4}$ ，即 $\Delta R _{ij}\doteq\Delta \tilde{R} _{ij} \mathrm{Exp}(-\delta \phi _{ij})$ 代入 $\Delta v _{ij}$ 的公式中，有

$$ \begin{align*} \Delta v _{ij} &= \sum _{k=i}^{j-1} \Delta R _{ik} \big(\tilde{a} _k - b _{a_i} - \eta _{{ad} _k} \big) \Delta t \newline &\simeq \sum _{k=i}^{j-1} \Delta \tilde{R} _{ik} \mathrm{Exp}(-\delta \phi _{ik}) \big(\tilde{a} _k - b _{a_i} - \eta _{{ad} _k} \big) \Delta t \newline {\small 利用 \exp(\delta\phi^\wedge) \approx I+\delta\phi^\wedge} \ &\simeq \sum _{k=i}^{j-1} \Delta \tilde{R} _{ik} (I-\delta\phi^\wedge _{ik}) \big(\tilde{a} _k - b _{a_i} - \eta _{{ad} _k} \big) \Delta t \newline {\small 忽略高阶小项\ \delta\phi^\wedge \eta _{ad_k}} \ &\simeq \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik} (I-\delta\phi^\wedge _{ik}) \big(\tilde{a} _k - b _{a_i} \big) \Delta t - \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline {\small 利用 \ \alpha^\wedge \beta = -\beta^\wedge \alpha} \ &\simeq \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big) \Delta t + \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t - \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline &= \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big) \Delta t \Big] + \sum _{k=i}^{j-1} \Big[\Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t - \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline &\doteq \Delta \tilde{v} _{ij} - \delta v _{ij} \end{align*} \tag{2.5}\label{2.5} $$

其中 $\Delta \tilde{v} _{ij} \doteq \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big) \Delta t \Big]$ 即为预积分速度测量值，它由 IMU 测量值和对 bias 的估计计算得到。而 $\delta v _{ij}$ 为其测量噪声。

2.1.3 $\Delta p _{ij}$ 项

将式 $\eqref{2.4}$ 和式 $\eqref{2.5}$ ，即 $\Delta R _{ij} \doteq \Delta \tilde{R} _{ij} \mathrm{Exp}(-\delta \phi _{ij})$ 和 $\Delta v _{ij} \doteq \Delta \tilde{v} _{ij} - \delta v _{ij}$ 代入 $\Delta p _{ij}$ 的公式中，有

$$ \begin{align*} \Delta p _{ij} &= \sum _{k=i}^{j-1} \Big[ \Delta v _{ik} \Delta t + \frac{1}{2} \Delta R _{ik} (\tilde{a} _k - b _{a_i} - \eta _{{ad} _k}) \Delta t^2 \Big] \newline &\simeq \sum _{k=i}^{j-1} \Big[ (\Delta \tilde{v} _{ik} - \delta v _{ik}) \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik} \mathrm{Exp}(-\delta \phi _{ik}) (\tilde{a} _k - b _{a_i} - \eta _{{ad} _k}) \Delta t^2 \Big] \newline &\quad\ {\small 利用 \exp(\delta\phi^\wedge) \approx I+\delta\phi^\wedge \ , 且忽略高阶小项\ \delta\phi^\wedge \eta _{ad_k}} \newline &\simeq \sum _{k=i}^{j-1} \Big[ (\Delta \tilde{v} _{ik} - \delta v _{ik}) \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik} (I-\delta\phi^\wedge _{ik}) \big(\tilde{a} _k - b _{a_i} \big) \Delta t^2 - \frac{1}{2} \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t^2 \Big] \newline &\quad\ {\small 利用 \ \alpha^\wedge \beta = -\beta^\wedge \alpha} \newline &\simeq \sum _{k=i}^{j-1} \Big[ \Delta \tilde{v} _{ik} \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big) \Delta t^2 \newline &\qquad\qquad- \delta v _{ik} \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t^2 - \frac{1}{2} \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t^2 \Big] \newline & \doteq \Delta \tilde{p} _{ij} - \delta p _{ij} \end{align*} \tag{2.6}\label{2.6} $$

其中 $\Delta \tilde{p} _{ij} = \sum _{k=i}^{j-1} \Big[ \Delta \tilde{v} _{ik} \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big) \Delta t^2 \Big]$ 即为预积分位置测量值，它由 IMU 测量值和对 bias 的估计计算得到。而 $\delta p _{ij}$ 为其测量噪声。

2.1.4 IMU 预积分测量模型

通过 2.1.1~2.1.3 我们有 $\Delta R _{ij} \doteq \Delta \tilde{R} _{ij} \mathrm{Exp}(-\delta \phi _{ij})$ 、 $\Delta v _{ij} \doteq \Delta \tilde{v} _{ij} - \delta v _{ij}$ 和 $\Delta p _{ij} \doteq \Delta \tilde{p} _{ij} - \delta p _{ij}$ 将之代入式 $\eqref{2.3}$ 既能获取 IMU 预积分测量模型，即

$$ \begin{align*} \Delta \tilde{R} _{ij} &= \Delta R _{ij} \mathrm{Exp}(-\delta \phi _{ij})^T = R_i^T R_j \mathrm{Exp}(\delta \phi _{ij}) \newline \Delta \tilde{v} _{ij} &= \Delta v _{ij} + \delta v _{ij} = R_i^T(v_j - v_i - g\Delta t _{ij}) + \delta v _{ij} \newline \Delta \tilde{p} _{ij} &= \Delta p _{ij} + \delta p _{ij} = R_i^T (p_j - p_i - v_i \Delta t _{ij} - \frac{1}{2} g \Delta t _{ij}^2) + \delta p _{ij} \end{align*} \tag{2.7}\label{2.7} $$

2.2 预积分项的噪声传递

根据式 $\eqref{2.7}$ ，将 IMU 预积分测量模型的噪声定义为 $[\delta \phi _{ij}^T,\delta v _{ij}^T,\delta p _{ij}^T]^T$ 。通常我们将噪声向量近似为零均的正态分布，但是准确地建模噪声的协方差是至关重要的。由于在优化过程用的是马氏距离，需要用到协方差的逆对优化各项进行加权。由此需要对 IMU 预积分测量模型的噪声进行分析（目的是给出协方差的计算表达式）。假设 IMU 预积分测量模型的噪声满足如下：

$$ \eta _{ij}^\Delta \doteq [\delta \phi _{ij}^T \quad \delta v _{ij}^T \quad \delta p _{ij}^T]^T \sim \mathcal{N}(0 _{9\times1},\Sigma _{ij}) \tag{2.8}\label{2.8} $$

为了证实该假设，首先我们来看 IMU 预积分测量旋转噪声 $\delta\phi _{ij}$ ，由式 $\eqref{2.4}$ 可得

$$ \begin{matrix} \mathrm{Exp}(-\delta\phi _{ij}) = \prod _{k=i}^{j-1} \mathrm{Exp}(-\Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t) \newline {\small 对两边去对数得:} \ \ \delta\phi _{ij} = -\mathrm{Log} \left[ \prod _{k=i}^{j-1} \mathrm{Exp}(-\Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t) \right] \newline {\small 令 \ x_k = \Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t \ , \ 由于\eta _{gd_k}是小量,于是有 J_r(x_k) \approx I,则有 J_r^{-1}(x_k) \approx I} \newline \begin{align*} \delta\phi _{ij} &= -\mathrm{Log}\Bigg[ \prod _{k=i}^{j-1} \mathrm{Exp}(-x_k) \Bigg] = -\mathrm{Log}\Bigg[ \mathrm{Exp}(-x_i) \prod _{k=i+1}^{j-1} \mathrm{Exp}(-x_k) \Bigg] \newline & \quad\ \ {\small 利用\ \mathrm{Log}(\mathrm{Exp(\phi)Exp(\delta\phi)})\approx \phi + J_r^{-1}\delta\phi} \newline & \approx - \left( -x_i + I \ \mathrm{Log}\Bigg[ \prod _{k=i+1}^{j-1} \mathrm{Exp}(-x_k) \Bigg] \right) \newline & = x_i - \mathrm{Log}\Bigg[ \prod _{k=i+1}^{j-1} \mathrm{Exp}(-x_k) \Bigg] \newline & = x_i - \mathrm{Log}\Bigg[ \mathrm{Exp}(-x _{i+1}) \prod _{k=i+2}^{j-1} \mathrm{Exp}(-x_k) \Bigg] \newline & \approx x_i + x _{i+1} - \mathrm{Log}\Bigg[ \prod _{k=i+2}^{j-1} \mathrm{Exp}(-x_k) \Bigg] \newline & \approx x_i + x _{i+1} + \cdots + x _{j-1} \newline & = \sum _{k=i}^{j-1} \Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t \end{align*} \end{matrix}\tag{2.9}\label{2.9} $$

由此易得 $\delta\phi _{ij}$ （一阶近似）满足零均高斯分布，因为它可以通过零均高斯噪声 $\eta _{gd_k}$ 的线性组合构成。

接下来我们来看 IMU 预积分测量速度噪声 $\delta v _{ij}$ 和测量位置噪声 $\delta p _{ij}$ ，由式 $\eqref{2.5}$ 和式 $\eqref{2.6}$ 易得：

$$ \begin{align*} \delta v _{ij} &= \sum _{k=i}^{j-1} \Big[- \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t + \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline \delta p _{ij} &= \sum _{k=i}^{j-1} \Big[ \delta v _{ij} \Delta t - \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t^2 + \frac{1}{2} \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t^2 \Big] \end{align*} \tag{2.10}\label{2.10} $$

通过上式易看出 $\delta v _{ij}$ 和 $\delta p _{ij}$ 由加速度噪声 $\eta _{ad_k}$ 和预积分测量旋转噪声 $\delta\phi _{ij}$ 线性组合而成，由于其两者都是零均的高斯噪声（ $\delta\phi _{ij}$ 在式 $\eqref{2.9}$ 已证明），所以 $\delta v _{ij}$ 和 $\delta p _{ij}$ 满足零均高斯分布。

通过式 $\eqref{2.9}$ 和式 $\eqref{2.10}$ 可知 IMU 预积分测量噪声 $\eta _{ij}^\Delta$ 可由 IMU 测量噪声 $\eta_k^d \doteq[\eta _{gd_k} ,\eta _{ad_k}],k=i,\cdots,j-1$ 线性组合构成。由于 IMU 测量噪声 $\eta_k^d$ 的协方差已知（可以通过标定或查阅数据手册），因此只需通过一系列线性组合就可以 IMU 预积分测量噪声 $\eta _{ij}^\Delta$ 的协方差 $\Sigma _{ij}$ 了。为了满足实时性避免重复计算，下面推导预积分测量噪声协方差的递推形式。

2.2.1 $\delta\phi _{ij}$ 项的递推形式

根据式 $\eqref{2.9}$ 有

$$ \begin{align*} \delta \phi _{ij} &\simeq \sum _{k=i}^{j-1} \Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t \newline &= \sum _{k=i}^{j-2} \Delta \tilde{R} _{k+1j}^T J_r^k \eta _{gd_k} \Delta t + \overbrace{\Delta \tilde{R} _{jj}^T}^{=I _{3\times3}} J_r^{j-1} \eta _{gd _{j-1}} \Delta t \newline &= \sum _{k=i}^{j-2} (\overbrace{\Delta\tilde{R} _{k+1j-1} \Delta\tilde{R} _{j-1j}}^{\Delta \tilde{R} _{k+1j}})^T J_r^k \eta _{gd_k} \Delta t + J_r^{j-1} \eta _{gd _{j-1}} \Delta t \newline &= \Delta\tilde{R} _{j-1j}^T \sum _{k=i}^{j-2} \Delta\tilde{R} _{k+1j-1}^T J_r^k \eta _{gd_k} \Delta t + J_r^{j-1} \eta _{gd _{j-1}} \Delta t \newline &= \Delta\tilde{R} _{j-1j}^T \delta\phi _{ij-1} + J_r^{j-1} \eta _{gd _{j-1}} \Delta t \newline \end{align*} \tag{2.11}\label{2.11} $$

2.2.2 $\delta v _{ij}$ 项的递推形式

根据式 $\eqref{2.10}$ 有

$$ \begin{align*} \delta v _{ij} &= \sum _{k=i}^{j-1} \Big[- \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t + \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline &= \sum _{k=i}^{j-2} \Big[- \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t + \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t \Big] \newline &\qquad\quad - \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \delta\phi _{ij-1} \Delta t + \Delta \tilde{R} _{ij-1} \eta _{ad _{j-1}} \Delta t \newline &= \delta v _{ij-1} - \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \delta\phi _{ij-1} \Delta t + \Delta \tilde{R} _{ij-1} \eta _{ad _{j-1}} \Delta t \end{align*} \tag{2.12}\label{2.12} $$

2.2.3 $\delta p _{ij}$ 项的递推形式

根据式 $\eqref{2.10}$ 有

$$ \begin{align*} \delta p _{ij} &= \sum _{k=i}^{j-1} \Big[ \delta v _{ij} \Delta t - \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t^2 + \frac{1}{2} \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t^2 \Big] \newline &= \sum _{k=i}^{j-2} \Big[ \delta v _{ij} \Delta t - \frac{1}{2} \Delta \tilde{R} _{ik} \big(\tilde{a} _k - b _{a_i} \big)^\wedge \delta\phi _{ik} \Delta t^2 + \frac{1}{2} \Delta \tilde{R} _{ik} \eta _{ad_k} \Delta t^2 \Big] \newline &\qquad\quad + \delta v _{ij-1} \Delta t - \frac{1}{2} \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \delta\phi _{ij-1} \Delta t^2 + \frac{1}{2} \Delta \tilde{R} _{i{j-1}} \eta _{ad _{j-1}} \Delta t^2 \newline &= \delta p _{ij-1} + \delta v _{ij-1} \Delta t - \frac{1}{2} \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \delta\phi _{ij-1} \Delta t^2 + \frac{1}{2} \Delta \tilde{R} _{i{j-1}} \eta _{ad _{j-1}} \Delta t^2 \end{align*} \tag{2.13}\label{2.13} $$

2.2.4 总结

整合式 $\eqref{2.11} \eqref{2.12} \eqref{2.13}$ ，可以得到 $\eta _{ij}^\Delta$ 的递推形式如下：

$$ \eta _{ij}^\Delta = \underbrace{\begin{bmatrix} \Delta\tilde{R} _{jj-1} & 0 & 0 \newline - \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \Delta t & I & 0 \newline - \frac{1}{2} \Delta \tilde{R} _{ij-1} \big(\tilde{a} _{j-1} - b _{a_i} \big)^\wedge \Delta t^2& \Delta t I & I \end{bmatrix}} _{\large A _{j-1}} \eta _{ij-1}^\Delta + \underbrace{\begin{bmatrix} J_r^{j-1}\Delta t & 0 \newline 0 & \Delta \tilde{R} _{ij-1} \Delta t \newline 0 & \frac{1}{2} \Delta \tilde{R} _{i{j-1}} \Delta t^2 \end{bmatrix}} _{\large B _{j-1}} \eta _{j-1}^d \tag{2.14}\label{2.14} $$

既有 $\eta _{ij}^\Delta = A _{j-1} \eta _{ij-1}^\Delta + B _{j-1} \eta _{j-1}^d$ ，由此根据协方差定义可得：

$$ \Sigma _{ij} = A _{j-1} \Sigma _{ij-1} A _{j-1}^T + B _{j-1} \Sigma _{\eta} B _{j-1}^T \tag{2.15}\label{2.15} $$

2.3 伴随 bias 迭代更新的预积分测量更新

在此之前的预积分都是假设了在两个关键帧 $k=i$ 和 $k=j$ 之间 IMU 的 bias $\lbrace\bar{b} _{a_k}, \bar{b} _{g_k}\rbrace$ 是恒定的基础上推导的。然而大多数情况下，IMU 的 bias 在优化过程中将伴随一个微小的增量 $\delta b$ 进行变化。如果当 bias 变化时重新进行预积分 IMU 测量值，这将耗费巨大的计算量。因此，为了决绝这个问题，提出了利用线性化来进行 bias 变化时预积分测量项的一阶近似更新方式（$\hat{b} \gets \bar{b} + \delta b$）：

$$ \begin{align*} \Delta \tilde{R} _{ij}(\hat{b} _{g_i}) &\simeq \Delta \tilde{R} _{ij}(\bar{b} _{g_i}) \mathrm{Exp} \left( \frac{\partial \Delta \bar{R} _{ij}}{\partial b_g} \delta b _{g_i} \right) \newline \Delta \tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) &\simeq \Delta \tilde{v} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i}) + \frac{\partial \Delta \bar{v} _{ij}}{\partial b_g} \delta b _{g_i} + \frac{\partial \Delta \bar{v} _{ij}}{\partial b_a} \delta b _{a_i} \newline \Delta \tilde{p} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) &\simeq \Delta \tilde{p} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i}) + \frac{\partial \Delta \bar{p} _{ij}}{\partial b_g} \delta b _{g_i} + \frac{\partial \Delta \bar{p} _{ij}}{\partial b_a} \delta b _{a_i} \end{align*} \tag{2.16}\label{2.16} $$

其中 $\bar{b}$ 为旧 bias (原始假定) ，$\hat{b}$ 为新 bias (迭代更新所得) ，而 $\delta b$ 是微小的增量。为了简化符号，假设已经在给定的偏差估计下的预积分变量为 $\Delta \bar{R} _{ij} \doteq \Delta \tilde{R} _{ij}(\bar{b} _{g_i}),\ \Delta \bar{v} _{ij} \doteq \Delta \tilde{v} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i}),\ \Delta \bar{p} _{ij} \doteq \Delta \tilde{p} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i})$ 。为了执行 $\eqref{2.16}$ 的更新，需要计算其中的雅克比项。

2.3.1 $\Delta \tilde{R} _{ij}$ 项关于 bias 的雅克比

根据式 $\eqref{2.4}$ 有：

$$ \begin{align*} \Delta \tilde{R} _{ij}(\hat{b} _{g_i}) &= \prod _{k=i}^{j-1} \mathrm{Exp} \big((\tilde{w} _k - \hat{b} _{g_i}) \Delta t \big) \newline &= \prod _{k=i}^{j-1} \mathrm{Exp} \Big(\big(\tilde{w} _k - (\bar{b} _{g_i} + \delta b _{g_i})\big) \Delta t \Big) \newline &= \prod _{k=i}^{j-1} \mathrm{Exp} \Big(\big( \tilde{w} _k - \bar{b} _{g_i} \big) \Delta t - \delta b _{g_i} \Delta t \Big) \newline \small{\text{利用 Exp}}& \small{(\phi+\delta\phi) \approx \mathrm{Exp}(\phi)\mathrm{Exp}(J_r(\phi)\delta\phi),\ \ \text{且令}\ \bar{J} _r^k \doteq J_r((\tilde{w} _k - \bar{b} _{g_i})\Delta t)} \newline &= \prod _{k=i}^{j-1} \mathrm{Exp} \Big(\big( \tilde{w} _k - \bar{b} _{g_i} \big) \Delta t \Big) \mathrm{Exp} \Big( - \bar{J} _r^k \delta b _{g_i} \Delta t \Big) \newline \small{\text{利用 Exp}}& \small{(\phi)\ R = R \ \mathrm{Exp}(R^T\phi)\ \text{将} \ \delta b _{g_i}\ \text{噪声项移至最后}} \newline &= \Delta \bar{R} _{ij} \prod _{k=i}^{j-1} \mathrm{Exp} \Big(- \Delta \bar{R} _{k+1j} \bar{J} _r^k \delta b _{g_i} \Delta t \Big) \newline \small{\text{利用 }\mathrm{Log}} & \small{\mathrm{(Exp(\phi)Exp(\delta\phi))} \approx \phi + J_r(\phi)^{-1}\delta\phi, \text{ 当 } \phi \text{ 也是小量时有} J_r(\phi)^{-1} \approx I} \newline \small{\text{由于 }\delta b _{g_i} \ } & \small{为小量，因此 } - \Delta \bar{R} _{k+1j} \bar{J} _r^k \delta b _{g_i} \Delta t \text{ 也是小量, 令 } \alpha_k = - \Delta \bar{R} _{k+1j} \bar{J} _r^k \delta b _{g_i} \Delta t\newline &= \Delta \bar{R} _{ij} \mathrm{Exp}(\alpha_i) \mathrm{Exp}(\alpha _{i+1}) \prod _{k=i+2}^{j-1} \mathrm{Exp} (\alpha_k) \newline &\approx \Delta \bar{R} _{ij} \mathrm{Exp}(\alpha_i + \alpha _{i+1}) \mathrm{Exp}(\alpha _{i+2}) \mathrm{Exp}(\alpha _{i+3}) \prod _{k=i+4}^{j-1} \mathrm{Exp} (\alpha_k) \newline &\approx \Delta \bar{R} _{ij} \mathrm{Exp} \left( \sum _{k=i}^{j-1} \alpha_i \right) \newline &= \Delta \bar{R} _{ij} \mathrm{Exp} \left( \sum _{k=i}^{j-1} \Big(- \Delta \bar{R} _{k+1j} \bar{J} _r^k \delta b _{g_i} \Delta t \Big) \right) \newline \end{align*} \tag{2.17}\label{2.17} $$

于是根据 $\eqref{2.16}$ 的定义有

$$ \frac{\partial \Delta \bar{R} _{ij}}{\partial b_g} = \sum _{k=i}^{j-1} \Big(- \Delta \bar{R} _{k+1j} \bar{J} _r^k \Delta t \Big) \ , \ \text{其中} \ \bar{J} _r^k \doteq J_r((\tilde{w} _k - \bar{b} _{g_i})\Delta t) \tag{2.18}\label{2.18} $$

2.3.2 $\Delta \tilde{v} _{ij}$ 项关于 bias 的雅克比

根据式 $\eqref{2.5}$ 有：

$$ \begin{align*} \Delta \tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) &= \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik}(\hat{b} _{g_i}) \big(\tilde{a} _k - \hat{b} _{a_i} \big) \Delta t \Big] \newline &= \sum _{k=i}^{j-1} \Big[ \Delta \tilde{R} _{ik}(\hat{b} _{g_i}) \big(\tilde{a} _k - (\bar{b} _{a_i} + \delta b _{a_i}) \big) \Delta t \Big] \newline \small{\text{结合式} \eqref{2.16} \eqref{2.17}} &= \sum _{k=i}^{j-1} \Big[ \Delta \bar{R} _{ik} \mathrm{Exp} \big( \frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \delta b _{g_i} \big) \big(\tilde{a} _k - \bar{b} _{a_i} - \delta b _{a_i} \big) \Delta t \Big] \newline {\small \text{利用} \exp(\delta\phi^\wedge) \approx I+\delta\phi^\wedge} &\approx \sum _{k=i}^{j-1} \Big[ \Delta \bar{R} _{ik} \big( I + (\frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \delta b _{g_i})^{\wedge} \big) \big(\tilde{a} _k - \bar{b} _{a_i} - \delta b _{a_i} \big) \Delta t \Big] \newline {\small \text{忽略高阶小项 }} & {\small (\frac{\partial \Delta \bar{R} _{ij}}{\partial b_g} \delta b _{g_i})^{\wedge} \delta b _{a_i} \ , \text{利用 } \alpha^\wedge \beta = -\beta^\wedge \alpha \ ,\ \Delta \bar{v} _{ij} \doteq \sum _{k=i}^{j-1} \Big[ \Delta \bar{R} _{ik} \big(\tilde{a} _k - \bar{b} _{a_i} \big) \Delta t \Big] } \newline &\approx \Delta \bar{v} _{ij} - \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} \Delta t \delta b _{a_i} + \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} (\frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \delta b _{g_i} )^{\wedge} (\tilde{a} _k -\bar{b} _{a_i}) \Delta t \newline &\approx \Delta \bar{v} _{ij} - \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} \Delta t \delta b _{a_i} - \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} (\tilde{a} _k -\bar{b} _{a_i})^{\wedge} \frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \Delta t \delta b _{g_i} \newline \end{align*} \tag{2.19}\label{2.19} $$

于是根据 $\eqref{2.16}$ 的定义有

$$ \begin{align*} \frac{\partial \Delta \bar{v} _{ij}}{\partial b_g} &= - \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} (\tilde{a} _k -\bar{b} _{a_i})^{\wedge} \frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \Delta t \newline \frac{\partial \Delta \bar{v} _{ij}}{\partial b_a} &= - \sum _{k=i}^{j-1} \Delta \bar{R} _{ik} \Delta t \end{align*} \tag{2.20}\label{2.20} $$

2.3.3 $\Delta \tilde{p} _{ij}$ 项关于 bias 的雅克比

根据式 $\eqref{2.6}$ 有：

$$ \begin{align*} \Delta \tilde{p} _{ij}(\hat{b} _{g}, \hat{b} _a) &= \sum _{k=i}^{j-1} \Big[ \Delta \tilde{v} _{ik}(\hat{b} _{g}, \hat{b} _a) \Delta t + \frac{1}{2} \Delta \tilde{R} _{ik}(\hat{b} _{g_i}) \big(\tilde{a} _k - \hat{b} _{a_i} \big) \Delta t^2 \Big] \newline &= \underbrace{\sum _{k=i}^{j-1} \Big[ \Delta \tilde{v} _{ik}(\hat{b} _{g}, \hat{b} _a) \Delta t \Big]} _{\enclose{roundedbox}{1}} + \underbrace{\frac{1}{2} \sum _{k=i}^{j-1} \Big[\Delta \tilde{R} _{ik}(\hat{b} _{g_i}) \big(\tilde{a} _k - \hat{b} _{a_i} \big) \Delta t^2 \Big]} _{\enclose{roundedbox}{2}} \newline \enclose{roundedbox}{1} &= \sum _{k=i}^{j-1} \Big[ \big( \Delta \bar{v} _{ik} + \frac{\partial \Delta \bar{v} _{ik}}{\partial b_g} \delta b _{g_i} + \frac{\partial \Delta \bar{v} _{ik}}{\partial b_a} \delta b _{a_i} \big) \Delta t \Big] \newline &= \sum _{k=i}^{j-1} \Big[ \Delta \bar{v} _{ik} \Delta t + \frac{\partial \Delta \bar{v} _{ik}}{\partial b_g} \Delta t \delta b _{g_i} + \frac{\partial \Delta \bar{v} _{ik}}{\partial b_a} \Delta t \delta b _{a_i} \Big] \newline \enclose{roundedbox}{2} &= \frac{1}{2} \sum _{k=i}^{j-1} \Big[\Delta \bar{R} _{ik} \mathrm{Exp} \big( \frac{\partial \Delta \bar{R} _{ik}}{\partial b_g} \delta b _{g_i} \big) \big(\tilde{a} _k - (\bar{b} _{a_i} + \delta b _{a_i}) \big) \Delta t^2 \Big] \newline &{\small \text{忽略高阶小项 } (\frac{\partial \Delta \bar{R} _{ij}}{\partial b_g} \delta b _{g_i})^{\wedge} \delta b _{a_i} \ , \text{利用 } \exp(\delta\phi^\wedge) \approx I+\delta\phi^\wedge, \ \ \alpha^\wedge \beta = -\beta^\wedge \alpha } \newline &\approx \frac{\Delta t^2}2 \sum _{k=i}^{j-1} \Big[ \Delta\bar{R} _{ik} \big( I + (\frac{\partial \Delta\bar{R} _{ik}}{\partial{b_g}} \delta b _{g_i})^\wedge \big) \big(\tilde{a} _k - \bar{b} _{a_i} - \delta b _{a_i} \big) \Big] \newline &\approx \frac{\Delta t^2}2 \sum _{k=i}^{j-1} \Big[ \Delta\bar{R} _{ik}(\tilde{a} _k - \bar{b} _{a_i}) - \Delta\bar{R} _{ik} \delta b _{a_i} - \Delta\bar{R} _{ik}(\tilde{a} _k - \bar{b} _{a_i})^\wedge \frac{\partial \Delta\bar{R} _{ik}}{\partial{b_g}} \delta b _{g_i} \Big] \newline \enclose{roundedbox}1 + \enclose{roundedbox}2 &= \sum _{k=i}^{j-1} \bigg[ \Big[ \Delta\bar{v} _{ik} \Delta t + \frac12 \Delta\bar{R} _{ik}(\tilde{a} _k -\bar{b} _{a_i}) \Delta t^2 \Big] + \newline &\qquad\quad\ \ \Big [ \frac{\partial\Delta\bar{v} _{ik}}{\partial{b} _g} \Delta t - \frac12 \Delta\bar{R} _{ik} (\tilde{a} _k - \bar{b} _{a_i})^\wedge \frac{\partial \Delta\bar{R} _{ik}}{\partial{b_g}} \Delta t^2 \Big] \delta b _{g_i} + \newline & \qquad\quad\ \ \Big[ \frac{\partial \Delta \bar{v} _{ik}}{\partial b_a} \Delta t - \frac12 \Delta\bar{R} _{ik} \Delta t^2 \Big] \delta b _{a_i} \bigg] \end{align*} \tag{2.21}\label{2.21} $$

于是根据 $\eqref{2.16}$ 的定义有

$$ \begin{align*} \Delta\bar{p} _{ij} &= \sum _{k=i}^{j-1} \Big[ \Delta\bar{v} _{ik} \Delta t + \frac12 \Delta\bar{R} _{ik}(\tilde{a} _k -\bar{b} _{a_i}) \Delta t^2 \Big] \newline \frac{\partial\Delta\bar{p} _{ij}}{\partial b_g} &= \sum _{k=i}^{j-1} \Big[ \frac{\partial\Delta\bar{v} _{ik}}{\partial{b} _g} \Delta t - \frac12 \Delta\bar{R} _{ik} (\tilde{a} _k - \bar{b} _{a_i})^\wedge \frac{\partial \Delta\bar{R} _{ik}}{\partial{b_g}} \Delta t^2 \Big] \newline \frac{\partial\Delta\bar{p} _{ij}}{\partial b_a} &= \sum _{k=i}^{j-1} \Big[ \frac{\partial \Delta \bar{v} _{ik}}{\partial b_a} \Delta t - \frac12 \Delta\bar{R} _{ik} \Delta t^2 \Big] \end{align*} \tag{2.22}\label{2.22} $$

通过 2.3.1~2.3.3 我们可知，当接收到新的测量数据时这些雅克比可以通过递增方式及时计算。

3. IMU 预积分残差

通过预积分测量模型 (式 $\eqref{2.7}$) 和测量噪声在一阶 (式 $\eqref{2.8}$) 上协方差为 $\Sigma _{ij}$ 的零均高斯噪声，因此可以得到关于 IMU 预积分因子的残差 $r _{\mathcal{I} _{ij}} \doteq [r^T _{\Delta R _{ij}}, \ r^T _{\Delta v _{ij}},\ r^T _{\Delta p _{ij}}] \in \mathbb{R}^9$ ，有：

$$ \begin{align*} r _{\Delta R _{ij}} &\doteq \mathrm{Log} \left( \Big( \Delta\tilde{R} _{ij}(\bar{b} _{g_i}) \mathrm{Exp} \big( \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \delta b _{g_i} \big) \Big)^T R_i^T R_j \right) \newline r _{\Delta v _{ij}} &\doteq R_i (v_j - v_i -g\Delta t _{ij}) - \Big[ \Delta \tilde{v} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i}) + \frac{\partial \Delta \bar{v} _{ij}}{\partial b_g} \delta b _{g_i} + \frac{\partial \Delta \bar{v} _{ij}}{\partial b_a} \delta b _{a_i} \Big] \newline r _{\Delta p _{ij}} &\doteq R_i^T(p_j - p_i - v_i \Delta t _{ij} - \frac12 g \Delta t _{ij}^2 ) - \Big[ \Delta \tilde{p} _{ij}(\bar{b} _{g_i}, \bar{b} _{a_i}) + \frac{\partial \Delta \bar{p} _{ij}}{\partial b_g} \delta b _{g_i} + \frac{\partial \Delta \bar{p} _{ij}}{\partial b_a} \delta b _{a_i} \Big] \end{align*} \tag{3.1}\label{3.1} $$

其中包含前面推导的伴随 bias 迭代更新的预积分测量如式 $\eqref{2.16}$ ，这种近似的修正方式避免了积分的重新计算，是预积分技术降低计算量的关键。

首先，需要先理解 IMU 预积分因子中的残差指的是预计预积分计算值 (由非 IMU 的其他方式估计的预积分值) 与 IMU 预积分测量值的差。在进行优化时，我们通常采用 “lift-solve-retract” 的方式，而每一次迭代过程将利用

$$ \begin{array}{rclcrcl} R_i & \gets & R_i\mathrm{Exp}(\delta\phi_i), &\qquad & R_j & \gets & R_j\mathrm{Exp}(\delta\phi_j), \newline p_i & \gets & p_i + R_i\delta p_i, & & p_j & \gets & p_j + R_j \delta p_j, \newline v_i & \gets & v_i + \delta v_i, & & v_j & \gets & v_j + \delta v_j, \newline \delta b _{g_i} & \gets & \delta b _{g_i} + \tilde{\delta}b _{g_i}, & & \delta b _{a_i} & \gets & \delta b _{b_i} + \tilde{\delta}b _{a_i} \end{array} \tag{3.2}\label{3.2} $$

重新进行参数化式 $\eqref{3.1}$ ，通过增量来更新状态进而 retract 估计量，而 solve 步骤需要围绕当前估计的结果成本进行线性化，而线性化展开的目的是便于解析计算残差的雅克比行列表达式。这个过程通常被称之为 lifting ，它使得残差成为定义在易于计算雅克比行列式的向量空间上的函数。因此我们接下来将推导残差关于向量 $\delta \phi_i, \delta p_i, \delta v_i, \delta\phi_j, \delta p_j, \delta v_j, \tilde\delta b _{g_i} \tilde\delta b _{a_i}$ 的雅克比行列式。

3.1 残差 $r _{\Delta R _{ij}}$ 的雅克比

由于 $r _{\Delta R _{ij}}$ 中不含 $p_i, p_j, v_i, v_j, \delta b _{a_i}$ ，因此 $r _{\Delta R _{ij}}$ 关于这些状态增量的雅克比都为 0 。在 3.1.1~3.1.3 的推导中采用 $E \doteq \mathrm{Exp}\Big( \frac{\partial\bar R _{ij}}{\partial b_g}\delta b _{g_i} \Big)$ 来进行速记。

3.1.1 残差 $r _{\Delta R _{ij}}$ 关于 $\delta\phi_i$ 的雅克比

$$ \begin{align*} r _{\Delta R _{ij}}(R_i \mathrm{Exp}(\delta\phi_i)) &= \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T (R_i \mathrm{Exp}(\delta\phi_i))^T R_j \big) \newline &= \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T \mathrm{Exp}(- \delta\phi_i) R_i^T R_j \big) \newline {\small \text{利用}\ \mathrm{Exp}(\phi)R = R\mathrm{Exp}(R^T\phi)} &= \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T R_i^T R_j \mathrm{Exp}(- R_j^T R_i \delta\phi_i) \big) \newline &= \mathrm{Log} \Big(\mathrm{Exp} \big( \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T R_i^T R_j \big)\big) \mathrm{Exp}(- R_j^T R_i \delta\phi_i) \Big) \newline {\small \text{利用}\ }& {\small \mathrm{Log}(\mathrm{Exp}(\phi)\mathrm{Exp}(\delta\phi)) \approx \phi + J_r^{-1}(\phi)\delta\phi} \newline &\approx r _{\Delta R _{ij}}(R_i) - J_r^{-1}(r _{\Delta R _{ij}}(R_i))R_j^T R_i \delta\phi_i \end{align*} \tag{3.3}\label{3.3} $$

3.1.2 残差 $r _{\Delta R _{ij}}$ 关于 $\delta\phi_j$ 的雅克比

$$ \begin{align*} r _{\Delta R _{ij}}(R_j \mathrm{Exp}(\delta\phi_j)) &= \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T R_i^T R_j\mathrm{Exp}(\delta\phi_j) \big) \newline &= \mathrm{Log} \Big(\mathrm{Exp} \big( \mathrm{Log} \big( (\Delta\tilde R _{ij} (\bar b _{g_i}) E)^T R_i^T R_j \big)\big) \mathrm{Exp}(\delta\phi_j) \Big) \newline {\small \text{利用}\ }& {\small \mathrm{Log}(\mathrm{Exp}(\phi)\mathrm{Exp}(\delta\phi)) \approx \phi + J_r^{-1}(\phi)\delta\phi} \newline &\approx r _{\Delta R _{ij}}(R_j) + J_r^{-1}(r _{\Delta R _{ij}}(R_j)) \delta\phi_j \end{align*} \tag{3.4}\label{3.4} $$

3.1.3 残差 $r _{\Delta R _{ij}}$ 关于 $\tilde\delta b _{g_i}$ 的雅克比

$$ \begin{align*} r _{\Delta R _{ij}}(\delta b _{g_i} + \tilde\delta b _{g_i}) &= \mathrm{Log} \Big( \Big( \Delta\tilde{R} _{ij}(\bar{b} _{g_i}) \mathrm{Exp} \big( \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} (\delta b _{g_i} + \tilde\delta b _{g_i}) \big) \Big)^T R_i^T R_j \Big) \newline {\small \text{利用}\ } & {\small \mathrm{Exp}(\phi + \delta\phi) \approx \mathrm{Exp}(\phi)\mathrm{Exp}(J_r(\phi)\delta\phi), \ \text{令}\ J_r^b \doteq J_r(\frac{\partial\Delta\bar R _{ij}}{\partial b_g}\delta b _{g_i})} \newline &= \mathrm{Log}\Big(\Big( \Delta\tilde{R} _{ij}(\bar{b} _{g_i}) E \ \mathrm{Exp} \big( J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \big) \Big)^T R_i^T R_j \Big) \newline &= \mathrm{Log}\Big(\mathrm{Exp} \big( J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \big)^T (\Delta\tilde{R} _{ij}(\bar{b} _{g_i}) E)^T R_i^T R_j \Big) \newline &= \mathrm{Log} \Big( \mathrm{Exp}\big(-J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \big) \mathrm{Exp}\big( r _{\Delta R _{ij}}(\delta b _{g_i}) \big) \Big) \newline {\small \text{利用}\ \mathrm{Exp}(\phi)R = R\mathrm{Exp}(R^T\phi)} &= \mathrm{Log} \Big( \mathrm{Exp}\big( r _{\Delta R _{ij}}(\delta b _{g_i}) \big) \cdot \newline & \qquad\quad\ \ \mathrm{Exp}\big( -\mathrm{Exp}(r _{\Delta R _{ij}}(\delta b _{g_i}))^T J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \big) \Big) \newline {\small \text{利用}\ }& {\small \mathrm{Log}(\mathrm{Exp}(\phi)\mathrm{Exp}(\delta\phi)) \approx \phi + J_r^{-1}(\phi)\delta\phi} \newline &\approx r _{\Delta R _{ij}}(\delta b _{g_i}) -J_r\big(r _{\Delta R _{ij}}(\delta b _{g_i})\big)\mathrm{Exp}(r _{\Delta R _{ij}}(\delta b _{g_i}))^T J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \end{align*} \tag{3.5}\label{3.5} $$

3.1.4 总结

根据上述的推导，残差 $r _{\Delta R _{ij}}$ 关于 $\delta \phi_i, \delta p_i, \delta v_i, \delta\phi_j, \delta p_j, \delta v_j, \tilde\delta b _{g_i} \tilde\delta b _{a_i}$ 的雅克比为：

$$ \begin{align*} \frac{\partial r _{\Delta R _{ij}}}{\partial\delta\phi_i} &= - J_r^{-1}(r _{\Delta R _{ij}}(R_i))R_j^T R_i & \frac{\partial r _{\Delta R _{ij}}}{\partial\delta p_i} &= 0 \newline \frac{\partial r _{\Delta R _{ij}}}{\partial\delta v_i} &= 0 & \frac{\partial r _{\Delta R _{ij}}}{\partial\delta\phi_j} &= J_r^{-1}(r _{\Delta R _{ij}}(R_j)) \newline \frac{\partial r _{\Delta R _{ij}}}{\partial\delta p_j} &= 0 & \frac{\partial r _{\Delta R _{ij}}}{\partial\delta v_j} &= 0 \newline \frac{\partial r _{\Delta R _{ij}}}{\partial\tilde\delta b _{a_i}} &= 0 & \frac{\partial r _{\Delta R _{ij}}}{\partial\tilde\delta b _{g_i}} &= -J_r\big(r _{\Delta R _{ij}}(\delta b _{g_i})\big)\mathrm{Exp}(r _{\Delta R _{ij}}(\delta b _{g_i}))^T J_r^b \frac{\partial\Delta\bar{R} _{ij}}{\partial b_g} \end{align*} \tag{3.6}\label{3.6} $$

3.2 残差 $r _{\Delta v _{ij}}$ 的雅克比

由于 $r _{\Delta v _{ij}}$ 中不含 $R_j, p_i, p_j$ ，因此 $r _{\Delta v _{ij}}$ 关于这些状态增量的雅克比都为 0 。

3.2.1 残差 $r _{\Delta v _{ij}}$ 关于 $\delta\phi_i$ 的雅克比

$$ \begin{align*} r _{\Delta v _{ij}}(R_i \mathrm{Exp}(\phi_i)) &= (R_i \mathrm{Exp}(\phi_i))^T (v_j - v_i - g\Delta t _{ij}) - \Delta\tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= \mathrm{Exp}(\phi_i)^T R_i^T (v_j - v_i - g\Delta t _{ij}) - \Delta\tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= \mathrm{Exp}(-\phi_i) R_i^T (v_j - v_i - g\Delta t _{ij}) - \Delta\tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline {\small \text{利用}\ \mathrm{Exp}(\delta\phi) \approx I + \delta\phi^{\wedge}} &\approx (I - \delta\phi_i^{\wedge}) R_i^T (v_j - v_i - g\Delta t _{ij}) - \Delta\tilde{v} _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta v _{ij}}(R_i) - \delta\phi_i^{\wedge} \big( R_i^T (v_j - v_i - g\Delta t _{ij}) \big) \newline {\small \text{利用}\ \alpha^\wedge\beta = -\beta^\wedge\alpha} &= r _{\Delta v _{ij}}(R_i) + \big( R_i^T (v_j - v_i - g\Delta t _{ij}) \big)^\wedge \delta\phi_i \end{align*} \tag{3.7}\label{3.7} $$

3.2.3 残差 $r _{\Delta v _{ij}}$ 关于 $\delta v_i$ 和 $\delta v_j$ 的雅克比

$$ \begin{align*} r _{\Delta v _{ij}}(v_i + \delta v_i) &= R_i^T (v_j - v_i -\delta v_i -g\Delta t _{ij}) - \Delta\tilde v _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta v _{ij}}(v_i) - R_i^T \delta v_i \newline r _{\Delta v _{ij}}(v_j + \delta v_j) &= R_i^T (v_j + \delta v_j - v_i -g\Delta t _{ij}) - \Delta\tilde v _{ij}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta v _{ij}}(v_i) + R_i^T \delta v_j \end{align*} \tag{3.8}\label{3.8} $$

3.2.3 残差 $r _{\Delta v _{ij}}$ 关于 $\tilde\delta b _{a_i}$ 和 $\tilde\delta b _{g_i}$ 的雅克比

由于 $r _{\Delta v _{ij}}$ 关于 $\delta b _{a_i}$ 和 $\delta b _{g_i}$ 是线性的，因此 $r _{\Delta v _{ij}}$ 关于 $\tilde\delta b _{a_i}$ 和 $\tilde\delta b _{g_i}$ 的雅克比可以直接依据线性系数得到，依赖于式 $\eqref{2.20}$ 易得：

$$ \begin{align*} r _{\Delta v _{ij}}(\delta b _{a_i} + \tilde\delta b _{a_i}) &= r _{\Delta v _{ij}}(\delta b _{a_i}) -\frac{\partial \Delta \bar{v} _{ij}}{\partial b_a} \tilde\delta b _{a_i} \newline r _{\Delta v _{ij}}(\delta b _{g_i} + \tilde\delta b _{g_i}) &= r _{\Delta v _{ij}}(\delta b _{g_i}) -\cfrac{\partial \Delta \bar{v} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \end{align*} \tag{3.9}\label{3.9} $$

3.2.4 总结

根据上述的推导，残差 $r _{\Delta v _{ij}}$ 关于 $\delta \phi_i, \delta p_i, \delta v_i, \delta\phi_j, \delta p_j, \delta v_j, \tilde\delta b _{g_i} \tilde\delta b _{a_i}$ 的雅克比为：

$$ \begin{align*} \frac{\partial r _{\Delta v _{ij}}}{\partial\delta\phi_i} &= \big( R_i^T (v_j - v_i - g\Delta t _{ij}) \big)^\wedge & \frac{\partial r _{\Delta v _{ij}}}{\partial\delta p_i} &= 0 \newline \frac{\partial r _{\Delta v _{ij}}}{\partial\delta v_i} &= -R_i^T & \frac{\partial r _{\Delta v _{ij}}}{\partial\delta\phi_j} &= 0 \newline \frac{\partial r _{\Delta v _{ij}}}{\partial\delta p_j} &= 0 & \frac{\partial r _{\Delta v _{ij}}}{\partial\delta v_j} &= R_i^T \newline \frac{\partial r _{\Delta v _{ij}}}{\partial\tilde\delta b _{a_i}} &= -\frac{\partial\Delta\bar{v} _{ij}}{\partial b_a} & \frac{\partial r _{\Delta v _{ij}}}{\partial\tilde\delta b _{g_i}} &= -\frac{\partial\Delta\bar{v} _{ij}}{\partial b_g} \end{align*} \tag{3.10}\label{3.10} $$

3.3 残差 $r _{\Delta p _{ij}}$ 的雅克比

由于 $r _{\Delta p _{ij}}$ 中不含 $R_j, v_j$ ，因此 $r _{\Delta p _{ij}}$ 关于这些状态增量的雅克比都为 0 。

3.3.1 残差 $r _{\Delta p _{ij}}$ 关于 $\delta\phi_i$ 的雅克比

$$ \begin{align*} r _{\Delta p _{ij}}(R_i \mathrm{Exp}(\delta\phi_i)) &= (R_i \mathrm{Exp}(\delta\phi_i))^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= \mathrm{Exp}(\delta\phi_i)^T R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= \mathrm{Exp}(-\delta\phi_i) R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline {\small \text{利用}\ \mathrm{Exp}(\delta\phi) \approx I + \delta\phi^\wedge} &\approx (I - \delta\phi_i^\wedge) \big( R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) \big) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta p _{ij}}(R_i) - \delta\phi_i^\wedge \big( R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) \big) \newline {\small \text{利用}\ \alpha^\wedge\beta = -\beta^\wedge\alpha} &= r _{\Delta p _{ij}}(R_i) - \big( R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) \big)^\wedge \delta\phi_i \end{align*} \tag{3.11}\label{3.11} $$

3.3.2 残差 $r _{\Delta p _{ij}}$ 关于 $\delta v_i$ 的雅克比

$$ \begin{align*} r _{\Delta p _{ij}}(v_i + \delta v_i) &= R_i^T (p_j - p_i - v_i\Delta t _{ij} - \delta v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta p _{ij}}(v_i) - R_i^T \Delta t _{ij} \delta v_i \end{align*} \tag{3.12}\label{3.12} $$

3.2.3 残差 $r _{\Delta p _{ij}}$ 关于 $\delta p_i$ 和 $\delta p_i$ 的雅克比

$$ \begin{align*} r _{\Delta p _{ij}}(p_i + R_i \delta p_i) &= R_i^T (p_j - p_i - R_i \delta p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta p _{ij}}(p_i) - I _{3\times3} \delta p_i \newline r _{\Delta p _{ij}}(p_j + R_j \delta p_j) &= R_i^T (p_j + R_j\delta p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) - \Delta\tilde{p}(\hat{b} _{g_i}, \hat{b} _{a_i}) \newline &= r _{\Delta p _{ij}}(p_i) + R_i^T R_j \delta p_i \end{align*} \tag{3.13}\label{3.13} $$

3.3.4 残差 $r _{\Delta p _{ij}}$ 关于 $\tilde\delta b _{a_i}$ 和 $\tilde\delta b _{g_i}$ 的雅克比

由于 $r _{\Delta v _{ij}}$ 关于 $\delta b _{a_i}$ 和 $\delta b _{g_i}$ 是线性的，因此 $r _{\Delta v _{ij}}$ 关于 $\tilde\delta b _{a_i}$ 和 $\tilde\delta b _{g_i}$ 的雅克比可以直接依据线性系数得到，依赖于式 $\eqref{2.22}$ 易得：

$$ \begin{align*} r _{\Delta p _{ij}}(\delta b _{a_i} + \tilde\delta b _{a_i}) &= r _{\Delta p _{ij}}(\delta b _{a_i}) -\frac{\partial \Delta \bar{p} _{ij}}{\partial b_a} \tilde\delta b _{a_i} \newline r _{\Delta p _{ij}}(\delta b _{g_i} + \tilde\delta b _{g_i}) &= r _{\Delta p _{ij}}(\delta b _{g_i}) -\cfrac{\partial \Delta \bar{p} _{ij}}{\partial b_g} \tilde\delta b _{g_i} \end{align*} \tag{3.14}\label{3.14} $$

3.3.5 总结

根据上述的推导，残差 $r _{\Delta p _{ij}}$ 关于 $\delta \phi_i, \delta p_i, \delta v_i, \delta\phi_j, \delta p_j, \delta v_j, \tilde\delta b _{g_i} \tilde\delta b _{a_i}$ 的雅克比为：

$$ \begin{align*} \frac{\partial r _{\Delta p _{ij}}}{\partial\delta\phi_i} &= \big( R_i^T (p_j - p_i - v_i\Delta t _{ij} - \frac12g\Delta t _{ij}^2) \big)^\wedge & \frac{\partial r _{\Delta p _{ij}}}{\partial\delta p_i} &= -I _{3\times3} \newline \frac{\partial r _{\Delta p _{ij}}}{\partial\delta v_i} &= -R_i^T \Delta t _{ij} & \frac{\partial r _{\Delta p _{ij}}}{\partial\delta\phi_j} &= 0 \newline \frac{\partial r _{\Delta p _{ij}}}{\partial\delta p_j} &= R_i^T R_j & \frac{\partial r _{\Delta p _{ij}}}{\partial\delta v_j} &= 0 \newline \frac{\partial r _{\Delta p _{ij}}}{\partial\tilde\delta b _{a_i}} &= -\frac{\partial\Delta\bar{p} _{ij}}{\partial b_a} & \frac{\partial r _{\Delta p _{ij}}}{\partial\tilde\delta b _{g_i}} &= -\frac{\partial\Delta\bar{p} _{ij}}{\partial b_g} \end{align*} \tag{3.15}\label{3.15} $$

4. Bias 模型

在介绍 IMU 模型时，式 $\eqref{1.1}$ ，我们说 biases 式随时间缓慢变化的量。因此，采用 “布朗运动 (Brownian motion)” 进行建模（补充：物理学中的布朗运动与数学中的 “维纳过程 (Wiener process)” 密切相关）。biases 白噪运动学的微分方程形式如下：

$$ \dot b_g(t) = \eta _{b_g}, \qquad \dot b_a(t) = \eta _{b_a} \tag{4.1}\label{4.1} $$

根据上式，在两个连续关键帧 i 和 j 之间的时间间隔 $[t_i, t_j]$ 上积分得：

$$ b _{g_j} = b _{g_i} + \eta _{b _{gd}}, \qquad b _{a_j} = b _{a_i} + \eta _{b _{ad}} \tag{4.2}\label{4.2} $$

其中，上式采用例如 $b _{g_i} \doteq b_g({t_i})$ 进行速记，且定义带有零均值和协方差分别为 $\Sigma _{b _{gd}} \doteq \Delta t _{ij} \mathrm{Cov}(\eta _{b_g})$ 和 $\Sigma _{b _{ad}} \doteq \Delta t _{ij} \mathrm{Cov}(\eta _{b_a})$ 的离散噪声 $\eta _{b _{gd}}$ 和 $\eta _{b _{ad}}$ 。根据式 $\eqref{4.2}$ 可以简单被包含到因子图优化中，作为 IMU 预积分因子的附加项。

5. IMU 噪声的形式与单位

当我们在使用 IMU 的时候，离不开对 IMU Allan 方差的标定。这时候，我们通常会采用 kalibr_allan 或 allan_variance_ros 工具进行标定。根据式 $\eqref{1.6}$ 和第 4 节的内容，可知 IMU 的噪声形式分为连续形式和离散形式。而不同形式下其单位不同。结合前面的内容与对应的参考文献，下面直接给出不同形式下名称与单位。

Reference