从零手写VIO：imu.cpp核心逻辑与IMU预积分实现解析

1. 为什么VIO的地基一定是imu.cpp

先说结论：视觉惯性里程计里，IMU这路数据是整个状态估计系统的"先验骨架"，而视觉是"修正器"。你可以在一个纯视觉里程计里把帧间关系理得很清楚，但一旦相机快速运动、画面出现运动模糊、或者面对一堵纯色白墙，纯视觉就废了。这时候IMU测量值不会中断，始终能给出短时间内的位姿递推。VIO之所以比单纯视觉稳那么多，关键就在IMU预测+视觉观测的紧耦合。

这篇笔记讲的，是从零手写VIO的第二节——imu.cpp代码解析。很多人第一次接触VIO源码会直接扑向优化器、滑动窗口、边缘化，结果读得一头雾水。我建议的顺序是：先把IMU这根线彻底读懂，因为后面所有观测残差、预积分因子、协方差传播，全部是从这个文件延伸出来的。imu.cpp如果理解不透，后面哪怕把代码跑通了，也只是"会run"，不是"会写"。

我在梳理这份代码时，给自己提了几个问题：

• IMU测的到底是什么物理量？加速度计和陀螺仪各自的噪声是怎么进入模型的？
• 从连续时间运动学方程到离散时间更新，代码里用的积分方式是什么？
• 除了预测位姿，IMU还要给后端提供什么信息？为什么协方差矩阵也在这里算？
• IMU初始化时静止放置几十帧，到底是要算什么东西？

这篇文章就把imu.cpp里的主线逻辑拆开讲，配合自己推导的数学步骤和实际踩过的坑。适合有一定ROS和Eigen基础、正准备手写VIO或正在啃VIO源码的人。如果你是纯小白，也能跟着理解IMU做状态递推的全过程，只是可能需要把矩阵基础补一补。

2. IMU测量模型：读代码前必须先理清的物理含义

很多人直接把代码上的acc_meas和gyro_meas当作"真值"，这是一个很危险的习惯。IMU输出的原始数据并不是理想的加速度和角速度，里面带着两项"脏东西"：零偏和噪声。在代码层面如果忽略这两者，那你的预积分和后面的视觉融合全部会偏。

2.1 加速度计的"比力"概念

加速度计测量的其实不是纯加速度，而是"比力"（specific force）。说白了，它测量的是物体所受的非引力加速度。也就是说，当IMU静止放在桌面上时，加速度计的读数不是0，而是向上的1个g（约9.81 m/s²），用来抵消重力。很多新手在这里卡住：静止时明明没有运动，为什么加速度计的z轴有一大堆读数？

因为地面给了物体一个向上的支撑力，加速度计感受到的是支撑力带来的加速度，方向和重力相反。所以当我们用IMU数据反推运动状态时，必须先减去重力加速度向量，剩下的才是真实运动加速度。VIO系统里最常见的坐标系约定是：IMU静止时，测得加速度的模长约等于重力加速度g，方向指向重力相反方向。这也是后续初始化时用来估计重力方向的核心依据。

陀螺仪相对好理解，它测的是IMU坐标系相对于惯性系的角速度，以角速度矢量的形式输出，单位通常是rad/s。陀螺仪同样有零偏和噪声，但它在短时间的漂移相对可预测，因为它积累的是角度，不像加速度计还有重力混合的问题。

2.2 噪声模型：从传感器出厂参数到代码里的高斯白噪声

代码里IMU参数通常来自标定文件或者仿真配置文件，标准噪声模型写成这样：

• 陀螺仪测量值 = 真实角速度 + 零偏 + 高斯白噪声
• 加速度计测量值 = 真实比力 + 零偏 + 高斯白噪声

高斯白噪声的离散化参数如何对应到连续时间，是很多人读源码时最困惑的地方。很多代码里用imu_gyro_noise、imu_acc_noise这样的变量，它们是连续时间噪声密度，单位是rad/s/√Hz和m/s²/√Hz。在离散时间中，采样间隔dt下，这个噪声对应的标准差要除以√dt。这一点非常关键，直接影响协方差矩阵的初值。如果这里搞错，IMU的协方差传播会整体放大或者缩小几个数量级，后端对IMU的信任程度就错了，优化结果也会怪。

2.3 IMU的坐标约定对代码的影响

另一个容易被忽略的点是坐标系约定。绝大多数VIO系统采用"右前上"或者"前上左"的IMU坐标系，而相机坐标系又是另一套。在和视觉观测融合前，必须先做好IMU和相机的外参标定，否则两方面观测对不上。imu.cpp一般不会直接处理外参，但它使用IMU测量值时，隐含假定了测量值已经投影到了IMU坐标系中，而且该坐标系与机体坐标系一致。

我把IMU测量模型的核心参数整理成了表格，方便对照代码里的变量：

这张表的作用不是让你背默认值，而是当你跑数据发现视觉和IMU融合发散时，要能判断问题出在传感器参数还是算法。我用过不少公开数据集，有些数据集的IMU噪声参数标得并不准，跑官方范例还行，换到自己采集的数据上就飘，问题往往就在这套参数没有重标定。

3. imu.cpp的文件骨架：类设计与数据流

拿到imu.cpp，第一步不是看每一个函数体，而是先理清类的成员变量和数据入口。imu.cpp的核心作用不是"保存原始IMU数据"，而是把原始数据转换成系统可用的状态递推信息。我见过有人把它当作一个简单的数据读取器，这是误解。它真正的定位是"IMU预积分器"，也就是从上一帧状态到当前帧状态，IMU贡献的那部分相对运动，包括位姿增量、速度增量以及协方差。

3.1 核心类和成员变量设计

从零手写VIO这节课里，imu.cpp对应的是一个IMU状态处理器，典型的成员包括：

• 当前位姿：旋转四元数_q和位置_p
• 当前速度：_v
• 陀螺仪零偏和加速度计零偏：_bg、_ba
• 上一帧IMU数据的时间戳和测量量
• IMU参数：噪声密度、重力加速度大小

这些成员存在的意义是有顺序的：有了上一帧时间戳，才能和当前时间戳算出dt；有了零偏，才能修正原始测量值；有了重力向量，才能从比力中剥离出真实加速度。我强烈建议你在读代码时把每个成员变量背后的物理含义写在注释旁边，这会大幅降低后续阅读难度。

另外，要留意IMU数据和图像数据是两个不同频率的流。IMU通常100Hz到200Hz，相机通常10Hz到30Hz。代码在接收新IMU帧时，通常会和上一次的IMU帧做积分；当来了一帧图像，就把这一段时间内累计的IMU增量交给优化器。imu.cpp是"按帧触发"的，而优化器是"按图像帧触发"的，看代码时不要把这两个触发时机搞混。

3.2 数据输入接口的设计思路

imu.cpp通常暴露这样的方法：

void addIMU(double t, const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr);

addIMU一进来，先判断是否已有上一帧数据。如果没有，说明这是系统启动后的第一帧IMU，只记录不积分。如果有，就进入积分流程。这个"第一帧不积分"的逻辑虽然简单，但非常容易被漏掉，我第一次写的时候没加这个判断，一开始就产生了一个无意义的巨大增量。

真正需要细看的，是积分流程里那两个因素：

1. 时间间隔dt的计算方法。是直接两帧时间戳相减，还是通过全局的时间戳队列做插值？如果IMU时间戳有跳变，直接相减会对积分产生脉冲式的干扰。
2. 测量值的零偏修正。代码里通常这样处理：

Eigen::Vector3d acc_meas_corrected = acc - _ba; Eigen::Vector3d gyr_meas_corrected = gyr - _bg;

这行代码看起来平平无奇，但它是IMU预积分的"净化"步骤。零偏如果估计得不好，所有后续的位姿预测都会叠加一个偏差，这也是为什么VIO系统普遍使用"在线零偏估计"而不是只靠出厂标定。

4. 核心积分逻辑：从连续运动学方程到中值积分

读imu.cpp代码，最核心的一段就是IMU运动学方程的离散化。连续时间的IMU运动学方程可以说是整个VIO的"总纲"：

• 旋转四元数变化率：受角速度驱动
• 速度变化率：受加速度（已减重力）驱动
• 位置变化率：等于当前速度

用公式表达就是：

• q_dot = q ⊗ [0, ω] （四元数微分）
• v_dot = a
• p_dot = v

注意这里的a是去重力后的真实加速度，是从比力中剥离重力得到的。

4.1 为什么选择中值积分而不是欧拉积分

代码里最常使用的是中值积分（mid-point integration），也就是用两个时刻的测量平均值来驱动状态更新。为什么不用更简单的欧拉积分？我用一个电梯类比来解释：欧拉积分相当于你只看电梯出发时的速度，然后一直用这个速度推算它在第10秒的位置；中值积分则取前几秒和后几秒速度的平均值，推算的位移自然更接近真实。IMU的采样频率在100Hz左右，同一个状态方程积分两次，欧拉和中值的差距虽然每一帧很小，但经过几百帧的递推，累积误差就会非常可观。

在imu.cpp里，中值积分的实现套路很固定：

double dt = t_curr - t_prev; Eigen::Vector3d acc_avg = 0.5 * (acc_prev_corrected + acc_curr_corrected); Eigen::Vector3d gyr_avg = 0.5 * (gyr_prev_corrected + gyr_curr_corrected); // 1. 用 gyr_avg 更新四元数 // 2. 在更新后的姿态坐标系下，用 acc_avg 更新速度 // 3. 用更新后的速度均值更新位置

这里每一步都有讲究。姿态更新用gyr_avg，速度更新要先把acc_avg旋转到当前姿态坐标系，位置更新则使用速度增量的中值。很多刚接触的人会产生疑问：为什么速度更新时要用"更新后"的姿态？因为加速度计的测量是在IMU机体坐标系下的，你必须先知道此刻的姿态，才能把机体坐标系的加速度转换到世界坐标系。而中值积分的思路是：先用这段时间的平均角速度算出中间时刻的姿态，再在这段中间姿态下进行加速度投影。

4.2 四元数更新的细节：一阶近似与归一化

四元数更新的典型写法如下：

Eigen::Quaterniond dq; Eigen::Vector3d axis = gyr_avg * dt; double angle = axis.norm(); if (angle > 1e-8) { axis /= angle; dq = Eigen::Quaterniond( Eigen::AngleAxisd(angle, axis) ); } else { dq.setIdentity(); } _q = (_q * dq).normalized();

这段代码背后隐藏着几个工程细节：

第一，如果角速度很小，旋转向量接近零向量，直接构造AngleAxisd会有数值不稳定的风险，所以要做阈值判断。第二，两个四元数相乘后必须归一化，四元数表示旋转本身要求模长为1，即使乘完也保证了局部近似的精度，但浮点运算会引入微小误差，长时间运行必须归一化。第三，有些代码会用四元数的一阶近似公式dq = [1, 0.5*gyr*dt]来简化计算，效果在dt很小时也能用，但为了精度稳定，我建议保留角度轴构造的方式，性能差异可以忽略。

4.3 位置与速度更新的顺序问题

位置的更新不能直接写成p += v * dt。因为v本身在dt内也在变化，所以要先更新速度，然后用更新前后速度的平均值来更新位置，这就是中值积分的体现：

Eigen::Vector3d p_pre = _p; Eigen::Vector3d v_pre = _v; // 速度增量 Eigen::Vector3d acc_world = _q * acc_avg; _v += acc_world * dt; // 位置使用速度前后均值 _p += 0.5 * (v_pre + _v) * dt;

这个顺序能保证：在恒定加速度假设下，位置的更新结果和解析解完全一致。如果你把位置更新放在速度更新前面，就会少算一半的速度变化量，在高动态场景下误差会非常明显。

5. 误差状态递推与协方差传播：为什么要在积分的同时算雅可比

IMU的积分不只是为了给一个位姿预测值。VIO后端做紧耦合优化时，需要知道"IMU预测出来的状态有多可信"，这个可信程度就是协方差矩阵。你算出来的预测位姿如果完全没有置信度信息，优化器就不知道应该信任IMU多少，融合视觉观测时权重就会错乱。

5.1 为什么要构造误差状态方程

直接对四元数求偏导非常麻烦，四元数有冗余自由度，而且必须满足单位模长约束。业界常用技巧是使用误差状态（error-state），也就是把真实状态和名义状态之间的偏差建模为一个小量，然后围绕这个小量推导线性化的递推关系。看imu.cpp里列出的雅可比方式，正是这套逻辑。

设误差状态为：

• 位置误差：δp
• 速度误差：δv
• 姿态误差：δθ （以旋转向量表示）
• 零偏误差：δbg、δba

对IMU连续运动学方程做一阶线性化，就能得到误差状态的连续时间微分方程：

δx_dot = F * δx + G * n

这里的F是状态转移矩阵，G是噪声输入矩阵，n是连续的IMU噪声。imu.cpp中最繁重的一段工作，就是把连续时间的F、G离散化，然后在此时间间隔内累积协方差。

对代码里的矩阵做这些操作时，我建议你在草稿纸上把推导走一遍，因为代码里的F = I + F*dt这种形式，实际上是一阶泰勒展开的离散化结果，对应的是：

Fd = Eigen::Matrix<double, 15, 15>::Identity() + F * dt;

5.2 雅可比和协方差更新代码的典型形态

协方差传播的核心代码长这样：

Eigen::Matrix<double, 15, 15> Fd = I_15 + F * dt; Eigen::Matrix<double, 15, 12> Gd; // 构造噪声输入矩阵 G Eigen::Matrix<double, 12, 12> Qc; // 构造连续噪声协方差矩阵 Eigen::Matrix<double, 12, 12> Qd = Qc * dt; _covariance = Fd * _covariance * Fd.transpose() + Gd * Qd * Gd.transpose();

这里有几个关键点：

第一，噪声协方差矩阵Qc的对角块对应陀螺仪噪声、加速度计噪声、陀螺仪零偏随机游走、加速度计零偏随机游走四组。它们的数值由传感器参数直接决定，但需要按照第2节提到的噪声密度/√dt方式换算。

第二，F矩阵里有一个明显的物理含义分区：位置误差受速度误差影响，速度误差受姿态误差和加速度计零偏影响，姿态误差受陀螺仪零偏影响，而零偏本身随机游走。我在读代码时把F矩阵的分块含义全部标出来，之后看任何VIO代码都顺了。

第三，不要以为协方差传播永远是对称的。由于数值舍入，长期迭代后协方差矩阵会略微失去对称性。有的代码在每次更新后会强制对称化处理。如果代码里没有这个操作，建议自己加上。

5.3 离散化方式对精度的影响

上面用的是最简单的"矩形积分"离散，适合IMU频率较高的情况。如果IMU频率较低（比如低于50Hz），这个近似可能不够，就要考虑更高精度的离散化，比如使用矩阵指数。不过在VIO的标准流程里，IMU频率普遍在100Hz以上，一阶近似完全够用，太复杂的离散化反而增加计算量，没啥必要。

6. 静止初始化：imu.cpp里的第一个隐藏关卡

很多人的困惑在于：程序起来以后，为什么要原地等待一段时间，显示"IMU初始化中"？这段时间imu.cpp在忙着算一件非常重要的事：估计重力方向，以及初始零偏的粗略值。

6.1 从静止加速度均值估计重力方向

当IMU静止时，加速度计唯一感受到的物理量就是重力（还有噪声），所以这段时间内加速度测量值的平均值，方向就是重力的反方向。代码里维护一个窗口，积累几十甚至上百帧静止IMU数据，然后：

_gravity = -acc_accumulate / acc_accumulate.norm() * G_NORM;

这里的G_NORM就是重力加速度大小，通常是9.8左右。如果IMU坐标系和世界坐标系相差一个旋转，那么这一步估计出的反向重力向量，就隐含了IMU当前姿态相对于世界坐标系的部分信息。后续首帧的姿态也可以用这个方向来确定水平面。

6.2 零偏初始化的两难问题

静止时，陀螺仪理想输出为0，所以它的初始零偏可以直接取静止数据的均值。加速度计的初始零偏比较麻烦，因为在重力存在的情况下，无法直接分离"加速度计零偏"和"重力投影"。所以大多数实现里，加速度计零偏初始化为0，等优化器在运行过程中慢慢修正。如果你在imu.cpp里看到初始化阶段只处理了陀螺仪零偏，不必惊讶，这是常规操作。

我在这里给你一个实测建议：初始化阶段一定要让IMU保持足够静止，不要手持晃动。有些人觉得"快点跑起来快点看效果"，就随便晃两下，结果重力方向估计偏了，后续所有轨迹都倾斜着飞出去。我自己调代码时浪费过半天时间，最后发现问题是初始化那段数据里有手抖。

7. 实际使用中必须避开的坑：从数据预处理到频率匹配

读完数学和代码主线，再讲几个imu.cpp在真实工程场景里容易翻车的细节。这些点很多是跑通demo之后才会遇到的，但提前知道能省很多时间。

7.1 时间戳：最不起眼却最致命的问题

IMU数据和图像数据必须时间对齐，这是老生常谈，但真正落地时还是会出问题。我记得有一次把仿真数据换成真实数据集，轨迹总是显示延迟，定位精度惨不忍睹，最后发现是IMU和相机的时间戳没有统一到同一时基。imu.cpp中时间戳的维护方式要格外小心：如果IMU时间戳有重复、乱序或者跳变，积分结果立刻变坏。你在代码里最好加上相邻帧时间差检查，dt异常大或异常小都应该打印警告。

有一个工程技巧：不要直接用两个时间戳相减的结果做积分，而要维护一个时间戳队列，每次处理前检查时间单调性。真实传感器偶尔会出现时间戳乱序或者掉帧，没有这个保护机制，系统稳定性很难保证。

7.2 IMU频率与相机频率的匹配

IMU频率越高，中值积分的精度越高，但计算量也线性增加。频率过低，预积分噪声变大，视觉与IMU的互补效果就变差。常见的动捕数据或手机IMU采集频率在100Hz到200Hz之间，VIO系统一般不需要超过400Hz，那是专用于高动态场景的特殊需求。如果你在代码里发现IMU数据频率低到30Hz以下，先不要急着调算法，优先检查传感器驱动和采集线程是否丢帧。

7.3 重力向量与世界坐标系对齐

当系统初始化完成后，世界坐标系的"水平面"应该与重力方向垂直。如果初始化没做好，整个世界坐标系是歪的，视觉轨迹虽然相对形状对，但绝对朝向全错。这会导致回环检测和新帧融合时频繁出现大的位姿跳变。

你在调imu.cpp初始化逻辑时，可以打印初始化前后重力向量的估计值，正常情况应该是[0,0,9.8]附近，具体分量取决于你的坐标系约定。如果一开始就跳出奇怪的向量，那一定还有某个坐标系转换没有做对。

7.4 数值精度带来的"又慢又歪"

还有一种隐蔽问题是计算顺序导致的精度损失。例如，在更新位置时先加大数再加小数，会损失大量有效数字。IMU数据数值上可能到几十米，而每帧位移增量只有几毫米，直接用float运算会让增量全部被舍入吞掉。VIO代码里位置、速度、协方差必须用double。我看到过有同学为了性能把double改成float，结果轨迹直接变成波浪线，这就是典型的数值精度教训。

8. 验证IMU处理是否正确的三种手段

代码写完了，如何验证自己的imu.cpp是对的？直接去跑整个VIO系统，如果失败了很难定位到IMU环节。我建议从这三个层次由浅入深地验证。

8.1 第一层：静止测试

把IMU放在桌面上，保持静止，运行预处理后的代码，观察输出的速度和位置增量。理论上速度和位置应保持为零，或仅有微小漂移。如果静止时位置飘出明显轨迹，那积分逻辑里一定有Bug，最常见的就是重力没有剥离干净，或者加速度计零偏处理错误。

8.2 第二层：直线运动测试

带着设备沿着已知方向缓慢平移一段距离，对比输出的轨迹终点和实际的距离。这段测试能验证加速度积分是否合理，速度更新顺序是否正确。这里要注意，设备必须做匀速和匀加速的混合运动，如果始终匀速，加速度积分几乎不参与，等于没测到问题。

8.3 第三层：与视觉轨迹对照

先把单目视觉跑通，得到相对轨迹，再和IMU积分得到轨迹放在一起看。两边的形状应该大致一致，只是IMU会随着时间逐渐发散。这是一个定性验证的好方法。如果两条轨迹从一开始就方向相反或者明显不重叠，说明IMU坐标系与相机坐标系的外参可能没对齐，或者重力方向估计反了。

我给了一个快速的诊断表，你可以参照它做自查：

这套验证方法不需要多复杂的装备，一张桌子、一条走廊、一把卷尺就够了，但能把imu.cpp这层做扎实。

9. 结合VIO全流程回看imu.cpp的定位

当读懂imu.cpp后再回头看整个VIO框架，你会有一个和之前完全不同的认知。IMU预积分值不是孤立的传感器数据，它要进入后端的残差模型，成为约束相邻帧相对运动的一个"测量"。这个测量的定义方式就是预积分残差：

• 相邻帧之间的相对旋转预测值与IMU积分值的残差
• 相对位置、速度预测值与IMU积分值的残差
• 零偏随机游走的约束残差

imu.cpp产生的那一堆雅可比和协方差，最终会被后端用来做加权最小二乘优化。所以你不能只把它当做一个"积分器"，它是整个紧耦合优化系统里"惯性约束"的工厂。

在从零手写VIO的进阶方向上，imu.cpp之后自然会进入视觉特征跟踪和紧耦合优化。那时你会发现，很多看起来复杂的东西，归根结底还是绕回到这个文件里定义的位姿、速度、零偏状态向量。状态向量是几维的（最常见是15维或18维，视是否加时间戳和光度参数而定），协方差矩阵长什么样，误差和雅可比怎么排布，这些都在imu.cpp这一层就定型了。

最后再分享一个个人习惯：我在阅读和手写IMU代码时，会专门建一个txt文档记录每一行核心代码对应的物理意义和公式来源。比如_q = (_q * dq).normalized()旁边写着"四元数表示姿态的乘法顺序，右乘代表在当前机体坐标系旋转"。这样的笔记在颜色渲染、视觉效果上毫无用处，但在调试和回溯时价值极高。到现在我回头给其他人讲VIO时，最常用的还是这份笔记里的图和推导，而不是堆在项目里的那一堆代码文件。写代码是一回事，把代码背后的惯性导航逻辑刻在脑里，才是真正能从零手写VIO的开始。