简介：这是一套用于全球导航卫星系统与惯性导航系统松组合导航的完整MATLAB仿真程序，面向导航算法学习者、科研人员以及航空航天和自动驾驶领域的工程技术人员。程序针对卫星信号受遮挡、惯性器件存在累积漂移等典型问题，演示了从轨迹生成、模拟惯导数据到卡尔曼滤波融合、运动状态更新的完整流程，便于理解松组合导航的数据融合原理与实现细节。压缩包总计30个文件，大小为6.49MB，其中包含26个m脚本作为核心源码，覆盖初始化配置、滤波更新、姿态解算等关键环节；另有2个mat数据文件用于存放轨迹与惯导数据，以及1个imu和1个asv辅助文件，整体目录清晰，便于按模块阅读。目前已有2483人学习下载，代码注释明确、结构完整。读者可直接运行调试，逐步查看每一步输出，还能修改参数、替换滤波算法，为后续深入研究组合导航或工程应用提供可靠基础。

1. 松组合导航的Matlab落地：一份能跑通的数据融合样板

在城市峡谷里跑车载数据，GNSS信号被高楼反射的瞬间，纯卫星定位轨迹能偏出一条街；纯IMU呢，十秒就开始飘。把两者用卡尔曼滤波组合起来，正是GNSS/INS松组合导航要解决的事。这份Matlab程序做的就是这件事：读入IMU原始角速度和比力数据、GNSS输出的位置和速度，用15维误差状态卡尔曼滤波把轨迹稳住，代码能直接跑出对比图。适合两类人：一类是做导航算法课设或项目预研的学生，需要一份完整可运行的参考实现；另一类是刚接手组合导航、想把松组合的流程从头到尾理清、再往紧组合和DR方向改的工程师。这项目不玄学，每一行都能跟着断点看懂。

2. 架构拆解：从IMU原始数据到经纬度输出的完整链路

2.1 松组合的整体数据流与滤波闭环

松组合（Loosely Coupled）的核心思路是“各算各的，再融合”。GNSS接收机内部已经完成定位解算，直接输出位置、速度；IMU则通过机械编排（Mech）独立推算位置、速度、姿态。两个结果各有各的误差——GNSS有高频噪声和多径跳变，IMU有随时间累积的漂移。EKF要做的，就是把两类误差按统计特性做最优加权。

真正跑起来时是这样一个闭环：每个IMU采样周期做一次机械编排，同时用误差状态方程做一次卡尔曼预测；每当收到一帧GNSS解，就构造位置残差和速度残差，做一次量测更新；更新得到的误差状态估计，再反馈回去修正机械编排里的位置、速度、姿态和零偏。反馈完以后误差状态要清零，否则下一次预测会把同一份误差重复计入。这一步是松组合最容易写错的地方，后面避坑章节会专门讲。

数据流在程序里是这样的：

IMU(acc, gyro) → 零偏补偿 → 机械编排(pos, vel, quat) → EKF预测 ↓ GNSS(LLA, vel) → 坐标转换(ENU) → 构造残差z → EKF量测更新 → 误差反馈

2.2 目录结构与关键文件职责

拿到程序以后别急着跑main.m，先把文件结构扫一遍。这套代码的目录划分很典型，适合直接当模板用：

├── main.m % 主入口：读数据、初始化、主循环 ├── config.m % 所有可调参数集中在这里 ├── core/ % 核心算法目录 │ ├── mech_update.m % IMU机械编排：姿态/速度/位置递推 │ ├── ekf_predict.m % 误差状态卡尔曼预测 │ ├── ekf_update.m % 量测更新，支持位置+速度 │ └── feedback.m % 闭环反馈校正 ├── utils/ % 工具函数 │ ├── lla2enu.m % 经纬高转站心ENU坐标 │ ├── quat2dcm.m % 四元数转方向余弦矩阵 │ └── align_initial.m % 初始对准（静态/速度对准） └── data/ % 测试数据 ├── imu.csv % 时间、三轴加速度、三轴角速度 └── gnss.csv % 时间、经纬高、速度

这套划分的用意很直接：config.m把噪声参数和初始条件全收拢，调参不用翻主循环；core/里的四个函数就是算法的全部，改滤波结构只动这四个文件；utils/里是通用数学工具，跟导航业务解耦。我一般拿到新项目，先看config.m和core/的函数签名，再看main.m的主循环，十分钟就能定位到要改的地方。

2.3 坐标帧约定和初始对准

组合导航里坐标帧混乱是最大的坑。这套代码采用ENU（东北天）站心坐标系作为导航系：原点取GNSS第一个有效定位点，x轴朝东，y轴朝北，z轴朝天。GNSS的经纬高在入口处统一用lla2enu转成ENU平面坐标，IMU的比力和角速度保留在载体坐标系（前-右-下，即x前y右z下）。所有卡尔曼滤波的状态都在ENU系下做，避免在地心地固系下处理重力向量。

初始对准这一步，代码里用了“静态初始化+速度航向对准”的组合。静态段（加速度计方差小于阈值）用加速度计估计俯仰和横滚角，GNSS首个有效速度方向作为初始航向的粗值。初始航向误差直接决定了滤波收敛速度，差个5度可能要好几分钟才能收敛。如果手里有磁力计数据，可以在align_initial.m里加一段磁航向融合，把粗对准精度提到1度以内，后面滤波的压力会小很多。

3. 核心模块拆解：EKF预测、量测更新与闭环反馈的实现

3.1 机械编排：姿态四元数递推与比力积分

机械编排是整个导航解算的地基。每个IMU周期拿到角速度增量，先更新姿态四元数，再把载体坐标系下的比力投影到导航系，减去重力后积分出速度和位置。代码里的核心实现在core/mech_update.m：

function [pos_n, vel_n, quat_n] = mech_update(pos_n, vel_n, quat_n, ... f_ib_b, w_ib_b, dt, g_n) % 姿态更新：四元数一阶积分后归一化 omega = [0, -w_ib_b(1), -w_ib_b(2), -w_ib_b(3); w_ib_b(1), 0, w_ib_b(3), -w_ib_b(2); w_ib_b(2), -w_ib_b(3), 0, w_ib_b(1); w_ib_b(3), w_ib_b(2), -w_ib_b(1), 0]; quat_n = quat_n + 0.5 * omega * quat_n * dt; quat_n = quat_n / norm(quat_n); % 比力从载体系投影到导航系 C_b_n = quat2dcm(quat_n); f_n = C_b_n * f_ib_b; % 速度递推：比力减重力才是真实加速度 vel_n = vel_n + (f_n - g_n) * dt; % 位置递推：一阶积分，高频IMU下精度足够 pos_n = pos_n + vel_n * dt; end

逻辑上注意三点。第一，四元数更新用的是增量式一阶积分，调用方传入的是去掉陀螺零偏后的角速度，如果零偏没补偿，姿态会以固定速率旋转漂移。第二，f_n减去g_n这一步用的是当地重力向量[0,0,-g]（ENU系下重力朝下），而不是只减标量9.8，因为比力投影后三个轴都有分量。第三，位置递推是一阶欧拉积分，IMU频率100Hz以上时误差可以接受；如果IMU是低频的，这段要换成二阶或者用上一周期的速度做梯形积分。

3.2 EKF预测：15维误差状态与状态转移矩阵

滤波器的状态取15维误差向量，顺序是：位置误差(3)、速度误差(3)、姿态误差(3)、陀螺零偏(3)、加速度计零偏(3)。把导航参数本身当状态会带来强非线性，误差状态则近似线性，这是INS/GNSS融合的标准做法。状态转移矩阵F通过误差方程的线性化得到，程序里按分块矩阵组装：

function Phi = build_Phi(F, dt) % 用一阶泰勒展开离散化状态转移矩阵 I15 = eye(15); Phi = I15 + F * dt; % 高阶项对低频IMU影响不大，20Hz以上用一阶足够 end function [x_pred, P_pred] = ekf_predict(x, P, F, Q) dt = F.dt; % 从结构体中取步长 Phi = build_Phi(F.A, dt); x_pred = Phi * x; % 误差状态预测：零均值，主要是协方差演化 P_pred = Phi * P * Phi' + Q; end

F矩阵的分块结构代表误差的传播方式：位置误差对速度误差的偏导是单位阵；速度误差对姿态误差的偏导是比力的反对称矩阵，也就是所谓的“比力耦合项”；姿态误差自身的演化由地球自转和载体运动共同决定。陀螺零偏和加计零偏在F中通过方向余弦矩阵耦合进姿态和速度误差。这一块最容易翻车的地方是F和Q的维度必须严格对齐15维，多一行少一行矩阵乘法直接报错。

Q矩阵是过程噪声协方差，由IMU的陀螺白噪声和加速度计白噪声驱动。一个常见做法是把IMU噪声密度（单位deg/sqrt(hr)和ug/sqrt(Hz)）换算成连续时间谱密度，再乘dt得到离散Q。换算单位在这里是个黑匣子，建议先按程序里默认值跑通，再拿艾伦方差分析结果去替换。

3.3 GNSS量测更新：残差构造与卡尔曼增益

量测更新在收到GNSS帧时触发。GNSS输出的经纬高先转成ENU坐标，和当前位置的差值构成位置残差；GNSS速度直接减当前速度构成速度残差。量测矩阵H是6行15列——前3行取位置误差对应的列，4到6行取速度对应的列，其余为0。

function [x_upd, P_upd] = ekf_update(x_pred, P_pred, z, H, R) % z: 6维残差，顺序是 [dpos_e, dpos_n, dpos_u, dvel_e, dvel_n, dvel_u] % 卡尔曼增益标准公式，注意用 / 而不是 inv 以避免数值问题 S = H * P_pred * H' + R; K = P_pred * H' / S; % 状态更新 x_upd = x_pred + K * z; % 协方差更新：Joseph式在工程上数值更稳 I15 = eye(15); P_upd = (I15 - K * H) * P_pred * (I15 - K * H)' + K * R * K'; end

这里的关键在z的含义。z不是原始的GNSS位置，而是“GNSS值减去INS预测值”的残差。如果INS预测的位置是100米，GNSS测到的是102米，残差就是2米。这个残差里包含着真正的位置误差，卡尔曼滤波把它按比例分配到15个状态上。R矩阵对应GNSS的定位噪声水平，城市环境下位置噪声取5到10米比较合理，开阔地可以压到2米。

有人会问为什么速度残差也要放进来——GNSS多普勒测速的精度通常比位置解算高一个量级，速度残差能有效约束INS的速度漂移，尤其是零偏估计对速度误差特别敏感。没有速度量测的松组合，只靠位置更新，加计零偏往往收敛很慢。

3.4 闭环反馈：误差状态回灌导航参数

这是松组合和开环积分最本质的区别。量测更新得到的15维误差状态，必须立刻反馈修正机械编排里的导航参数，然后清零。如果不做这一步，位置误差会一直自由漂移，卡尔曼滤波成了“只估不校”的摆设。

function [pos, vel, quat, gyro_bias, acc_bias, x] = feedback(... pos, vel, quat, gyro_bias, acc_bias, x) % 位置和速度直接做差 pos = pos - x(1:3); vel = vel - x(4:6); % 姿态修正：小角度近似构造增量四元数 dphi = x(7:9); dq = [1; 0.5 * dphi(1); 0.5 * dphi(2); 0.5 * dphi(3)]; dq = dq / norm(dq); quat = quatmultiply(quat, dq); % 零偏累积，后续机械编排用的就是修正过的零偏 gyro_bias = gyro_bias + x(10:12); acc_bias = acc_bias + x(13:15); % 反馈完必须清零，避免下一次预测重复估计 x = zeros(15, 1); end

姿态修正用的小角度近似成立的前提是姿态误差本身不大，卡尔曼滤波持续工作的情况下这个假设是安全的。转弯剧烈或GNSS长时间中断后重新定位，姿态误差可能超过几度，此时dq要换成精确的四元数乘法。零偏是累积量，每次反馈加上去，机械编排入口要做bias补偿，这个配对关系别弄反。

4. 运行步骤与避坑：从数据准备到结果验证

4.1 数据准备：格式对齐是最费时间的环节

拿到程序先别急着改算法，把数据喂对才是第一关。IMU数据需要四列：时间戳（秒）、三轴角速度（rad/s）、三轴加速度（m/s²）。GNSS数据需要时间戳、经纬度（度）、高度（米）、三轴速度（m/s）。数据里最容易出问题的是时间基准：IMU和GNSS的时间戳是各自独立的时钟，直接按行对齐跑出来的轨迹会带相位差，城市环境下一秒钟的错位就是几十米的偏差。

我整理数据的习惯是三步走。第一步，把两套数据按时间戳画一条分布曲线，确认各自的采样频率和丢帧情况；第二步，用GNSS的PPP或者RTK解算结果做基准，检查IMU的静态段零偏水平；第三步，如果两套数据时间戳对不上，先做线性插值重采样，把GNSS插到IMU时间轴上。程序里data/目录下已经准备好了对齐过的样例数据，第一次跑通用样例，换自己的数据时重点检查时间对齐。

4.2 配置与运行：config.m里的每一项都要看懂

config.m集中了所有运行参数，用Matlab的脚本结构体组织，修改起来很方便。核心参数分四组：IMU参数（采样频率、陀螺噪声密度、加计噪声密度、杆臂）、GNSS参数（采样频率、位置噪声、速度噪声）、初始条件（初始位置、初始速度、初始姿态）、滤波参数（初始协方差P0、过程噪声Q比例因子）。

初次运行时保持默认参数，先跑通再调。P0设置有一个原则：位置方差给3米平方，姿态角方差给1度平方，零偏方差给IMU手册上的零偏重复性。P0给得太大会让滤波前期震荡剧烈，给得太小又会让滤波“自信过头”，后面GNSS量测进来也掰不回来。Q和R的比例决定了滤波更相信谁——GNSS质量差的时候把R调大，IMU质量差的时候把Q调大，这个平衡是组合导航调参的核心玄学。

运行main.m后程序会依次执行：读取数据、初始对准、主循环（逐帧处理IMU和GNSS）、保存结果。跑完之后工作区里会生成三组变量：ins_only（纯惯导解算轨迹）、gnss_only（GNSS原始轨迹）、fused（组合导航轨迹），还有一个gnss_enu_std存GNSS噪声设置。画对比图只需要对比这三条轨迹在平面上的偏差。

4.3 常见问题排查：五个能让你卡半天的坑

第一个坑：融合轨迹出现锯齿形折线。现象是轨迹看起来在GNSS轨和INS轨之间来回跳，折点正好在GNSS更新时刻。原因是GNSS时间戳和IMU没对齐，量测更新在错误的时刻触发。解决方法是重新检查时间戳对齐，或者在主循环里加一个时间同步判断，只在GNSS时间戳与当前IMU时刻差小于半个IMU周期时才执行量测更新。

第二个坑：滤波发散，轨迹直接飞掉。现象是滤波跑几十秒后位置误差指数增长，速度和姿态剧烈震荡。原因多半是Q矩阵量纲写错，把角度的方差写成了弧度而实际用的单位是度，或者F矩阵里少了比力耦合项。排查手法是打印每一时刻的P阵对角线，看速度误差方差是否持续膨胀，如果是，检查Q和F的对应分块。

第三个坑：姿态估计长期不收敛，平台轨迹缓慢旋转。现象是静止状态下滤波输出的航向角一直朝一个方向漂。原因是陀螺零偏初值给了0，而真实零偏可能在0.01度/秒量级，这个量级的零偏足以让航向持续积分漂移。解决方法是先在静态段用平均值估算零偏初值，再启动滤波。

第四个坑：GNSS天线和IMU中心不重合导致的转弯偏差。现象是每次转弯瞬间轨迹先甩出去再拉回来，幅度达到几米。原因是杆臂效应——GNSS天线和IMU的安装位置不在同一点，车辆转弯时两者速度天然不一致。解决方法是配置杆臂向量（天线相对IMU的位移），在速度残差里补偿。

第五个坑：反馈清零没做，导致估计结果全部反了。现象是滤波结果比纯GNSS还差，位置误差持续增大。原因是feedback把误差状态回灌后没有清零，下一轮机械编排又用旧的误差状态修正了一次，等于误差被算了两遍。解决方法是每次反馈后强制把x置零，并在主循环里加断言检查。

5. 二次开发：从松组合到带零速约束的扩展改造

拿到这套松组合代码，最容易见效的改造是加一个ZUPT（零速修正）模块，针对车载或手持设备在红灯停车、静止等待场景下的速度漂移问题。做法是利用IMU本身判断载体是否零速，判断条件用窗口内加速度计的方差和角速度的模值——两者都低于阈值就认为处于静止状态。零速状态下的真实速度为零，这就是一个额外的量测约束，能把速度误差和加计零偏显著压低。

% ZUPT量测更新：静止时刻把速度误差约束到零 if is_zupt(acc_window, gyro_window, std_thresh) H_z = zeros(3, 15); H_z(:, 4:6) = eye(3); % 只观测速度误差 z_z = -vel_n; % 期望速度为0，残差为0-vel_n R_z = eye(3) * 1e-4; % 零速约束噪声需要给很小 [x, P] = ekf_update(x, P, z_z, H_z, R_z); [pos, vel, quat, gyro_bias, acc_bias, x] = feedback(... pos, vel, quat, gyro_bias, acc_bias, x); end

这段代码加在main.m主循环的GNSS量测更新之后，利用的是同一套ekf_update接口，量测矩阵换成速度观测对应的分块。关键点在于R_z要设置得比GNSS速度噪声小一到两个量级，否则零速约束起不到压制作用；但也不宜过小，否则静止时的抖动会被当成真实速度误差，反而把姿态拉偏。

如果是做紧组合方向，改动会更大一些：把GNSS的伪距和载波相位观测引入，量测方程从位置速度残差换成卫星几何距离残差，需要增加卫星星历计算和电离层对流层误差模型。我建议先在松组合这版上把反馈逻辑、协方差调参手感练熟，再往紧组合切会顺很多。从那以后我每改一版滤波参数，都强制先跑一遍静态数据再跑动态数据，两遍都符合预期才敢往下走。希望帮到你。

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