简介面向航空航天、自动驾驶与航海等导航应用场景这份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误差的传播模型在短时间内近似线性可以直接套用标准卡尔曼滤波的线性方程第二加速度计和陀螺零偏虽然也在缓慢变化但它们的量级远小于导航输出当作随机游走建模非常自然。如果直接估计位置速度姿态状态方程是强非线性的还要上扩展卡尔曼滤波或者无迹卡尔曼滤波工程复杂度立刻上去。状态量维度含义一般初值δp3NED位置误差单位米全零δv3NED速度误差单位米每秒全零ε3姿态失准角单位弧度全零b_g3陀螺常值零偏单位弧度每秒全零b_a3加速度计常值零偏单位米每平方秒全零在松组合里滤波器的预测环节只靠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 状态转移矩阵F15维误差状态怎么在时间上传播卡尔曼滤波预测部分的核心是状态转移矩阵。连续时间状态方程是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粗差带偏”的假故障。希望帮到你。本文还有配套的精品资源点击获取
