IMU预积分

滑动窗口

在VIO中，最重要的一件事就是确保视觉信息可以和IMU信息对齐，但是IMU的高频率（1000Hz以上）和视觉的频率（10Hz）必然存在差别。在视觉还没有出下一帧的时候，IMU正在疯狂发送信号，我们要做的就是确保每帧中间的IMU信息不能丢失，并且和下一帧到来的图像一起作用于状态优化。

时间 ──────────────────────────────────────────────────►

图像   ●───────────●───────────●───────────●─────   10 Hz
       帧0         帧1         帧2         帧3

IMU    ▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪▪    1000 Hz
       └──100个样本──┘

对于一次状态估计，我们需要一定区间内的有效信息，而且需要在保证信息时效性的同时，避免过早信息的丢失，又要兼顾运算的效率。综合以上需求，我们就不能简单的将开机以来的全部信息用作状态估计，也不能草率的直接丢弃所有过期帧。我们需要在所需区间内（在本定位仓库中为 20 帧对应 19 条边）进行处理：每次积累到 21 帧，就对最老的十帧做边缘化（即信息的压缩处理），把压缩出的先验融合进后续的状态估计中。

具体参数（$\texttt{swf\_options.h}$）：$\texttt{state\_slots}=21$、$\texttt{max\_window\_frames}=20$、$\texttt{group\_removal\_frames}=10$。所以窗口是锯齿式振荡的：

$$21\ \text{帧}\ \xrightarrow{\ \text{删最老 10 帧}\ }\ 11\ \text{帧}\ \xrightarrow{\ \text{再长到}\ }\ 21\ \text{帧}\ \xrightarrow{\ \text{再删}\ }\ 11\ \text{帧}\ \cdots$$

这种处理方法确保了每次状态估计都只处理 11 到 21 帧之间的信息，同时保证了每次压缩都能保留一定意义的过期信息，并且持续性地影响后面十轮估计。

按照刚才的区分，我们把一个从10到20帧的关注区间称为窗口，这就是我们进行的BA滑动窗口。其中每个帧作为一个节点，节点之间就是边。我们需要在每一个节点进入时进行估计，采用来自图像的信息以及所有边的IMU累计信息。这个累加IMU信息是确保帧之间的本地位姿变换准确的重要信息，用下面这个式子表示

$$\mathbf{p}_j^w = \mathbf{p}_i^w + \mathbf{v}_i^w \Delta t - \frac{1}{2} \mathbf{g}^w \Delta t^2 + \iint_{t} \mathbf{R}_t^w (\hat{\mathbf{a}}_t - \mathbf{b}_a) \, dt^2$$

可以这么理解：世界坐标系下的位姿等于上一个时刻（帧）的位姿加上当前速度对时间的累计，加上重力加速度对当前位姿的影响，再加上IMU信息在这两帧之间的累计作用。注意到后面的积分部分其实就是加计减去零漂之后转换到世界坐标系之后对时间做二重积分。

在这个过程中只有第一项和第三项强依赖当前状态，而只有第三项实质参与积分。需要每次都重新计算。

预积分完整流程

这个过程我理了很久才终于整明白了，希望接下来的记录能够正确呈现完整过程，但是与其说是只有预积分也不太对，这个步骤完整涵盖了一次预积分的迭代过程，也大体上就是一次前端加后端对画面的处理过程。

按照相机进帧的顺序，我们可以先理清楚所有阶段的称呼：

根据上面滑动窗口原理的介绍，我们可以这么区分

相机画面被传入的时候就是一帧，这个帧就是我们所有计算进行的节点
帧之间的间隔留给IMU信息的累积，叫做一个边
预积分进行在每帧到达的时候，是对该帧前一个边的IMU数据进行处理的步骤
优化步骤在预积分进行完成之后，读取所有观测和状态，求残差矩阵然后构建代价函数进行优化，最后得到320维（最大）的状态矩阵，其包括所有帧的位姿状态以及所有帧的零偏信息，还有20个全局状态。

具体的流程可以用这张图来展示：（deepseek你帮我画在下面）

怎么理解这个流程呢？

对于一个正常运转的系统来说，窗口内的帧数在11到20之间。首先，在上一帧结束之后，IMU不断传入信息，我们存储但是不进行处理。
直到下一帧图片进来后，我们立刻对边上的所有IMU信息进行预积分。暂时可以理解成得到了关于位姿和运动状态的一些状态量，还有这些预积分的残差协方差，用来描述这些预测准确性的，后续用来参与马氏范数加权优化。
刚才计算出来的状态变化量和之前记录的状态（以往轮次得到的状态）作差得到残差，当作IMU残差和视觉重投影残差一起传入优化器构建代价函数，用狗腿法优化后得到新的预测状态矩阵，里面包含之前所有帧的位姿、运动、零偏（传感器的随机误差，很大程度上影响观测精度，需要在优化器中进行实时估计）等以及一些全局信息。
等待下一帧进入，这次使用刚才估计出来的最新零偏作为本轮采用的零偏（也就是用优化器在更新零偏）。同时采用上一轮的C等进行递推。

上面只是一个大致的流程，涵盖了完整的预积分迭代过程，具体内容将在下面展开叙述。

预积分产物

实际上预积分的状态产物有三个量，它们是同步递推的。

旋转预积分

$$\Delta \mathbf{R}_{k+1} = \Delta \mathbf{R}_k \cdot \text{Exp}\left( \frac{1}{2} (\hat{\boldsymbol{\omega}}_k + \hat{\boldsymbol{\omega}}_{k+1} - 2 \mathbf{b}_g) \Delta t \right)$$

速度预积分

$$\Delta \mathbf{v}_{k+1} = \Delta \mathbf{v}_k + \bar{\mathbf{a}} \Delta t$$

位置预积分

$$\Delta \mathbf{p}_{k+1} = \Delta \mathbf{p}_k + \Delta \mathbf{v}_k \Delta t + \frac{1}{2} \bar{\mathbf{a}} \Delta t^2$$

其中 $\bar{\mathbf{a}}$是两时刻加速度的平均：

$$\bar{\mathbf{a}} = \frac{1}{2} \left[ \mathbf{R}_k (\hat{\mathbf{a}}_k - \mathbf{b}_a) + \mathbf{R}_{k+1} (\hat{\mathbf{a}}_{k+1} - \mathbf{b}_a) \right]$$

这个过程产生了“改变在该时间区间内产生了什么样的变化”的数据，比如旋转了的角度（四元数）、变化了的速度、变化了的位置等。上述式子表达了一次预积分中，所有IMU信息是怎么叠加在初始数值上，然后得到这一边的总积分数量的。注意到这里就采用了中值法进行积分。

事实上，预积分不止产生这些记录状态变化的量，还有这些量的协方差以及偏差雅可比矩阵等。

预积分量与状态的关系

预积分量本身不依赖于状态（位置、速度、旋转、重力），只依赖于 IMU 测量和偏差。

状态递推（用预积分量表示）：

$$\mathbf{R}_j^w = \mathbf{R}_i^w \Delta \mathbf{R}_{ij}$$

$$\mathbf{v}_j^w = \mathbf{v}_i^w - \mathbf{g}^w \Delta t + \mathbf{R}_i^w \Delta \mathbf{v}_{ij}$$

$$\mathbf{p}_j^w = \mathbf{p}_i^w + \mathbf{v}_i^w \Delta t - \frac{1}{2} \mathbf{g}^w \Delta t^2 + \mathbf{R}_i^w \Delta \mathbf{p}_{ij}$$

关键：预积分量只与 IMU 测量和偏差有关。当偏差更新时，只需对预积分量做一阶修正，不需要重新积分。

预积分对偏差的雅可比矩阵

问题：优化过程中，偏差 bg,babg,ba 会不断更新。如果每次更新都重新积分，计算量巨大。

解决方案：预积分量对偏差做一阶泰勒展开。

$$\Delta \mathbf{R}_{ij} (\mathbf{b}_g + \delta \mathbf{b}_g) \approx \Delta \mathbf{R}_{ij} (\mathbf{b}_g) \cdot \text{Exp}\left( \frac{\partial \Delta \mathbf{R}_{ij}}{\partial \mathbf{b}_g} \delta \mathbf{b}_g \right)$$

$$\Delta\mathbf{v}_{ij}(\mathbf{b}_g+\delta\mathbf{b}_g,\ \mathbf{b}_a+\delta\mathbf{b}_a)\approx\hat{\Delta\mathbf{v}}_{ij}+\frac{\partial\Delta\mathbf{v}_{ij}}{\partial\mathbf{b}_g}\delta\mathbf{b}_g+\frac{\partial\Delta\mathbf{v}_{ij}}{\partial\mathbf{b}_a}\delta\mathbf{b}_a$$

$$\Delta\mathbf{p}_{ij}(\mathbf{b}_g+\delta\mathbf{b}_g,\ \mathbf{b}_a+\delta\mathbf{b}_a)\approx\hat{\Delta\mathbf{p}}_{ij}+\frac{\partial\Delta\mathbf{p}_{ij}}{\partial\mathbf{b}_g}\delta\mathbf{b}_g+\frac{\partial\Delta\mathbf{p}_{ij}}{\partial\mathbf{b}_a}\delta\mathbf{b}_a$$

带上标的 $\hat{\Delta\mathbf{R}},\hat{\Delta\mathbf{v}},\hat{\Delta\mathbf{p}}$ 表示积分时用旧零偏算出来、存下来的值。

这些雅可比在代码里是什么

「预积分对偏差的雅可比」不是一个孤立的公式，它是 $\texttt{pre.J}$ 这个 15×15 矩阵里对应的块，事实上，就像之前提到的那样，预积分产物的误差仅仅测量误差也就是零偏相关，所以我们只用关注所有状态对于零偏的变化情况即可。这个雅可比矩阵就表示所有变量对于零偏微小变化的变化，用来衡量零偏在每次新估计被更新之后对之前的状态会产生什么影响，直接使用可以避免每次都求新的。误差状态的排列是：

$$\delta\mathbf{x}=\big[\underbrace{\delta\mathbf{p}}_{0:3},\ \underbrace{\delta\boldsymbol\theta}_{3:6},\ \underbrace{\delta\mathbf{v}}_{6:9},\ \underbrace{\delta\mathbf{b}_a}_{9:12},\ \underbrace{\delta\mathbf{b}_g}_{12:15}\big]$$

代码里 $\texttt{J}$ 初始为单位阵，每一步 $\texttt{J}=\texttt{F}\cdot\texttt{J}$ 累积。因为零偏没有动力学（$\partial\mathbf{b}/\partial\mathbf{b}=\mathbf{I}$），所以 $\texttt{J}$ 里对应偏差的那几列，直接就是「预积分量对偏差的雅可比」：

代码里的块	数学含义	量级
$\texttt{J[3:6,12:15]}$	$\partial\delta\boldsymbol\theta/\partial\mathbf{b}_g$	$3.0\times10^{-2}$ ★
$\texttt{J[6:9,9:12]}$	$\partial\Delta\mathbf{v}/\partial\mathbf{b}_a$	$3.0\times10^{-2}$ ★
$\texttt{J[6:9,12:15]}$	$\partial\Delta\mathbf{v}/\partial\mathbf{b}_g$	$4.4\times10^{-3}$
$\texttt{J[0:3,9:12]}$	$\partial\Delta\mathbf{p}/\partial\mathbf{b}_a$	$4.5\times10^{-4}$
$\texttt{J[0:3,12:15]}$	$\partial\Delta\mathbf{p}/\partial\mathbf{b}_g$	$4.6\times10^{-5}$

F表示状态转移矩阵，是由物理规律决定的状态变化关系，乘以前一个状态的一些数据，就能够利用之前的数据进行迭代，得到新的状态矩阵。

而这里$\texttt{F}$ 里对应的块就是单步增量（源码 $\texttt{ref\_imu\_math.hpp}$ 的 $\texttt{push()}$）：

$$\texttt{F}(3{:}6,\,12{:}15)=-\mathbf{I}\,dt\qquad\text{（零偏让姿态少转 }\mathbf{b}\,dt\text{）}$$

$$\texttt{F}(6{:}9,\,9{:}12)=-\tfrac12(\mathbf{R}_0+\mathbf{R}_1)\,dt\qquad\text{（中值法的两端平均）}$$

$$\texttt{F}(6{:}9,\,12{:}15)=\tfrac12\mathbf{R}_1\mathbf{A}_1\,dt^2\qquad\text{（间接：零偏→姿态→加速度方向）}$$

$$\texttt{F}(0{:}3,\,9{:}12)=-\tfrac14(\mathbf{R}_0+\mathbf{R}_1)\,dt^2\qquad\text{（二次积分，多一个 }dt\text{）}$$

$$\texttt{F}(0{:}3,\,12{:}15)=\tfrac14\mathbf{R}_1\mathbf{A}_1\,dt^3\qquad\text{（间接 + 二次积分，两个 }dt\text{）}$$

一个漂亮的规律：每一次积分多一个 $dt$。所以 $\partial\Delta\mathbf{p}/\partial\mathbf{b}_g$ 最小（$10^{-5}$）——它经历了三次时间累积。

累积 $N$ 步后（姿态近似不变时）：

$$\frac{\partial\delta\boldsymbol\theta}{\partial\mathbf{b}_g}\approx-\mathbf{I}\cdot(N\Delta t),\qquad \frac{\partial\Delta\mathbf{v}}{\partial\mathbf{b}_a}\approx-\mathbf{I}\cdot(N\Delta t)$$

物理含义：约等于负的总时长——「零偏每多 1 rad/s，姿态就多转错 (总时长) 弧度」。

数值验证：用 3 步 × $dt=0.01$ 实测，$\texttt{J[3:6,12:15]}$ 的对角元 = $-0.03000$，正好等于 $-(3\times0.01)$。与直接数值微分（扰动 $\mathbf{b}_g$ 看预积分量怎么变）对比，相对误差 $10^{-9}$ 量级。

这套雅可比怎么被用上

用途一：修残差（$\texttt{ref\_imu\_math.hpp}$）——零偏更新后不用重新积分：

$$\texttt{dp}=\texttt{pre.p}+\texttt{pre.J}(0{:}3,9{:}12)\cdot\texttt{dba}+\texttt{pre.J}(0{:}3,12{:}15)\cdot\texttt{dbg}$$

用途二：当原料（$\texttt{ref\_imu\_jacobians.hpp}$）——残差对状态的雅可比里，对零偏的那几列直接取自它：

$$\texttt{out.vb\_i}(0{:}3,3{:}6)=-\texttt{pre.J}(0{:}3,9{:}12)$$

$$\texttt{out.vb\_i}(6{:}9,3{:}6)=-\texttt{pre.J}(6{:}9,9{:}12)$$

IMU 残差

优化时，IMU 残差是“预积分预测”与“状态推算”之差。

旋转残差：

$$\mathbf{r}_{\Delta \mathbf{R}} = \text{Log}\left( \Delta \mathbf{R}_{ij}^T \mathbf{R}_i^{wT} \mathbf{R}_j^w \right)$$

速度残差：

$$\mathbf{r}_{\Delta \mathbf{v}} = \mathbf{R}_i^{wT} \left( \mathbf{v}_j^w - \mathbf{v}_i^w + \mathbf{g}^w \Delta t \right) - \Delta \mathbf{v}_{ij}$$

位置残差：

$$\mathbf{r}_{\Delta \mathbf{p}} = \mathbf{R}_i^{wT} \left( \mathbf{p}_j^w - \mathbf{p}_i^w - \mathbf{v}_i^w \Delta t + \frac{1}{2} \mathbf{g}^w \Delta t^2 \right) - \Delta \mathbf{p}_{ij}$$

偏差残差：

$$\mathbf{r}_{\mathbf{b}_g} = \mathbf{b}_{g,j} - \mathbf{b}_{g,i}$$

$$\mathbf{r}_{\mathbf{b}_a} = \mathbf{b}_{a,j} - \mathbf{b}_{a,i}$$

这些残差构成了 VIO 中 IMU 约束的核心。

推荐配合阅读：整体流程：从 IMU 样本到状态估计
那篇讲的是整条链怎么串起来——预积分、缓存、优化三层如何分工，以及几个常见误解的逐句核对。如果对「哪一步在算什么、估计到底发生在哪里」还有疑问，先看那篇。

本节目录

整体流程：从 IMU 样本到状态估计 — 一句话记住：预积分是「算」，不是「估」。它把 100 个 IMU 样本压缩成一个约束；真正的估计发生在窗口优化层面，不在…