IMU惯导与里程计组合导航：轮式平台定位的工程实践

简介：针对车载惯性/卫星组合导航实验，该压缩包提供完整MATLAB源代码与数据处理流程，适合惯性导航、组合导航、里程计融合方向的科研人员、高年级本科生或研究生参考。实验设计为车辆先静止晃动、后行驶的场景，采集IMU、里程计与GPS数据；代码利用前2分钟粗对准、后8分钟精对准，分别输出纯惯性、惯性/GPS、惯性/里程计三种导航结果，便于横向对比不同传感器融合策略。压缩包共16个文件，全部为m脚本/函数，大小仅12KB，涵盖主控制流、惯性导航解算、粗/精对准、姿态矩阵与欧拉角/四元数相互转换、GPS与里程计卡尔曼滤波修正、参数初始化、数据预处理等功能模块，结构紧凑清晰。目前已有729人学习下载，适合需要快速掌握组合导航对准流程、滤波实现与多方案对比分析的读者直接研读与复现。

1. exp3.zip 里的 IMU 惯导里程计到底是什么：轮式平台的定位短板与补法

拿到一个名为「exp3.zip_IMU_惯导里程计_组合导航」的压缩包时，第一反应通常是：里面装的是一套把 IMU 和轮式里程计融合起来的定位方案。这类实验包在轮式机器人和低速无人平台上非常常见，解决的核心痛点是——轮式里程计在打滑、颠簸、爬坡时输出的位置会漂得离谱，而 IMU 惯导在短时间内的姿态变化又是可信的，两者一组合，短时用惯导撑精度，长时用里程计拉回来，整体定位的连续性和鲁棒性都能上一个台阶。这篇笔记就沿着这条链路拆开讲：从航位推算的基本模型，到误差状态卡尔曼滤波的工程实现，再到标定、时间同步和那些让你翻车的细节。

这套东西适合谁？手里有 Differential 底盘或 Ackermann 底盘、想让小车在室内或园区里稳定跑出厘米级到分米级轨迹的人；或者在校学生拿到了 exp3.zip 但对着数据不知道从哪一帧开始处理的。下面从原理开始，逐步把手上的 IMU 和编码器数据变成一条平滑的低漂移轨迹。

2. IMU 惯导与轮式里程计的组合逻辑：从航位推算到误差状态卡尔曼滤波

2.1 为什么先做航位推算而不是直接上组合导航

很多同学拿到 exp3.zip 之后，第一件事是打开数据文件看格式，然后急着写卡尔曼滤波。我建议反过来：先把航位推算（DR）跑通，再谈组合。原因很简单——组合导航的观测量和状态量全都建立在航位推算的坐标系定义和运动模型之上，DR 跑不通，滤波器一定发散。

航位推算对双轮差速底盘来说，模型非常直观。设左右轮编码器测得的线速度分别为 (v_L) 和 (v_R)，轮距为 (b)，车体坐标系下的线速度 (v_x) 和角速度 (\omega_z) 为：

v_x = (v_L + v_R) / 2 omega_z = (v_R - v_L) / b

有了车体系的速度和角速度，再结合车体当前的航向角 (\theta)，就可以在世界坐标系中积分：

x_new = x_old + v_x * cos(theta) * dt y_new = y_old + v_x * sin(theta) * dt theta_new = theta_old + omega_z * dt

这段代码的坑在于轮距 (b)。我见过不少翻车案例：有人直接用底盘标称轮距，结果跑 20 米直线没问题，原地转 3 圈之后位置偏出去半米。原因是轮距标称值和实际有效值之间往往有 1% 到 2% 的误差，而这个误差在转弯时会被角速度积分持续放大。所以做 DR 的第一步，是先花十分钟把轮距标定出来——具体方法后面单独讲。

DR 跑通之后，你会立刻看到它的问题：直线行驶时轨迹还行，一旦经过瓷砖接缝、地毯边缘或者路面不平的地方，编码器会瞬间输出一个错误的增量，轨迹上立刻出现一个小鼓包。更麻烦的是，长时间运行时，编码器的尺度因子误差也慢慢积累。这就是 IMU 惯导要介入的原因——IMU 输出的加速度和角速度不依赖车轮与地面的接触，短时间内的相对变化是准的，正好可以拿来平滑和校正编码器的跳变。

2.2 误差状态卡尔曼滤波（ESKF）里的状态量和观测量怎么选

常见做法是用误差状态卡尔曼滤波（ESKF）而不是直接对全状态做 KF。两者的区别在于：ESKF 维护的是「真实状态 = 名义状态 + 误差状态」这套体系，名义状态由 IMU 递推得到，误差状态由滤波器估计。这样做的工程好处有两个：一是陀螺仪和加速度计的零偏可以放进误差状态里慢慢估，不用在递推时做复杂的补偿；二是四元数姿态的误差可以用三维小角度向量表示，避免四元数归一化的麻烦。

在 exp3.zip 这种轮式平台上，我一般把状态量设为 15 维：

名义状态 x_nom： p(3): IMU 在世界系下的位置 v(3): IMU 在世界系下的速度 q(4): IMU 在世界系下的姿态四元数 误差状态 δx： δp(3), δv(3), δθ(3): 位置、速度、姿态角误差 ba(3): 加速度计零偏 bg(3): 陀螺仪零偏

观测量的选择有讲究。轮式里程计给出的速度定义在车体系下，指向车的正前方；IMU 给出的速度和位置定义在世界系下。所以做观测方程之前，首先要明确里程计观测量是「车体系 x 方向速度」而不是「世界系速度」。很多刚上手的同学把轮速计的速度直接当世界系速度用，结果航向一偏，滤波器的观测残差就剧烈震荡，最后协方差发散。

对双轮差速底盘来说，最稳的观测是：车体系 x 方向速度（由左右轮速平均得到），以及零速假设——当两个轮速都为零且持续一小段时间时，车体系三轴速度都为零，这个观测对抑制 IMU 零漂非常管用。至于轮式里程计的姿态角，我一般不太建议直接作为观测量，因为差速底盘的角速度由轮速差推算，噪声较大，不如让陀螺仪积分配合零速修正来得干净。

2.3 松组合与紧组合的边界：什么时候松组合够用

组合导航里有个绕不开的概念：松组合和紧组合。在 exp3.zip 这类低速轮式平台上，绝大多数情况下松组合就够用了，而且实现成本低一个量级。

松组合的思路是：IMU 做递推，里程计的速度作为观测量进滤波器，两者在状态层面融合。紧组合的思路是：直接把左右轮编码器的增量/脉冲数作为原始量测进滤波器，和 IMU 的加速度、角速度在更底层融合。紧组合理论上能处理编码器部分打滑的工况，比如单侧轮悬空时，松组合会因为该轮速度归零而把轨迹拉偏，紧组合则可以通过 IMU 的约束把这个异常辨识出来。

但紧组合的工程代价不小：你要为左右轮各自建观测模型，要考虑编码器脉冲的量化误差，调试起来比松组合麻烦得多。我的判断标准是：如果你的底盘是普通的差速驱动，行驶路面是室内地板或平整园区路面，松组合 + 零速修正 + 合理的野值剔除，已经能跑到轨迹误差 < 0.5% 的水平。如果你在室外草地、碎石路或者有频繁的单轮悬空工况，才需要考虑紧组合。后面第 6 章会讲从松组合往紧组合迁移的第一个切入点。

3. 数据准备是组合导航的一半工程：时间同步、坐标系标定与零偏估计

3.1 时间同步：微秒级的对齐方法

组合导航里最常见的隐性 bug 就是时间戳没对齐。IMU 的数据频率通常是 100Hz 到 200Hz，里程计（编码器）的数据频率低一些，常见是 50Hz 到 100Hz，ROS 下的机器人还可能经过不同的驱动节点转发，两个数据流的时间戳差异可能达到几十毫秒。组合导航对时间同步的要求是硬性的：IMU 积分用的是离散增量，如果里程计观测时刻和 IMU 递推时刻差 20ms，在低速平台上可能只差几个厘米，但在高速转弯时误差会被角速度放大成明显的轨迹畸变。

处理方式分两层。第一层是在数据采集端把时间基准统一到同一台机器的同一时钟源上，比如都用 ROS 的 clock 时间。第二层是在算法端做插值对齐。最常见的做法是：当里程计观测到来时，从 IMU 缓冲区里找到最近的前后两帧，线性插值出观测时刻的 IMU 状态。我写过一个简单的同步函数：

def sync_imu_at_ts(imu_buffer, ts_target): """ 从 imu_buffer 中找出 ts_target 时刻的插值后的 IMU 测量值。 imu_buffer: 按时间戳升序排列的 IMU 帧列表，每帧为 (ts, accel(3), gyro(3)) ts_target: 里程计观测的时间戳 返回: (accel_interp(3), gyro_interp(3)) """ # 二分查找 ts_target 位于 buffer 中的哪两个相邻帧之间 left = 0 right = len(imu_buffer) - 1 while right - left > 1: mid = (left + right) // 2 if imu_buffer[mid][0] <= ts_target: left = mid else: right = mid ts_left, accel_left, gyro_left = imu_buffer[left] ts_right, accel_right, gyro_right = imu_buffer[right] # 线性插值权重由时间差决定 dt = ts_right - ts_left if dt < 1e-9: return accel_left, gyro_left w = (ts_target - ts_left) / dt accel_interp = accel_left * (1 - w) + accel_right * w gyro_interp = gyro_left * (1 - w) + gyro_right * w return accel_interp, gyro_interp

逻辑说明：这个函数本质上是在做分段线性插值，把 IMU 离散测量流变成连续的时间函数，再在任意时刻取值。二分查找保证实时性，避免了每次线性扫描的浪费。参数说明：如果 IMU 频率是 200Hz，相邻两帧时间差约 5ms，插值误差远小于 IMU 噪声，完全够用。如果 IMU 频率低于 50Hz，建议换用更高阶插值，否则插值本身会成为噪声源。另外要注意边界条件——如果 ts_target 落在 IMU 缓冲区最早的帧之前或最晚的帧之后，说明数据同步有问题，应该直接丢弃该观测而不是外推，外推会引入几倍于正常噪声的误差。

3.2 轮速计的标定：有效半径与安装角

编码器输出的原始数据通常是脉冲数或者轮速 m/s，后者已经做了轮径换算。但这里的轮径是「名义轮径」，实际中的有效轮径会受轮胎气压、负载形变影响，可能与标称值差 1% 以上。对组合导航来说，这个误差会直接转化为速度观测的偏差，最终体现为轨迹的尺度误差——直线跑 100 米，偏出去 0.5 米，可能就是轮径标定有 0.5% 的偏差。

标定方法很简单，选一段已知长度的直线，让车匀速通过，记录编码器累积的里程，然后用已知距离反推实际有效轮径：

def calibrate_wheel_radius(known_distance_m, pulse_total, pulses_per_rev, gear_ratio): """ 标定有效轮半径。 known_distance_m: 标定直线段的真实长度（米），建议用卷尺量 10 米以上 pulse_total: 通过该直线段期间编码器累计脉冲数 pulses_per_rev: 电机编码器每圈脉冲数（含倍频后） gear_ratio: 电机到车轮的减速比（>1 表示减速） 返回: 有效轮半径（米） """ wheel_revs = pulse_total / (pulses_per_rev * gear_ratio) # 车轮实际转过的圈数 wheel_radius_effective = known_distance_m / (2 * 3.1415926535 * wheel_revs) return wheel_radius_effective

逻辑说明：核心思想是「用轮子实际走过的路程反推半径」。这里的关键是确保直线行驶时两个轮的累计脉冲数差不多，如果差距超过 2%，说明底盘本身有跑偏问题（可能是电机不一致或机械装配），先解决跑偏再标定。参数说明：标定距离越远，标定结果越稳定，10 米距离可以做到半径精度到 0.1%；如果场地只有 3 米，建议至少往返跑 3 次取平均。这个标定值后续就作为轮速换算的标准，不要再变，除非换轮胎或者明显调整胎压。

3.3 IMU 零偏与 Allan 方差：别让陀螺仪「带病上岗」

IMU 数据进来之后，最忌讳的就是直接拿原始加速度和角速度做积分。消费级 MEMS IMU 的陀螺仪零偏通常在几十度每小时到几度每秒之间，加速度计零偏也有几十毫 g 水平，直接积分的话，几秒钟姿态就歪了。所以在做组合导航之前，必须先做零偏估计。

工程上最简单的做法是：把设备静止放置，采集 60 秒到 120 秒的 IMU 数据，然后取均值作为零偏，这段静止数据同时可以用来估算加速度计的噪声方差（Allan 方差的简化版）。代码实现很简单：

def estimate_static_bias(accel_data, gyro_data, sample_rate_hz, ignore_start_sec=5.0): """ 利用静止数据估计加速度计和陀螺仪的零偏以及噪声方差。 accel_data/gyro_data: N x 3 的数组，单位分别为 m/s^2 和 rad/s sample_rate_hz: IMU 采样率 ignore_start_sec: 丢弃开始几秒数据，避免上电瞬间的扰动 返回: accel_bias(3), gyro_bias(3), accel_noise_var(3), gyro_noise_var(3) """ import numpy as np start_idx = int(ignore_start_sec * sample_rate_hz) accel_static = accel_data[start_idx:] gyro_static = gyro_data[start_idx:] accel_bias = np.mean(accel_static, axis=0) gyro_bias = np.mean(gyro_static, axis=0) # 零偏补偿后的残差，其标准差反映了白噪声水平 accel_noise_var = np.var(accel_static - accel_bias, axis=0) gyro_noise_var = np.var(gyro_static - gyro_bias, axis=0) return accel_bias, gyro_bias, accel_noise_var, gyro_noise_var

逻辑说明：静止时真实加速度为重力（约 9.8 m/s²），但这里没有进行水平校正，所以 accel_bias 会包含重力分量，这没关系——后续 ESKF 的初始对准会处理倾斜问题。这里的噪声方差会作为后面滤波器观测噪声协方差 R 的底数。参数说明：静止时间越长，均值估计越准，但 120 秒之后收益递减；如果环境有振动源（比如旁边有冰箱压缩机），建议换到安静的地方采集。还有一点：陀螺仪零偏会随温度漂移，开机后前 5 分钟变化最明显，所以这个零偏估计应该在热机之后再做，或者配合后续滤波器的在线零偏估计来补偿。

4. 用 EKF 把 IMU 和里程计组合起来：核心代码与参数调节

4.1 系统方程：IMU 递推 + 里程计约束

ESKF 的预测步用 IMU 测量值递推名义状态，误差状态则由线性化状态转移矩阵更新。在轮式平台上，IMU 通常安装位置与车体中心不重合，因此模型里要加入杆臂（lever arm）补偿。设 IMU 相对车体中心的平移为 (l)（在车体系下），那么车体速度转成 IMU 速度时要做一次叉乘修正，这在代码实现里容易漏掉。下面是一个精简的预测步实现：

def eskf_predict(imu_acc, imu_gyro, dt, state): """ 名义状态递推。state 包含 p, v, q（四元数 w,x,y,z 顺序）。 imu_acc: 加速度计读数（含重力） imu_gyro: 陀螺仪读数 dt: 积分步长，必须与 IMU 采样间隔一致 返回: 更新后的名义状态 """ import numpy as np from scipy.spatial.transform import Rotation as R p, v, q = state['p'], state['v'], state['q'] # 当前姿态旋转矩阵 Cbn = R.from_quat(q).as_matrix() # 加速度去除重力，转换到世界系 acc_world = Cbn @ (imu_acc - state['acc_bias']) + np.array([0, 0, -9.8]) # 陀螺仪补偿零偏 gyro_comp = imu_gyro - state['gyro_bias'] # 四元数微分更新（一阶龙格库塔在 dt 小于 10ms 时可接受） omega = gyro_comp omega_skew = np.array([ [0, -omega[2], omega[1]], [omega[2], 0, -omega[0]], [-omega[1], omega[0], 0] ]) dq = np.zeros(4) dq[0] = 1 dq[1:] = 0.5 * omega * dt q_new = R.from_quat(q).apply(R.from_quat(dq).as_matrix()) # 简化示意 # 更规范写法：q_new = quat_multiply(q, dq_normalized) v_new = v + acc_world * dt p_new = p + v * dt + 0.5 * acc_world * dt * dt return {'p': p_new, 'v': v_new, 'q': q_new, 'acc_bias': state['acc_bias'], 'gyro_bias': state['gyro_bias']}

逻辑说明：这个预测步把加速度计读数变换到世界系后减重力，再进行速度和位置的积分。关键点有两个：一是重力补偿必须用当地重力矢量，如果平台在二楼或地下室，海拔变化对重力的影响虽小，但方向一定要对准世界系 Z 轴；二是四元数更新前必须归一化，否则多次迭代后四元数模长漂移导致姿态错误。参数说明：dt 必须严格使用 IMU 数据帧之间的实际时间差，不能取固定值，因为某些采集系统的 IMU 帧间隔本身有抖动，固定 dt 会让积分产生随机游走误差。

4.2 观测方程：速度观测与位置观测的两种接法

里程计观测最关键的一点是：轮速计给出的速度定义在车体坐标系中，通常只有 x 方向有值。因此观测方程里的测量矩阵 H 需要做坐标变换：把车体系 x 方向的速度投影到世界系，再与状态预测值做差。同时要考虑杆臂，即 IMU 安装位置与车体中心之间的偏移，因为 IMU 测到的速度包含了车体旋转带来的附加速度。

一般我会写两种观测：速度观测和零速观测。速度观测用于正常行驶；零速观测在检测到车停止时给三轴速度都赋零值。观测更新代码：

def eskf_update_vel(state, P, v_obs_body, R_vel, lever_arm, Cbn): """ 轮速计速度观测更新。 v_obs_body: 车体系下的速度观测（通常形如 [vx, 0, 0]） R_vel: 速度观测噪声协方差（3x3 矩阵） lever_arm: IMU 相对车体中心的偏移（车体系） """ import numpy as np # 把 IMU 速度转换到车体系需要扣除杆臂影响 # 车体系下 IMU 速度 = v_body + omega_body x lever_arm # 但我们通常不知道 v_body，所以先把状态里的 IMU 速度转世界系 v_world = state['v'] # 从世界系转到车体系：v_body = Cbn^T * v_world - omega x lever_arm omega_body = state['gyro'] # 当前角速度（车体系） v_pred_body = Cbn.T @ v_world - np.cross(omega_body, lever_arm) # 新息（innovation）就是预测与观测的速度差 innovation = v_obs_body - v_pred_body # H 矩阵是 v_body 对误差状态的雅可比，这里简化处理： # 只取速度相关部分，实际实现里要对 δv 部分取 Cbn^T，对 δθ 部分取 -Cbn^T * skew(v_world) H = np.zeros((3, 15)) H[:, 3:6] = Cbn.T # 对应 δv # 省略姿态误差项的推导，正常需要处理 δθ 对 v_body 的影响 # 卡尔曼增益和协方差更新 S = H @ P @ H.T + R_vel K = P @ H.T @ np.linalg.inv(S) dx = K @ innovation # 状态修正：把误差状态叠加到名义状态 return dx, K, innovation

逻辑说明：这个代码里最反直觉的就是 H 矩阵的前两行可能接近零——因为差速底盘侧向速度几乎为零，但轮速计观测里没有给侧向速度的约束，只有前向。实际使用中很多实现会把观测降到一维，只取 vx 方向，这样 R 矩阵也变成标量，调试简单很多。我这里保留三维是为了给你展示杆臂补偿的位置。参数说明：R_vel 的取值直接决定了滤波器的时间常数——R 小代表相信轮速计，滤波结果更贴编码器轨迹，但在打滑时会被带偏；R 大会更相信 IMU 积分，轨迹平滑但长时漂移会增大。经验值：室内地板 R_vel 取 (0.01 m/s)² 到 (0.03 m/s)² 之间，室外粗糙路面取 (0.05 m/s)² 以上。

4.3 协方差初值怎么设：R 和 Q 的「手感」调试

很多同学在 exp3.zip 上跑出来的轨迹要么完全贴着轮速计（说明滤波器没起作用），要么乱飘（说明 Q 太大或 R 太小）。协方差矩阵需要根据物理含义来设，而不是随便填数字。

逻辑说明：Q 矩阵对角线值反映的是模型误差随时间累积的速率，如果你做静止实验而滤波器的位置协方差快速增长，说明 acc_noise_var 设大了。R 矩阵是观测噪声，可以先从 Allan 方差估计的白噪声底数出发，再现场调节。参数说明：调参顺序有个惯用套路——先调陀螺仪噪声和零偏游走，让静止姿态不发散；再调轮速计 R，让直线轨迹的横向误差在 0.1 米以内；最后调加速度计相关项，处理转弯和加减速的动态工况。不要在第一步就同时动四五个参数，那样你根本分不清是谁把轨迹搞坏的。

5. 组合导航避坑指南：从数据翻车到滤波发散的五条血泪经验

5.1 现象一：零速的时候轮速计还在「走路」

现象：车明明刹停了，但融合后的轨迹还在缓慢向前漂，速度观测却显示车在动。原因：编码器在停车瞬间可能因为机械回程间隙输出少量脉冲，或者轮速换算的阈值太低，把微小的抖动当真了。解决：加零速检测——左右轮速都低于阈值且持续 50ms 以上，强制把观测切换为零速。阈值一般取 0.005 m/s，持续时长取 3 到 5 个轮速计采样周期；切零速的同时把侧向速度也约束为零，这对抑制 IMU 水平零漂效果明显。

5.2 现象二：拐弯时组合结果比纯里程计还差

现象：直线融合得很漂亮，但一个急转弯之后，轨迹半径明显比实际道路小，或者转弯结束后航向角残留好几度偏差。原因：轮距 (b) 没标准。轮距误差不影响直线行驶，但转弯时角速度计算直接除以轮距，轮距小了角速度就偏大，轨迹会往里收。解决：先回到第 3 章的标定方法，但标定场景从直线换成圆形——让车原地转 N 圈，对比编码器累计转角与陀螺仪累计转角，用比值修正轮距。经验公式：轮距修正量 ≈ (陀螺仪累计转角 / 编码器累计转角 - 1) × 当前轮距。另外还要检查转弯时同一时刻左右轮速度是否出现过大的跳变，如果是，把该帧观测直接当作野值剔除，否则滤波器会被带出一个错误的弧线。

5.3 现象三：跑几分钟后位置误差突然「跳变」

现象：前 3 分钟轨迹都贴着真值，第 4 分钟突然一次性偏出去 0.3 米，之后又慢慢融合回来。原因：这个跳变通常不是滤波器的问题，而是数据流里出现了重复的时间戳或者丢帧——某一段 IMU 数据被重复送进积分，或者时间同步函数在缓冲区边界做了错误的外推，导致状态预测里多了一段虚拟运动。解决：检查时间戳的单调性，写一段校验逻辑，把时间戳差超过 2 倍采样周期的数据帧标记为异常；异常帧直接丢弃，而不是靠滤波器去「消化」这个错误。我一般会用下面的方式做数据健康检查：

# 检查 IMU 时间戳的间隔分布，正常应集中在采样周期附近 awk 'NR>1{print $1-prev} {prev=$1}' imu_timestamps.txt | sort -n | uniq -c | sort -rn | head -20

逻辑说明：这条命令统计相邻时间戳差的分布，如果出现大量「0」或「2倍采样周期」的间隔，说明采集节点有重复发送或丢帧。注意：如果峰值出现在 0 附近，优先检查采集程序是不是在多线程里对同一个时间戳重复发送；如果出现在 2 倍周期，检查驱动是否因为回调阻塞跳过了一帧。参数说明：对于 100Hz 的 IMU，正常间隔应集中在 0.0099 到 0.0101 秒之间，超过 0.015 秒的帧就要打上可疑标记。

5.4 现象四：时间戳对不齐导致的高频抖动

现象：融合轨迹整体趋势正确，但高频段有肉眼可见的锯齿，像是有 5Hz 左右的抖动叠加在轨迹上。原因：IMU 和轮速计的时间戳各有自己的时钟源，两者之间的固定偏置没有被对齐。解决：用互相关方法估计时间偏置——先把两段数据保存成数组，对轮速计做不同时移，计算与 IMU 数据（比如角速度 Z 轴）的相关系数，相关系数最大的时移就是偏置。这个方法我之前在某次模拟项目X里用过，效果很好：找到偏置量后，在后续处理里把轮速计的时间戳统一加上这个偏置，再进同步函数。注意，偏置不会随运行时间变化太大，开机后标定一次，运行中就不需要反复估了。

5.5 现象五：协方差疯狂膨胀，滤波器失去约束

现象：滤波器的位置协方差对角元素随运行时间持续增长，几个小时后数值大得离谱，后续观测进来几乎对状态不起作用。原因：这是 ESKF 的经典问题——预测步的 Q 矩阵设置过大，或观测更新太稀疏，导致协方差矩阵趋于稳态时数值很大。另外，协方差在长时间运行后可能失去对称正定性，引发数值发散，这在 float32 环境下更容易出现。解决：一是调小 Q 中的加速计随机游走项，二是每 N 帧做一次协方差对称化手术——P = (P + P.T) / 2，并且检查对角线是否有负值，如果有就用上一次的 P 替换。关键教训：协方差矩阵不是越大越好，它代表的是「滤波器对自己状态的信任度」，如果信任度衡量的尺度与实际误差不匹配，状态修正会出现超调或迟滞。

6. 紧组合尝试与可观测性验证：把组合导航往前再推一步

走到这一步，松组合的精度你已经榨得差不多了，接下来可以考虑两个方向的进阶：一是把松组合升级成紧组合，二是用可观测性分析验证你的滤波器是否「真的在干活」。

紧组合的第一个改动点，是把左右轮编码器的脉冲增量当作原始观测，而不是先换算成速度再观测。这个改动的意义在于：编码器脉冲的量化误差可以被显式建模，而且当一个轮子打滑时，脉冲增量与 IMU 推算的位移会产生矛盾，滤波器能识别出并降低该侧观测的权重。工程实现从松组合迁移，核心改动是观测方程从速度观测变成「左右轮位移增量观测」，H 矩阵需要增加左右轮轮距相关项，R 矩阵则要按每个轮子的实际打滑程度做自适应。

可观测性验证是一个性价比很高的动作，不用改代码，只要观察滤波器的「新息序列」——也就是观测值减预测值。如果滤波器设计合理，新息应该是一个白噪声序列；如果新息序列出现明显的非零均值或周期性震荡，说明模型有偏差，比如杆臂没补偿、轮距标定错、IMU 零偏初值不对。我通常会保存里程计观测的新息并做均值和自相关分析：均值大于 0.02 m/s 说明存在固定偏差；自相关在滞后 1 和 2 处显著非零说明模型阶次不够或者时间同步还有残差。

做完紧组合和可观测性验证之后，整个 IMU 惯导里程计组合导航的调试闭环就完整了。回看整个 exp3.zip 的实现过程，我自己的教训是：永远不要一开始就追求滤波器公式的漂亮，先把数据清洗、时间同步、参数标定这三件「脏活」做扎实，滤波器本身反而不太容易出问题。某次在某公司的项目里，同事调试了半个月的滤波发散，最后发现是轮距标称值错了 4 毫米——这个教训我记到现在。希望你在跑自己的数据时，也能从最小可复现实验开始，一步步验证每个环节的输入输出，别让组合导航变成不敢碰的黑匣子。希望帮到你。

本文还有配套的精品资源，点击获取