简介：面向航空航天、自动驾驶与航海等导航应用场景，这份GNSS/INS松组合导航Matlab程序适合需要研究卫星/惯性组合导航算法的工程师与在校生。程序围绕GNSS与INS数据融合展开：先由GNSS轨迹生成模拟惯导输出，再经卡尔曼滤波、四元数姿态更新与惯导解算完成运动状态估计，可复现松组合导航定位流程。压缩包共30个文件，以26个m脚本为主，辅以2个mat数据、imu与asv工程文件，涵盖初始化、轨迹生成、滤波更新、结果绘图等模块，整体大小6.49MB。已有2482人学习下载，可结合源码与数据文件快速理解松组合导航的信号处理、滤波理论和随机过程知识，并在此基础上进行实验调参与算法优化。
1. 松组合导航：怎么把“会飘的惯导”和“会跳的GNSS”拧成一条能用的轨迹
在车载和无人机定位里经常遇到一个奇怪现象：纯惯导跑不到3分钟，轨迹就开始明显漂移；单点GNSS在楼群中又会被多径折磨得跳来跳去。GNSS/INS松组合导航这个Matlab程序，就是来解决这个“两边都不太靠谱”的问题的——它用卡尔曼滤波器把两路信号拧在一起，GNSS负责纠正INS随时间累积的漂移，INS负责在GNSS短暂失锁时填补输出。对刚接触组合导航的开发者来说，这是理解数据融合最直观的起点，代码量不大，每个矩阵都能对着公式找得到。适合做课题验证、车载或无人机定位算法预研，也适合想从零搭起组合导航框架的开发者。拿到源码以后，先别急着跑，把状态向量、坐标转换、反馈校正这条链路捋清楚，后面调参和加新观测都顺很多。
2. 系统架构：从IMU原始数据到位置输出的完整链路
2.1 松组合和紧组合的区别：为什么要从松组合开始
先说数据流。松组合里，GNSS接收机输出的不是伪距、载波相位，而是已经解算好的位置和速度。INS这边做一套完整的机械编排，输出高频的位置速度。两者在同一时刻做差，作为卡尔曼滤波器的量测输入。GNSS和INS的导航解对于滤波器来说都是黑匣子，它只关心残差，不关心内部怎么算出来的，所以实现起来比紧组合简单一个量级。
紧组合则要把GNSS原始观测（伪距、载波相位、多普勒）直接放进量测方程，状态量里还要加入接收机钟差、钟漂甚至模糊度参数，方程非线性更强。松组合最大的好处是起步容易、调试直观，GNSS本身不参与状态传播，即使某一阵卫星质量差，也不至于污染INS的机械编排。缺点是GNSS定位精度直接决定了组合输出的天花板，当GNSS长时间得不到定位解时，松组合就退化为纯惯导。
这个项目选松组合是很实际的定位：先把卡尔曼融合、误差反馈、坐标系转换这条链路跑通。等你理解了误差状态滤波器的每一行代码，再往紧组合升级时，你已经知道滤波器哪里容易出问题，而不是拿一个黑盒工具包在那里调参。
2.2 坐标系约定：ECEF、NED、载体坐标系对齐是组合的前提
组合导航里最常见的错误就是坐标系没对齐。IMU原始数据是在载体坐标系下的角速度和比力，一般定义成“右前上”或“前右下”；GNSS位置通常在WGS84的ECEF下给出，也可能直接给经纬高；导航解算则在当地NED坐标系下完成。三个坐标系之间，要用姿态矩阵把比力转到NED，要用大地坐标转换矩阵把ECEF转到NED。任何一个转换里符号写反，组合出来的轨迹就会带固定偏差，滤波器使劲推都推不回来。
这个程序里我习惯把坐标转换单独抽成一个工具模块，避免在主循环里写一堆三角函数。下面这段是ECEF转NED的核心，参考点是数据起点附近的一个经纬高：
% ECEF转NED：以参考点 lat0,lon0,h0 为原点 function r_ned = ecef2ned(r_ecef, ref_llh) lat = ref_llh(1); lon = ref_llh(2); h = ref_llh(3); % WGS84椭球参数 a = 6378137.0; f = 1/298.257223563; e2 = f * (2 - f); % 参考点ECEF坐标 ref_ecef = llh2ecef(ref_llh, a, e2); d = r_ecef - ref_ecef; % NED旋转矩阵，按N、E、D三个方向排列 R = [-sin(lon), cos(lon), 0; -sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat); cos(lat)*cos(lon), cos(lat)*sin(lon), sin(lat)]; r_ned = R * d; end这里R矩阵的第一行对应北向，第二行对应东向，第三行对应地向。参数a是长半轴，f是扁率，e2是第一偏心率的平方，如果你的数据来自其他椭球模型，这三个数要整体替换。最容易犯的错是参考点ref_llh直接用全零或者随便填了一个值，导致转换后的NED坐标带一个固定偏置，这个偏置会一直被当成位置误差“处理”，但又永远消不掉。
2.3 15维误差状态滤波器：为什么估计“误差”而不是估计“位置”
主滤波器不是直接对位置、速度、姿态做估计，而是估计INS机械编排结果与真值之间的误差。这个误差向量的维度是15，我习惯按这个顺序组织：位置误差3维、速度误差3维、姿态误差3维、陀螺零偏3维、加速度计零偏3维。
用误差状态的好处有两点。第一，INS误差的传播模型在短时间内近似线性，可以直接套用标准卡尔曼滤波的线性方程；第二，加速度计和陀螺零偏虽然也在缓慢变化，但它们的量级远小于导航输出，当作随机游走建模非常自然。如果直接估计位置速度姿态，状态方程是强非线性的，还要上扩展卡尔曼滤波或者无迹卡尔曼滤波，工程复杂度立刻上去。
| 状态量 | 维度 | 含义 | 一般初值 |
|---|---|---|---|
| δp | 3 | NED位置误差，单位米 | 全零 |
| δv | 3 | NED速度误差，单位米每秒 | 全零 |
| ε | 3 | 姿态失准角，单位弧度 | 全零 |
| b_g | 3 | 陀螺常值零偏，单位弧度每秒 | 全零 |
| b_a | 3 | 加速度计常值零偏，单位米每平方秒 | 全零 |
在松组合里，滤波器的预测环节只靠INS机械编排驱动，每帧IMU数据都做一次时间更新。量测更新不是每次都触发，只有等GNSS数据到来时才做。这就带来一个工程上的关键点：高频预测、低频校正，状态误差在每次校正后被反馈回INS解算模块，把漂移“掰”回来。理解这个“高频预测、低频校正”的节奏，是阅读后面代码的一把钥匙。
3. 核心代码逐段走读：一次预测与量测更新的完整链路
3.1 INS机械编排：每帧IMU数据进来后最先发生的事
主循环里，每收到一帧IMU数据，先做机械编排。机械编排干的事很纯粹：用陀螺角速度更新姿态，用加速度计比力更新速度，再用速度更新经纬高。下面这个函数把这三步放在一起：
function [att_quat, vel_n, pos_llh] = ins_mech_one_step(...) % 输入：上一帧四元数att_quat、速度vel_n、经纬高pos_llh % 陀螺角速度gyro(3x1)、加速度计比力acc(3x1) % 陀螺零偏gyr_bias、加计零偏acc_bias、时间间隔dt % 姿态更新：一阶四元数积分，角增量转旋转向量再转四元数 delta_ang = (gyro - gyr_bias) * dt; if norm(delta_ang) > eps dq = [cos(norm(delta_ang)/2); sin(norm(delta_ang)/2) * delta_ang / norm(delta_ang)]; else dq = [1; 0; 0; 0]; end att_quat = quat_normalize(att_quat * dq); % 比力从载体坐标系转到NED系 f_n = quat_rotate(att_quat, acc - acc_bias); % 速度更新：扣除科氏加速度和重力 omega_en = [vel_n(2)/(Rn + pos_llh(3)); -vel_n(1)/(Rm + pos_llh(3)); -vel_n(2)*tan(pos_llh(1))/(Rn + pos_llh(3))]; omega_ie_n = [omega_ie*cos(pos_llh(1)); 0; -omega_ie*sin(pos_llh(1))]; vel_n = vel_n + (f_n - cross(2*omega_ie_n + omega_en, vel_n) + g_n) * dt; % 位置更新：子午圈半径Rm和卯酉圈半径Rn随纬度变化 dLat = vel_n(1) / (Rm + pos_llh(3)) * dt; dLon = vel_n(2) / ((Rn + pos_llh(3)) * cos(pos_llh(1))) * dt; dAlt = -vel_n(3) * dt; pos_llh = pos_llh + [dLat; dLon; dAlt]; end这段代码的要点有三个。第一，陀螺或加速度计零偏在递推前就要减掉，零偏来自滤波器上一轮的反馈，不是固定常数。第二，速度更新里cross那一项是地球自转和载体在地球表面运动带来的有害加速度，车载低速场景下量级不大，但跑高动态数据时不处理，速度误差会缓慢累积。第三，位置更新里的Rm和Rn必须根据当前纬度实时计算，不能用一个固定值代替——纬度每变化一度，子午圈曲率半径的变化对位置增量是有影响的，高精度场景下省掉这一项会让纬度方向误差线性增长。
欧拉角表示姿态在高俯仰角时会遇到万向锁问题，所以姿态部分全部走四元数，只在初始对准和最终输出时转成欧拉角。这也是这套程序里比较省心的设计，如果你自己写姿态更新，千万不要拿欧拉角直接做积分。
3.2 状态转移矩阵F：15维误差状态怎么在时间上传播
卡尔曼滤波预测部分的核心是状态转移矩阵。连续时间状态方程是dx/dt = F * dx + w，离散化后Phi = I + Fdt + (Fdt)^2 / 2。在松组合里，F由几个分块矩阵拼起来，每一块都有明确的物理来源。
function F = build_state_transition(f_n, vel_n, pos_llh, omega_en, omega_ie_n) F = zeros(15, 15); % 姿态误差方程：失准角一阶导数约等于 -omega_in_n × epsilon % 这里的omega_in_n = omega_ie_n + omega_en F(1:3, 1:3) = -skew(omega_ie_n + omega_en); % 姿态误差对陀螺零偏的耦合，符号是负 F(1:3, 10:12) = -eye(3); % 速度误差方程：比力误差项 + 科氏项 F(4:6, 1:3) = skew(f_n); % 姿态误差通过比力影响速度 F(4:6, 4:6) = -skew(2*omega_ie_n + omega_en); % 科氏耦合 F(4:6, 7:9) = eye(3); % 位置误差对重力的影响 F(4:6, 13:15) = eye(3); % 加计零偏直接进速度误差 % 位置误差方程：速度误差的积分 F(7:9, 4:6) = eye(3); % 陀螺零偏和加计零偏按随机游走建模，F中对应块为零 % 不确定性由过程噪声协方差Q驱动 end这个矩阵的符号特别容易写反。姿态误差块里的负号来自失准角的定义，速度误差方程里skew(f_n)的符号也很容易错。如果F矩阵搭错了，滤波器会在量测更新时“越纠越偏”，轨迹发散得很有规律——你一眼看上去像是噪声没调好，其实方向都错了。
F(4:6, 7:9)在严格模型里是位置误差对重力矢量和科氏项的偏导，完整版还需要考虑曲率半径的导数项。这个项目采用简化写法，直接置eye(3)，在大多数车载数据集上完全够用。真到高空长航时，你会发现需要对这一项做高阶修正。从工程角度说，先跑通简化版再回来补这一项，是一个比一上来就追求完整模型更务实的路径。
3.3 量测更新：GNSS位置和速度进入滤波器的时刻
量测更新只在有GNSS帧时发生。松组合的量测方程是线性的，H矩阵非常简单：GNSS位置速度和INS位置速度之差，直接对应状态向量里的位置误差和速度误差。
function [x_new, P_new, innovation] = kalman_update(x_pred, P_pred, z, H, R) % 新息协方差 S = H * P_pred * H' + R; % 卡尔曼增益 K = P_pred * H' / S; % 状态修正 x_new = x_pred + K * (z - H * x_pred); % 协方差更新，用Joseph形式避免P失去对称性 P_new = (eye(15) - K * H) * P_pred * (eye(15) - K * H)' + K * R * K'; end这里的z是六维观测残差，由GNSS的NED位置速度和INS的NED位置速度做差得到。H矩阵在设计时按状态顺序填：第1到3列填单位阵对应位置误差，第4到6列填单位阵对应速度误差，其余列都是零。调用时的H大概长这样：
H = zeros(6, 15); H(1:3, 1:3) = eye(3); H(4:6, 4:6) = eye(3);R矩阵的大小直接决定滤波器信GNSS还是信INS。如果R给得过于乐观，滤波器会被GNSS噪声带着跳，位置输出看起来更“贴”GNSS，但短时噪声反而放大；如果给得过大，INS漂移纠正得慢，误差容易拉出一个弧形。常见做法是静止采集一段数据，看GNSS位置输出的标准差，再把R的对角线设为标准差的平方。这里不要拍脑袋填，数据说话最稳。
3.4 反馈校正：误差状态如何掰回机械编排
滤波器的输出是15维误差状态，它还不是最终导航结果。真正的位置、速度、姿态仍然在INS机械编排那边，更新完必须把误差反馈回去，否则下一帧机械编排会带着旧误差继续跑。
% 位置、速度直接减去误差 pos_llh(1:3) = pos_llh(1:3) - x(1:3); vel_n(1:3) = vel_n(1:3) - x(4:6); % 姿态误差是失准角，用左乘旋转矩阵修正 Cbn = quat_to_dcm(att_quat); Cbn = (eye(3) - skew(x(7:9))) * Cbn; att_quat = dcm_to_quat(Cbn); % 零偏做累加更新 gyr_bias = gyr_bias + x(10:12); acc_bias = acc_bias + x(13:15); % 反馈后重置误差状态，下一轮从零开始 x(1:15) = zeros(15, 1);注意零偏和位置误差的符号方向。如果机械编排用的是“gyro - gyr_bias”，那么估计出来的零偏误差要加到原来的gyr_bias上，而不是减。这个符号一旦错了，滤波器会陷入稳态偏差——残差看着不大，但位置总差一截。反馈后要把误差状态清零，这对应闭环反馈模式。如果不做清零，误差会重复累加，协方差不收敛，代码跑久后精度反而下降。很多从视觉SLAM转过来的朋友习惯把误差状态一路攒着不重置，在这套流程里要改过来。
4. 跑通流程：数据准备、入口脚本与结果判读
4.1 工程目录：每个文件是干什么的
这套源码包的目录结构大致是这样：
GNSS_INS_Loose/ main_loose_gnss_ins.m % 主入口，跑完整段数据 init_param.m % 参数配置，频率、噪声、初值 data/ imu_data.csv % IMU原始数据：时间、陀螺、加计 gnss_data.csv % GNSS位置速度：时间、经纬高、NED速度 test_rtk.mat % RTK参考轨迹，用来评估组合结果 scripts/ load_dataset.m % 读取CSV，整理成统一结构体 align_time.m % 时间对齐，GNSS和IMU时间戳匹配 modules/ ins_mech.m % 机械编排 build_F.m % 状态转移矩阵 kalman_update.m % 量测更新 feedback.m % 反馈校正 coord_convert.m % ECEF/NED/经纬高转换 results/ trajectory_compare.png % 轨迹对比图 position_error.png % 位置误差曲线数据和代码是分离的，跑之前先把load_dataset.m里的路径指对。align_time.m值得多说一句，它负责把GNSS帧的时间戳对齐到最近的IMU时刻，因为松组合里量测更新必须在IMU时间轴上发生，不能单独走一条时间线。
4.2 运行入口与核心配置
运行入口是main_loose_gnss_ins.m，但真正要改的是init_param.m。第一次跑通之前，先把init_param.m里的参数过一遍：
cfg.imu_rate_hz = 200; % IMU输出频率 cfg.gnss_rate_hz = 10; % GNSS输出频率 cfg.att_init_deg = [0; 0; 90]; % 初始横滚/俯仰/航向（度） cfg.vel_init_ned = [0; 0; 0]; cfg.pos_init_llh = [0; 0; 0]; % 会被数据第一帧覆盖 % GNSS量测噪声（位置单位米，速度单位米每秒） cfg.R_pos = diag([1.5^2; 1.5^2; 3^2]); cfg.R_vel = diag([0.1^2; 0.1^2; 0.2^2]); % 过程噪声：陀螺、加计、零偏随机游走 cfg.Q_gyro = 1e-4 * eye(3); cfg.Q_acc = 1e-3 * eye(3); cfg.Q_bg = 1e-6 * eye(3); cfg.Q_ba = 1e-5 * eye(3);然后回到主入口直接运行：
cd /path/to/GNSS_INS_Loose matlab -batch "init_param; main_loose_gnss_ins"不想用命令行的话，直接在Matlab编辑器里先运行init_param.m，再运行main_loose_gnss_ins.m，效果一样。运行时控制台会打印两类信息：一是主循环的进度百分比，二是每次GNSS量测更新后的新息模长。如果新息模长持续超过某个阈值，说明滤波没有收敛，或者某一段GNSS数据里有粗差。这个是判断程序是否正常工作的重要信号，不要只盯着最后的图看。
4.3 结果图怎么读：三条轨迹线和两组误差曲线
程序跑完后，results目录下会有几张图。第一张是轨迹对比，通常画三条线：GNSS单独解算的轨迹、INS机械编排的轨迹、组合导航输出。松组合正常的画面上，GNSS轨迹在转弯处会有锯齿，INS轨迹光滑但整体缓慢偏移，组合输出则贴着参考轨迹走。
第二张是位置误差曲线，N、E、D三个方向分别画。松组合在GNSS正常时，位置误差会被拉回两三米以内；一旦GNSS持续丢星超过几十秒，误差会明显上抬，这个上抬的斜率就是纯惯导的发散速率。第三张是速度误差曲线，它应该比位置误差更平稳，因为速度是GNSS直接观测的量。
如果发现“组合输出比GNSS单独解算还差”，先别急着调Q和R。我见过太多人卡在这里，把噪声矩阵来回调了几十组也没用。正确的排查顺序是：先确认时间对齐有没有问题，再确认坐标转换参考点是不是用错了，然后才轮到噪声参数。这两个结构性错误不解决，Q和R怎么调都是白搭。
5. 避坑指南：松组合调试里最容易翻车的几个细节
5.1 轨迹整体偏离一个固定值，而且滤波救不回来
现象：组合轨迹比GNSS单独解算的位置相差几十米，滤波器不断更新，但误差曲线像一个平台一样推不过去，位置误差稳定在一个固定值上不下降。
原因：GNSS位置在ECEF系下，转到NED时参考点和数据实际起点不一致，转换后的NED坐标带了一个固定偏置。组合滤波把这个偏置当成位置误差的一部分去估计，但状态向量里没有“GNSS系统偏差”这个自由度，所以它永远消不掉。
解决：在load_dataset阶段用GNSS数据第一帧的经纬高作为NED参考点，而不是用零向量或拍脑袋填的值。检查坐标转换时ref_llh的来源，只要是来自数据本身，这个坑基本就绕过去了。
5.2 滤波发散，状态量出现NaN
现象：程序跑了几百帧，某一次量测更新后P矩阵对角线变成NaN，之后整条轨迹飞掉，控制台开始刷Inf和NaN警告。
原因：多数时候是数据里混入了异常值，最典型的就是GNSS某一帧位置直接是0或者某个极端跳变。卡尔曼把它当成正常观测，新息变成几千，协方差更新时矩阵奇异，P失去正定性，数值直接崩掉。
解决：在主循环入口加一个数据有效性检查，发现位置为0或时间戳不递增时跳过量测更新。协方差更新改用Joseph形式，也就是第3章里写的那种方式，能减少数值退化。顺手给P的对角线加一个最小值下限，例如1e-12，不会影响正常滤波，但能挡住数值爆炸。这三个保护加完，NaN场景基本不会再出现。
5.3 GNSS和INS轨迹之间有一个小角度旋转
现象：轨迹形状整体是对的，但组合输出和参考轨迹之间存在一个固定小角度，转弯处能看出差两三度，直线段看不出来。
原因：这是天线杆臂问题。GNSS天线相位中心和IMU质心在载体坐标系里不在同一个点，这个距离在转弯时会被姿态变化放大成速度误差。纯粹的误差状态模型里没有包含杆臂，滤波结果就会带一个系统性的角度偏差。
解决：在GNSS量测进入滤波器之前，把GNSS位置先挪到IMU质心：r_gnss_imu = r_gnss - C_b_n * lever_arm。lever_arm需要实测，一般车辆的GNSS天线比IMU高几十厘米到一米，用卷尺量一下填进去。这个补偿在自行车、无人机这类小载体上尤其重要，因为杆臂相对载体尺寸的比例更大。
5.4 静止时速度误差反而比动态时更大
现象：车辆停在原地，GNSS位置看起来稳定，但组合输出的速度时不时跳到零点几米每秒，曲线比行驶时还毛糙。
原因：GNSS速度在静态下其实并不为零，多径、接收机噪声都真实存在，但如果R_vel给得过于乐观，滤波器会把GNSS速度噪声当成真实速度变化吸收进状态，反而放大了速度输出。
解决：用一段静态数据估计GNSS速度噪声的真实方差作为R_vel，不要凭感觉填0.01。如果你懒，还有一个经验做法：静止时只做位置量测更新，不做速度量测更新，速度状态交给INS递推和位置误差的耦合去修正，效果往往更好。
5.5 时间戳明明对齐了，新息却周期性跳变
现象：GNSS在1Hz和10Hz之间切换时，误差曲线呈现周期性尖峰，尖峰出现的位置和GNSS帧完全对应，位置误差在0.5米到1米之间来回振荡。
原因：IMU和GNSS数据来自不同采集设备，时间基准不统一。GNSS帧用的是接收机时间，IMU用自己的内部时钟，长时间运行后两者时钟漂移累加，导致GNSS帧实际是拿前一组IMU递推出的位置和这一帧GNSS做差，新息带着一个与速度方向相关的错位量。
解决：在align_time.m里不要只按四舍五入对齐到最近的IMU时间戳，而要做线性插值。找到GNSS时间戳前后两帧IMU的位置速度，按时间比例插出一个“与GNSS同一时刻”的INS输出，再送进滤波器。这个改造工作量不大，但对周期性尖峰有立竿见影的效果。
6. 二次开发：给松组合加一路观测，从哪里下手
这里我从一个最小改造说起：加入GNSS粗差剔除。这个功能在松组合调试里特别实用，因为GNSS不像想象中那么可靠，信号遮挡和突然跳变在实测数据里屡见不鲜。不加防护的滤波器会被一条粗差观测带偏，后面的轨迹要很多帧才能拉回来。在kalman_update前面加一个“新息门”检查，用马氏距离判断这一帧观测是否可信：
% 新息向量和协方差 innovation = z - H * x_pred; S = H * P_pred * H' + R; % 马氏距离 d2 = innovation' / S * innovation; if d2 > chi2inv(0.99, 6) % 认为这一帧GNSS是粗差，整帧跳过量测更新 x_new = x_pred; P_new = P_pred; return; end这里的chi2inv(0.99, 6)取的是6自由度卡方分布的99%分位数，自由度必须和量测维度一致——松组合是位置加速度的6维观测，所以填6。如果后面你加了气压计高度，量测变成7维，这里也要跟着改。
改完之后要验证，办法也简单：手动往GNSS数据里某一帧的位置上加50米偏移，跑一遍程序看是否被拒收；再把它改成5米偏移，看在阈值内是否正常接受。我一般会在数据预处理阶段做一次同样的测试，确认防护逻辑没有被后续改动破坏。这个门槛之外，还有两条常用的升级路线可以考虑。一条是往量测方程里加气压计高度，气压计输出的海拔可以直接作为第七维量测，H矩阵对应位置误差第三列填1；另一条是往状态里加更多随机游走项，把GNSS的位置慢变趋势也估计出来。不管走哪条，先在粗差剔除上打好底座，后续加观测都不会慌。
从那以后，我每次跑新数据集都不会直接看轨迹了，而是先在预处理阶段做一次粗差注入测试，确认保护是开着的，再去看误差曲线。这个习惯帮我挡掉了好几次“看起来像滤波器发散、其实是被GNSS粗差带偏”的假故障。希望帮到你。
本文还有配套的精品资源，点击获取
