简介：面向车载惯性/卫星/里程计组合导航研究的MATLAB程序包，适用于测绘、自动驾驶及组合导航课程实验与课题复现，便于对比惯性导航、惯性/GPS组合导航与惯性/里程计组合导航的实现差异。实验场景为车辆先静止晃动十分钟后行驶，采集IMU、GPS及里程计数据，利用前两分钟粗对准、后八分钟精对准，可得到不同信息源下的导航定位结果。压缩包共16个文件，全部为.m脚本，约12KB，包含主程序、惯导解算、姿态转换、初始对准以及卡尔曼滤波校正GPS/里程计等模块，结构紧凑，便于直接阅读和二次开发。已有729人学习下载。通过该资源可快速搭建组合导航仿真框架，掌握粗对准与精对准流程，理解卡尔曼滤波在惯性/GPS与惯性/里程计融合中的具体用法，是车载组合导航实验的实用参考资料。
1. 一包 MATLAB 代码看懂 IMU 惯导里程计组合导航：从 exp3.zip 聊起
做车载组合导航的人大多有过这种经历：手里有 IMU 数据、里程计数据和 GPS 数据，但把三者真正融合起来时，坐标系对不上、时间对不齐、姿态更新发散，最后画出来的轨迹根本没法看。这份 exp3.zip 是一个车载惯性/卫星组合导航实验的实验包，场景设定很典型：起始时刻车辆停止，但车身一直有轻微晃动，10 分钟后车辆开始行驶，采集了 IMU、里程计和 GPS 数据。代码用前 2 分钟静止段做粗对准，再用后 8 分钟数据做精对准，分别输出纯惯性导航、惯性/GPS 组合导航、惯性/里程计组合导航三条结果线。适合正在学捷联惯导、卡尔曼滤波组合导航，或者想对比 GPS 与里程计两种量测约束效果的人。
2. 姿态链路拆解：欧拉角、方向余弦矩阵与四元数怎么互相倒腾
2.1 exp3 里的姿态转换函数族：eul2dcm、eul2qua、dcm2eul、dcm2qua、qua2dcm
打开 exp3.zip 里的文件列表，会发现里面有一套完整的姿态转换函数：eul2dcm、eul2qua、dcm2eul、dcm2qua、qua2dcm。这套函数不是摆设，它是整个捷联惯导解算的地基。捷联惯导的核心是把陀螺和加速度计输出的机体坐标系数据转换到导航坐标系，而这个转换关系就是用姿态矩阵表达的。不同阶段用的姿态表达不一样：初始对准阶段用欧拉角更直观，中间解算阶段用四元数更新更高效，输出阶段又要转回欧拉角给用户看。
我一般会先跑一遍转换函数，验证正反变换能不能还原。比如欧拉角转四元数，再做四元数转欧拉角，角度应该回到原值，这个闭环测试能提前暴露符号约定问题。exp3 里的约定是导航坐标系采用北东地（NED）还是东北天（ENU）直接影响转换公式，从代码里 initpara.m 的初值设置能看出来。常见做法是车载惯导用 NED，里程计和 GPS 也统一到这个坐标系下，这样后面组合时不用来回切。
% 欧拉角转方向余弦矩阵，再转四元数，验证闭环 roll = 0.02; pitch = -0.01; yaw = 1.2; % 单位：rad dcm = eul2dcm([roll, pitch, yaw]); qua = dcm2qua(dcm); dcm2 = qua2dcm(qua); euler_back = dcm2eul(dcm2); % 应还原为 [roll, pitch, yaw] 附近这段代码的核心在于：eul2dcm 按航向、俯仰、横滚的旋转顺序构造方向余弦矩阵，dcm2qua 把矩阵转成四元数，qua2dcm 再转回来。参数说明里最容易出问题的是旋转顺序，如果内部用的是 Z-Y-X 顺序，而你在外部按 X-Y-Z 传角，结果直接偏掉。验证闭环时，建议把 yaw 设成 60° 以上的大角度，因为小角度下各种近似都成立，误差不明显；大角度才能暴露顺序和方向符号的差异。我自己跑 exp3 时发现 dcm2eul 输出的 yaw 范围在 ±180°，如果你的后续判向逻辑需要 0~360° 的航向角，得自己做越界处理。
2.2 四元数乘法与姿态更新：qua_multiply 在解算循环里的角色
exp3 里有一个 qua_multiply.m，这个函数是四元数乘法，看似简单，却是惯导解算循环里调用频率最高的函数之一。每次拿到新的角增量，都要把角增量转成四元数，再与上一时刻的姿态四元数做乘法完成姿态更新。四元数乘法满足结合律但不满足交换律，顺序写反一次、符号差一位，姿态就飞了。
% 姿态更新核心步骤：角增量转四元数，再右乘到当前姿态 delta_q = [cos(norm(omega)*dt/2), ... % 标量部分 sin(norm(omega)*dt/2) * omega' / norm(omega)]; % 矢量部分 q_new = qua_multiply(q_old, delta_q); % 注意：先当前姿态，后增量 q_new = q_new / norm(q_new); % 归一化，防模长漂移这里的参数含义要拎清楚：omega 是陀螺输出的角速度向量（rad/s），dt 是解算步长，norm(omega)*dt 是这段时间内的总转角增量。qua_multiply 的调用顺序决定了是机体坐标系相对导航坐标系的姿态更新，如果两个参数对调，物理含义就变成了导航系相对机体系的更新，导航结果会北向和东向互换。exp3 的 ins_navi.m 里每个解算周期都做一次归一化，这是捷联惯导防发散的基本动作。四元数模长漂移是纯积分累积的必然结果，不做归一化，几轮迭代后姿态矩阵的正交性就保不住。我第一次跑这类代码时偷懒省掉了归一化，结果定位误差在几分钟内从米级涨到百米级，从那次以后我习惯在每次姿态更新后强制归一化，并且在关键节点打印四元数模长做检查。
3. 对准链路与数据预处理：粗对准、精对准和 pre_data 做了什么
3.1 粗对准与精对准：前 2 分钟静止晃动段怎么变成初始姿态
摘要里说得很清楚：前 2 分钟车辆停止但有干扰晃动，后 8 分钟车辆行驶。exp3 的对准策略是拿前 2 分钟做粗对准，后 8 分钟做精对准。粗对准的物理原理是利用静止时段加速度计测量的重力方向来确定水平姿态（横滚和俯仰），利用 GPS 速度或磁力计来确定航向。车载场景下航向粗对准通常用 GPS 的航迹方向，因为车在静止时航向角是观测不到的，等车走起来才有可观测性。
% 粗对准：取静止段加速度计均值求水平姿态 acc_mean = mean(acc_data(1:N_static, :), 1); % 静止段比力均值 pitch0 = atan2(-acc_mean(1), sqrt(acc_mean(2)^2 + acc_mean(3)^2)); roll0 = atan2(acc_mean(2), acc_mean(3)); % 航向先给初值，精对准阶段再用速度/位置观测量修正参数说明：acc_mean 是静止段三轴加速度计输出的平均值，取均值的目的就是消除车身晃动带来的高频干扰，但前 2 分钟里有晃动意味着均值本身也会被低频扰动污染。稳妥做法是取整段静止数据的均值，再用滑动平均或低通滤波把晃动压掉，同时剔除明显的野值点。pitch0 和 roll0 的计算本质是用重力矢量在机体坐标系下的投影反推姿态角。精对准则进入卡尔曼滤波框架，把姿态误差、速度误差、位置误差和陀螺零偏、加计零偏作为状态量，用 GPS 或里程计的量测不断修正。这一阶段 exp3 中由 KF_correct_gps.m 和 KF_correct_odo.m 分别驱动，相当于两条独立的精对准路径。
3.2 pre_data.m 到底在预处理什么：单位、时间戳和坐标系对齐
车载组合导航最容易翻车的不是滤波公式，而是数据没对齐。pre_data.m 负责把 IMU、里程计和 GPS 的原始数据整理成解算能用的格式，这里面有几个关键点。第一个是单位统一：陀螺要换算成 rad/s，加速度计要换算成 m/s²，GPS 经纬度要换算成米制的当地坐标系偏移，里程计脉冲要乘标度因数得到速度。这些换算系数如果看原始数据文件里的注释，通常能找到；找不到就只能靠经验反推。
% 预处理：GPS 经纬度转局部平面坐标（米制），并做时间对齐 lat0 = gps_raw(1, 1); lon0 = gps_raw(1, 2); % 基准点 gps_xy = zeros(size(gps_raw, 1), 2); for i = 1:size(gps_raw, 1) gps_xy(i, 1) = (gps_raw(i, 1) - lat0) * deg2m_lat; % 北向偏移，m gps_xy(i, 2) = (gps_raw(i, 2) - lon0) * deg2m_lon; % 东向偏移，m enddeg2m_lat 和 deg2m_lon 分别是纬度和经度方向的每度对应米数，纬度方向的尺度接近常数约 111320 米/度，经度方向的尺度要乘 cos(latitude)。这个细节是我在排查某次长距离跑偏问题时注意到的，当时经度转换直接用了固定系数，纬度越高误差越大。时间对齐上，GPS 通常是 1Hz 或 5Hz，IMU 和里程计可能到 100Hz 以上，组合滤波只在有量测的历元更新，其余时间只做机械编排，exp3 的 KF_correct_gps 和 KF_correct_odo 就是在不同时刻被调用的。
4. 三条导航主线代码走读：纯惯导、惯导/GPS、惯导/里程计怎么分工
4.1 main_ins_navi 与 ins_navi.m：没有量测修正的纯机械编排
exp3 的第一条输出线是纯惯性导航，代码入口对应 main_ins_navi 流程里的 ins_navi.m。这条线没有外部量测修正，纯粹依靠陀螺和加速度计积分，短时间内在小范围内结果勉强可用，时间一长姿态误差累积，位置发散是必然的。它的意义在于作为组合导航的基准线，用来对比加了 GPS 或里程计量测后误差收敛了多少。
ins_navi.m 的内部循环结构可以概括为：读入当前时刻陀螺和加计数据，用上一时刻的姿态四元数把比力从机体坐标系转到导航坐标系，再扣除重力和哥氏加速度项，对加速度积分得到速度，对速度积分得到位置。姿态更新用角增量计算。这里的重力补偿最容易漏，因为加速度计测的是比力而不是纯加速度，不扣重力，垂直通道会快速发散，水平通道也会因为姿态误差而耦合出虚假加速度。
% 纯惯导机械编排核心循环（伪代码结构） for k = 2:N omega_ib = imu_data(k, 1:3); % 陀螺角速度 f_ib = imu_data(k, 4:6); % 加速度计比力 % 姿态更新 q = qua_multiply(q, delta_q(omega_ib, dt)); q = q / norm(q); % 比力坐标变换 f_n = qua2dcm(q) * f_ib'; % 速度更新：扣除重力与哥氏力 v = v + (f_n - [0;0;g] + coriolis) * dt; % 位置更新 pos = pos + v * dt; end参数说明里需要提醒的是 dt 必须和 IMU 数据实际采样间隔一致，exp3 的数据是离散采集文件，不能想当然按 100Hz 算，要用时间戳差分确认。哥氏项在低精度短时演示里经常被忽略，但在车辆速度超过 20m/s 且仿真时间较长时，哥氏项带来的偏航率补偿还是有影响的。我在做这类实验时习惯先把纯惯导 10 分钟之后的轨迹画出来，如果位置漂移方向和量级符合预期（几分钟内几百米），说明机械编排没问题，可以继续做组合；如果纯惯导就飞了，后面组合滤波再强也救不回来。
4.2 main_ins_gps 与 KF_correct_gps：位置/速度量测的松组合实现
第二条主线是惯性/GPS 组合，入口是 main_ins_gps，量测更新由 KF_correct_gps.m 执行。这里的组合方式是松组合：惯导输出位置和速度，GPS 也输出位置和速度，两者之差作为卡尔曼滤波的量测新息。状态量一般取姿态误差、速度误差、位置误差加陀螺零偏和加计零偏，量测矩阵 H 在位置误差和速度误差对应的位置取单位阵参与运算。
量测噪声协方差 R 的设置是这一节最关键的参数。GPS 位置精度假设在 2~5 米，速度精度在 0.05~0.2 m/s，R 矩阵对角线对应位置项设成 25 左右、速度项设成 0.01 左右比较常见。如果 R 设得太小，滤波会过度相信 GPS，导致轨迹跟着 GPS 噪声跳；如果 R 设得太大，滤波更新力度不足，惯导误差得不到有效修正。我一般会先把 GPS 静基座下的标准差算出来，再按这个实际值去设 R，而不是凭感觉给一个数。
% GPS 量测更新：位置和速度作为观测量 z = [gps_pos_xy' - ins_pos; gps_vel' - ins_vel]; % 量测新息 H = [eye(2), zeros(2, 4), zeros(2, 3); zeros(2, 2), eye(2), zeros(2, 3)]; % 位置、速度观测 R = diag([5^2, 5^2, 0.1^2, 0.1^2]); % 位置方差 25 m^2，速度方差 0.01 (m/s)^2 [x, P] = KF_correct(x, P, z, H, R);这里面 H 矩阵的维度取决于状态向量排列顺序，exp3 的 initpara.m 里状态排列如果前面是位置误差和速度误差，H 矩阵就按上述方式构造；如果状态顺序不同，H 要相应调整。KF_correct.m 是通用的卡尔曼量测更新函数，输入先验状态、先验协方差、量测新息、量测矩阵和噪声阵，输出后验状态和后验协方差。GPS 量测是离散的，不是每个惯导解算周期都有，所以调用前要判断当前历元是否有 GPS 数据，没有就跳过更新只做传播。
4.3 main_ins_odo 与 KF_correct_odo：里程计速度约束与完整性约束
第三条主线是惯性/里程计组合，入口是 main_ins_odo，量测更新由 KF_correct_odo.m 执行。里程计在车上只输出前进方向的速度，但它隐含了一个重要的运动约束：车辆正常情况下不会侧向滑动，也不会垂直弹跳。这就是非完整性约束（NHC），即侧向速度和垂向速度为零。把惯导解算出的侧向和垂向速度约束为零，等效于给滤波提供量测，能有效抑制航向漂移。
直观理解是，GPS 在天线被遮挡或城市峡谷里会丢失，里程计不依赖外部信号，持续输出速度。但里程计也有自己的坑：标度因数误差直接变成速度误差，安装角偏差会让前进速度耦合到侧向和垂向通道。exp3 里 KF_correct_odo 的做法是取惯导解算速度转换到车身坐标系后，用侧向和垂向速度作为量测，同时可以引入里程计前向速度作为第三个量测通道。
% 里程计约束：利用非完整性约束，侧向/垂向速度为零 C_nb = qua2dcm(q); % 导航系到机体系的方向余弦矩阵 v_b = C_nb * ins_vel'; % 转换到车身坐标系 z = [v_b(2); v_b(3)]; % 侧向、垂向速度应接近 0 H = [C_nb(2, :), zeros(1, 3), zeros(1, 6); C_nb(3, :), zeros(1, 3), zeros(1, 6)]; R = diag([0.01, 0.01]); % 约束松紧可调 [x, P] = KF_correct(x, P, -z, H, R);这里的 R 取值很有讲究，如果设成 0.001 以下，相当于强制侧向和垂向速度绝对为零，一旦车身真的发生侧滑或路面颠簸，滤波会被假的约束带偏。我一般设 0.01 到 0.1 之间，既能抑制漂移又允许一定程度的运动偏离。为什么惯导/里程计组合在短暂 GPS 中断时依然能维持几十秒的可用精度，正因为 NHC 约束从几何上限制了轨迹形态：车只能沿着当前航向向前走，不能横向飘。代价是如果车辆原地转弯或倒车，里程计的标度因数和安装角模型会引入额外的误差，这也是在数据中能看到转弯段误差增大的原因。
5. 避坑与排查：IMU 晃动、量测噪声和坐标单位导致的五个经典问题
5.1 对准结果发散：静止段晃动被当成了真实运动
现象：粗对准完成后进入精对准阶段，姿态角在几分钟内缓慢漂移，位置误差发散速度远快于理论预期。 原因：前 2 分钟的静止段并不完全静止，车身晃动让加速度计的输出包含了水平方向的干扰分量。直接把整段数据平均或原始比力代入对准公式，相当于把晃动加速度当成了真实运动。 解决：给静止段数据先过滑动窗口均值滤波，或者取静止段后几十秒的稳定窗口单独算姿态；实在不行把加速度计输出在对准坐标系下做低通滤波，截止频率设在 0.5Hz 左右，把车身晃动的低频分量压掉。
5.2 GPS 位置跳变让滤波震荡：经纬度没有转成米制
现象：GPS 组合导航的轨迹在每次 GPS 更新时出现明显跳变，滤波协方差一直不收敛，曲线呈现锯齿状。 原因：GPS 的经纬度单位是度，而惯导的位置单位是米，两者直接做差，量测新息数值在 1e-5 量级，配合 H 矩阵的米制系数，等价于给了滤波一个错误量级的观测量。 解决：先做经纬度到局部平面坐标（北向和东向，单位米）的转换，转换基准点选起始点或轨迹中心点，再进量测更新。转换后用 GPS 在静基座下的位置标准差去检查新息，如果新息均值不为零且标准差远大于 R，说明坐标系或基准点还有问题。
5.3 里程计约束把轨迹拉直：转弯段误差变大
现象：加了里程计约束后，直线段轨迹很干净，但转弯路段位置误差明显增大，转弯结束后的航向和真实轨迹对不上。 原因：非完整性约束假设侧向速度为零，但在快速转弯时车辆确实存在侧向加速度和少量侧滑，强制侧向速度为零相当于把转弯时的真实运动部分丢弃，滤波会把这种差异解释为姿态误差，进而污染航向。 解决：把约束的 R 从 0.001 放宽到 0.05，允许少量侧向运动；同时检查里程计的前向速度标度因数是否准确，标度误差在转弯时会被放大。另外，转弯段不要开里程计前向速度作为量测，只用侧向和垂向约束，能减少耦合误差。
5.4 四元数模长漂移导致的姿态缓慢畸变
现象：长时间运行后横滚和俯仰出现缓慢的、周期性的偏移，位置误差不收敛，把姿态角打印出来看到四元数模长在 1 附近缓慢偏离。 原因：四元数更新过程中每一步的数值积分都引入微小误差，模长偏离 1 后，方向余弦矩阵的正交性被破坏，姿态变换就不再是纯旋转。 解决：在每次姿态更新完成后强制归一化，这是最有效的兜底手段。同时在姿态链路的输出端做 dcm2eul 之前再检查一次四元数模长，发现偏离超过 1e-4 就直接报错，别等发散到不可挽回。
5.5 GPS 和 IMU 时间戳不同步导致量测新息乱跳
现象：组合滤波在多数时间表现正常，但每过一段时间就会出现一次极大的新息脉冲，协方差骤降后立即反弹。 原因：GPS 和 IMU 的采样时刻不对齐，直接把 GPS 数据当成当前惯导历元做量测更新，相当于用了十几毫秒甚至更早的观测量来修正当前状态。 解决：在预处理阶段做一个最小时间间隔匹配，找到每个惯导历元之前最近的 GPS 时间戳，保存对应的 GPS 量测，在量测更新时用这一组数据。更严格的做法是引入量测延迟补偿，但在实验包里先做时间对齐就能解决大部分问题。检查办法是画出新息序列，如果脉冲出现的时间间隔和 GPS 采样周期重合，基本就是时间同步问题。
6. 精度验证法：把纯惯导、GPS 组合、里程计组合的误差量化成对比
三条解算线跑完之后，不能只看轨迹图说「感觉里程计组合好一些」，要把误差量化出来。做法是以 GPS 轨迹或已知的行驶路径为参考，分别计算纯惯导、惯导/GPS、惯导/里程计在每一时刻的位置误差。我一般先做离线误差统计，再做分段对比。
% 计算位置误差序列并统计均方根误差（RMSE） err_ins = sqrt(sum((pos_ins - pos_ref).^2, 2)); err_gps = sqrt(sum((pos_ins_gps - pos_ref).^2, 2)); err_odo = sqrt(sum((pos_ins_odo - pos_ref).^2, 2)); rmse_ins = sqrt(mean(err_ins.^2)); rmse_gps = sqrt(mean(err_gps.^2)); rmse_odo = sqrt(mean(err_odo.^2)); % 输出对比：正常情况下 odo 在无 GPS 段优于 ins，全程不如 gps fprintf('INS RMSE: %.2f m\nGPS RMSE: %.2f m\nODO RMSE: %.2f m\n', ... rmse_ins, rmse_gps, rmse_odo);这段代码以 pos_ref 为参考轨迹，实际使用中如果要公平对比，参考轨迹可以把 GPS 和里程计组合的结果再平滑一次，或者手动挑选几段直线路径做分段验证。参数说明：err_ins 是纯惯导与参考轨迹的逐点误差，err_gps 和 err_odo 同理。RMSE 是最直观的单指标，但只看 RMSE 会掩盖误差分布特征，比如里程计组合可能在直线段 RMSE 很低、转弯段很高，而 GPS 组合误差分布更均匀。我习惯同时输出最大误差和误差的 95 百分位，最大误差反映最差情况，95 百分位反映典型情况。
调参的习惯步骤是：先跑纯惯导，确认机械编排没有大问题；再跑 GPS 组合，调 R 让新息序列均值接近零、协方差与理论值匹配；最后加里程计约束，先只用 NHC 约束跑一遍，确认转弯段行为合理后，再决定是否把里程计前向速度也作为量测通道。整个过程里最关键的一步是初始对准，粗对准姿态如果偏了 1 度，后面所有组合导航的结果都会带上这个偏差。当时我在这套实验上翻过一次车：只改了 GPS 的 R 矩阵，没重跑粗对准和预处理，换了个数据集就崩了。从那以后我每次拿到新的 IMU 数据都强制走一遍完整流程，先粗对准、再纯惯导、再单量测、最后组合，任何一步异常都先解决再往下走，希望这个习惯对你有帮助。
本文还有配套的精品资源，点击获取
