简介面向GNSS/INS组合导航研究者的Matlab程序设计包适用于航空航天、自动驾驶、航海等需要连续可靠定位的场景。程序将全球卫星导航与惯性导航做松组合利用卡尔曼滤波对两类传感器数据进行融合缓解GNSS信号遮挡和INS时间累积漂移带来的影响同时通过模拟轨迹数据生成惯导测量值形成从卫星导航仿真、误差注入到组合滤波的完整闭环适合课程设计、毕业设计以及算法预研。压缩包为rar格式共30个文件含26个m源码脚本、2个mat数据文件、imu原始数据文件和asv自动保存文件m脚本覆盖轨迹生成、惯导解算、滤波更新、误差补偿和结果绘图等模块目录结构清晰在Matlab中可直接运行并定制参数。压缩包整体约6.49MB目前已有2482人学习/下载。通过阅读和调试这套程序可深入理解松组合导航的数据融合流程掌握惯导解算、四元数表示与卡尔曼滤波的算法实现要点并能对照仿真数据复现实验为科研或工程中的组合定位方案提供可复用参考。1. GNSS/INS松组合导航Matlab程序先把框架跑通再谈滤波器玄学做 GNSS/INS 松组合导航 Matlab 程序调试时困扰我的从来不是卡尔曼滤波公式推导而是数据跑到一半轨迹突然发散或者静态时速度噪声大得像在抖。这套松组合资源的价值不在于把算法写得多炫而是把“GNSS 位置速度读取、IMU 机械编排、误差状态卡尔曼滤波、结果绘图”全链路焊在一起让我拿到真实数据后能直接复现、改参数而不是从零敲轮子。它特别适合刚入行做组合导航、正在写课程设计或者想快速验证一个位置估计算法的人。相比紧组合松组合的状态维度和观测矩阵都简单不少踩坑时好定位问题。2. 先看懂“松”在哪原理、文件结构与主流程2.1 松组合 vs 紧组合GNSS 在这套程序里扮演什么角色松组合的“松”体现在 GNSS 和 INS 在进入滤波器之前是各自解算的。INS 部分用陀螺仪角增量和加速度计比力做机械编排连续输出位置、速度、姿态GNSS 接收机独立解算输出经纬高和速度通常以 NMEA 协议或二进制格式给出。两者唯一的交集是卡尔曼滤波器INS 做预测GNSS 做观测更新。紧组合则把 GNSS 的伪距、伪距率甚至载波相位直接放进观测方程。对比下来松组合在实现上有几个很实际的优势。第一GNSS 端不需要修改任何能输出 GGA 和 RMC 语句的接收机都能接入第二观测矩阵 H 是常数矩阵代码维护简单第三状态维数通常只有 15 维Matlab 跑起来非常轻。代价也要提前说清楚。松组合在卫星数量少于一定程度时无法利用残余观测量在城市峡谷里会退化成纯惯性推算如果 GNSS 被多路径干扰位置输出有跳变松组合只能靠滤波增益去抑制没法从伪距层面剔除异常。所以拿到这套程序时不要奢望它能处理所有遮挡场景它的主战场是开阔道路、跑道、水面、地面机器人这类 GNSS 信号连续性较好的环境。2.2 程序包里常见目录文件名背后的功能我拆这类资源有一个习惯先看主脚本再从主脚本调用的函数反推目录结构。这套 GNSS/INS 松组合导航 Matlab 程序典型的目录组织大概是下面这样文件或目录作用你大概率要改的地方main_loose_gnss_ins.m主入口控制整个滤波循环数据路径、IMU 频率、粗差阈值config/init_filter.m初始化状态向量和协方差矩阵P0、Q、R 的初值sins/mech_update.mINS 机械编排姿态/速度/位置递推比力投影、重力模型gnss/read_gnss.m读取 GNSS 数据转成 ENU 位置和速度坐标系原点、时间戳偏移filter/loose_kf.m松组合卡尔曼预测与更新H 矩阵、观测噪声 Rtools/plot_result.m绘制轨迹、误差曲线图线单位、颜色主脚本的运行逻辑通常只有四步。第一步读 IMU 和 GNSS 数据统一时间基准第二步普通循环对 IMU 做机械编排得到高频率的导航解第三步当检测到新 GNSS 观测时计算新息z z_gnss - H * x_pred第四步调用卡尔曼更新修正误差状态然后把修正量反馈回机械编排结果。很多新手拿到程序后第一时间去看滤波函数这是错的。应该反过来先看主脚本的循环找出滤波器被调用的条件再跳到init_filter.m查看状态向量是什么顺序。因为状态顺序直接决定了 H 矩阵的构造如果顺序是速度在前位置在后而init_filter.m里换算成了位置在前那后面的轨迹一定对不上。2.3 机械编排与误差状态为什么状态向量要取 15 维松组合的 INS 部分常用“误差状态”而不是全量状态建模。原因是误差状态量级小线性化误差小卡尔曼滤波的近似假设更容易成立。常见状态向量由三组组成姿态误差角phi3 维单位 rad速度误差dv3 维单位 m/s位置误差dpos3 维单位 m陀螺零偏gyro_bias3 维单位 rad/s加速度计零偏accel_bias3 维单位 m/s²连起来就是 15 维。为什么必须估计零偏因为消费级 MEMS IMU 的陀螺零偏如果不在线估计几分钟内就会出现明显漂移。零偏估计不是靠 GNSS 直接观测出来的而是通过位置和速度误差在一段时间内累积形成的“可观测性”间接估计的。松组合里航向角可观测性本来就弱零偏估计会非常慢所以初始化时如果姿态误差给得太大滤波过程就会长时间发散这在后面避坑章节会重点展开。3. 跑数据主循环时间同步、初始化、预测更新与反馈3.1 时间同步先把 IMU 时钟和 GNSS 时钟对齐几乎所有 GNSS/INS 程序包都会遇到时间不同步问题。IMU 通常由采集板内部时钟控制采样率固定 100Hz 或 200HzGNSS 接收机输出时刻由卫星秒脉冲锁定两者不会完全对齐。常见做法是对每个 GNSS 时间戳在 IMU 时间序列里找最近邻索引。% imu.t 已经减去起始时刻gnss.t 同理 imu_n length(imu.t); gnss_n length(gnss.t); idx_gnss2imu zeros(gnss_n, 1); for k 1:gnss_n [~, idx_gnss2imu(k)] min(abs(imu.t - gnss.t(k))); end % 检查最大时间差便于后面调整延迟补偿 dt_sync abs(imu.t(idx_gnss2imu) - gnss.t); fprintf(最大时间差: %.6f s\n, max(dt_sync));逻辑说明对每个 GNSS 采样点在 IMU 时间轴上做全搜索min的第二个返回值就是最近邻索引。这套写法虽然有两层循环但数据量小的时候完全够用。如果 IMU 频率是 100HzGNSS 是 5Hz最大时间差理论上是半个 IMU 周期也就是 5ms。对车辆定位来说5ms 误差引起的位置偏差可以忽略但如果 IMU 只有 10Hz最大时间差会到 50ms高速过弯时就能看到明显横偏。参数说明dt_sync打印出来如果超过 1ms就要考虑给 GNSS 时间戳加延迟补偿。方法很简单在找索引前给gnss.t整体加上或减去一个固定值比如 0.1s再用同样的搜索逻辑跑一遍看组合定位误差是否下降。这个技巧也是后文验证章节的基础。3.2 滤波器初始化15 维状态和协方差不能瞎填初始化参数是踩坑重灾区。很多人直接从网上下了一段初始化代码单位都不改就填进去最后滤波发散还怪算法有问题。下面是一份可改的初始化模板X zeros(15, 1); % 初始协方差矩阵 P P zeros(15, 15); P(1:3, 1:3) (0.5 * pi/180)^2 * eye(3); % 姿态误差 P(4:6, 4:6) (0.1)^2 * eye(3); % 速度误差 P(7:9, 7:9) (2.0)^2 * eye(3); % 位置误差 P(10:12,10:12) (0.01 * pi/180 / 3600)^2 * eye(3); % 陀螺零偏 P(13:15,13:15) (1e-4 * 9.8)^2 * eye(3); % 加速度计零偏 % 连续系统噪声密度 gyro_noise 0.2 * pi/180 / 3600; % 角度随机游走 acc_noise 0.02 * 9.8; % 速度随机游走 gyro_bias_w 1e-6; % 陀螺零偏过程噪声 acc_bias_w 1e-5 * 9.8; % 加速度计零偏过程噪声 Qc zeros(15, 15); Qc(1:3, 1:3) gyro_noise^2 * eye(3); Qc(4:6, 4:6) acc_noise^2 * eye(3); Qc(10:12,10:12) gyro_bias_w^2 * eye(3); Qc(13:15,13:15) acc_bias_w^2 * eye(3); dt 0.01; % IMU 周期 100Hz Q Qc * dt;逻辑说明P矩阵对角块表示初始误差置信度。姿态误差给 0.5°速度误差给 10cm/s位置误差给 2m。Qc是连续时间噪声密度再乘dt得到离散时间过程噪声Q。角度随机游走和速度随机游走在 IMU 手册里通常以deg/sqrt(hr)形式给出必须先把deg转成rad再把hr转成sqrt(s)否则填进去会差一到两个数量级。参数说明消费级 MEMS 惯性器件的gyro_noise典型值在 0.1~1 deg/sqrt(hr)光纤陀螺可能在 0.001 以下。这份松组合程序包如果面向低成本 IMUP(10:12,10:12)不要给太小否则滤波会认为自己零偏很准后续 GNSS 修正量很难快速改变零偏估计。同理acc_bias_w给太大会让位置误差波动明显给太小则加速度计零偏修正缓慢。3.3 预测与更新F、Q、H、R 四个矩阵怎么摆预测步的核心是状态转移矩阵F和过程噪声协方差Q。F需要根据比力、角速度、重力实时构造很多程序包内部封装了state_matrix.m函数。这里只展示预测和更新的通用骨架。function [X_pred, P_pred] predict(X, P, F, Q, dt) % 一阶离散化 F_dis eye(15) F * dt; X_pred F_dis * X; P_pred F_dis * P * F_dis Q; end逻辑说明一阶离散化对大多数松组合场景足够。如果 IMU 频率低于 50Hz或者载体动态特别强建议改用二阶离散化F_dis I F*dt 0.5*F*F*dt^2。F中与旋转相关的块如果不能实时更新陀螺中高频转动会拖累滤波精度。更新步的观测矩阵取决于 GNSS 提供了什么观测。如果只使用 GNSS 速度H 矩阵就是 3×15如果同时用位置和速度H 是 6×15function [X_corr, P_corr] update(X, P, z, H, R) S H * P * H R; K P * H / S; X_corr X K * z; P_corr (eye(size(P)) - K * H) * P; end逻辑说明z是测量新息顺序必须与 H 矩阵对应。如果 H 前 3 行对应速度、后 3 行对应位置那z的前 3 个数也必须是速度误差后 3 个数是位置误差。顺序一错滤波器无法收敛。参数说明K P * H / S使用了右除内部会走 Cholesky 分解比inv(S)稳定。这里的 R 矩阵要跟 H 的观测顺序保持一致。常见的松组合 R 矩阵是R diag([R_vel, R_pos])对角线上依次是速度方差和位置方差。3.4 误差反馈滤波修正完怎么把误差填回导航解卡尔曼更新得到的是误差状态X不是最终导航解。必须在主循环里把误差反馈回att/vel/pos否则 INS 机械编排结果不会被修正滤波器成了摆设。% 假设 att, vel, pos 是当前机械编排输出 att att - X(1:3); % 姿态误差直接减 vel vel - X(4:6); % 速度误差直接减 pos pos - X(7:9); % 位置误差直接减用于 ENU 局部坐标 % 零偏反馈加到陀螺和加速度计零偏估计上 gyro_bias gyro_bias X(10:12); accel_bias accel_bias X(13:15); % 反馈后清零误差状态但一般保留零偏估计 X(1:9) 0;逻辑说明位置误差反馈这里写的是减法前提是pos和dpos都是 ENU 局部坐标。如果程序里用经纬高表示位置需要把dpos先转换为经纬度增减量再叠加。所有代码包的细节差异几乎都集中在这一段所以拿到程序后优先检查它使用的是全局坐标还是局部坐标。参数说明反馈频率只发生在 GNSS 更新时刻IMU 高频机械编排段不会进行误差反馈这是松组合的标准做法。如果反馈频率过高或过低从误差曲线上能看出一层一层的台阶状那就说明反馈逻辑里时间对齐有问题。4. 参数调优与质量评估别让滤波器“看起来正常”4.1 协方差矩阵初值选多少从哪里找依据很多人把 Q 和 R 当自由参数试其实完全可以从传感器数据和 GNSS 定位源推断。下面是我常用的一张初始化参照表适配普通 MEMS IMU 加单点 GNSS 的场景参数含义建议初值怎么验证R_posGNSS 位置噪声方差单点 3~25 m²RTK 0.01~0.25 m²看静态 GNSS 位置标准差R_velGNSS 速度噪声方差0.01~0.04 m²/s²看静态 NMEA 速度输出Q_gyro角度随机游走0.01~0.5 deg/sqrt(hr)IMU 手册或 Allan 方差实测Q_acc速度随机游走0.01~0.1 m/s/sqrt(s)同上Q_bg陀螺零偏过程噪声1e-7~1e-5 rad/s²长时间静态跑看零偏收敛速度Q_ba加速度计零偏过程噪声1e-5~1e-3 m/s³静态加动态对比P_att0初始姿态误差方差0.1~1° 平方初始对准精度这些初值不需要很精确但量级不能错。比如单点 GNSS 的位置标准差通常在 2~5m却把R_pos填成 0.01滤波器会极度信任 GNSS位置轨迹会保留 GNSS 的所有跳变IMU 平滑作用完全消失。反过来R_pos填 100GNSS 基本被无视轨迹会沿着 IMU 漂移几分钟后偏出好几米。4.2 滤波收敛与否的快速检查看误差曲线形态判断滤波器好不好我一般看 NED 误差曲线而不是只看地图轨迹。误差曲线能暴露收敛速度、是否存在固定偏置、是否发散。% err_ned 是 NED 误差序列单位 m rmse_ned sqrt(mean(err_ned.^2, 1)); fprintf(NED RMSE: N %.3f m, E %.3f m, U %.3f m\n, ... rmse_ned(1), rmse_ned(2), rmse_ned(3)); figure; plot(t, err_ned(:,1)); hold on; plot(t, err_ned(:,2)); plot(t, err_ned(:,3)); grid on; legend(N, E, U); xlabel(时间 (s)); ylabel(误差 (m));逻辑说明误差曲线应该在 GNSS 更新瞬间被拉回接近零然后在两次更新之间缓慢爬升。如果曲线呈现出周期性锯齿说明 GNSS 校正起作用这是正常形态。如果曲线一直停留在某个非零偏置上优先怀疑坐标系投影偏置。如果锯齿振幅越来越大说明 Q 矩阵相对 R 矩阵过大滤波器信任预测多、信任观测少。参数说明跑完一段数据后前 10% 的 RMSE 通常会偏大这是滤波器收敛期。统计完整段误差时可以把收敛期数据剔除只在航迹中后段计算稳态 RMSE。比较不同参数时只看稳态段的 RMSE 会更公平。4.3 把粗差检测加进松组合主循环GNSS 在树荫、楼边和多路径环境里偶尔会吐出一个明显错误的位置。松组合没有伪距层信息只能从新息大小来判断。标准做法是卡方检验或标准差阈值。% z 是新息S 是新息协方差 z_norm z ./ sqrt(diag(S)); if max(abs(z_norm)) 5 % 认为该 GNSS 点是粗差不执行更新 return; end逻辑说明z_norm表示新息相对理论标准差的倍数。diag(S)包含了预测和观测的不确定度用它归一化后正常新息应服从近似标准正态分布超过 5 倍标准差的可能性是小概率事件基本可以断定是异常观测。参数说明阈值 5 对应约 1% 的尾概率比较保守。如果场景中有频繁遮挡可以放宽到 6避免正常动态点被误杀。如果设备跑得很平稳可以收紧到 4。粗差检测虽然简单但在松组合里非常有效它能避免滤波器被单点故障拉飞。5. GNSS/INS松组合的Matlab程序高频踩坑点与排查记录5.1 现象一滤波发散轨迹直接“起飞”现象跑完程序地图轨迹在中间某段突然飞出几十米然后又拉回来或者彻底不回来。原因最常见是初始姿态误差设太大。松组合的 GNSS 位置和速度观测对航向角的可观测性本来就弱如果初始航向误差超过 5°滤波器会在直行段反复修正位置误差被放大。解决先把初始姿态误差控制在 0.5° 以内。如果数据包含一段静置序列我一般会取开头 30 秒数据用加速度计均值求水平姿态再用 GNSS 航迹或磁航向求初始航向。没有静置数据就把P_att0从 5° 缩到 0.5°再重跑。某模拟项目 X 里只改了这个数发散问题就消失了。5.2 现象二组合结果滞后一拍转弯时轨迹对不上现象直线段没有明显问题但每到一个急弯组合导航轨迹都比 GNSS 轨迹慢半拍横偏达到一两米。原因GNSS 接收机输出位置有内部延迟常见在 50~200ms。如果你直接把 GNSS 时间戳和 IMU 时间戳做最近邻匹配等于拿当前 IMU 状态去对应过去 GNSS 观测量滤波输出自然滞后。解决给 GNSS 时间戳加上延迟补偿比如gnss_time_comp gnss_time 0.1再跑一次时间同步。延迟值需要实测可以在急转弯段看 GNSS 航向和组合航向峰值出现时间差然后把差值补偿进去。我见过几个工程加了延迟补偿后水平 RMSE 直接下降一倍。5.3 现象三静态速度噪声大位置还在缓慢漂移现象设备静止不动组合导航输出的速度在 0.1~0.5 m/s 抖动位置有缓慢漂移。原因R_vel给太小滤波器对 GNSS 速度噪声的建模过于乐观导致速度量测噪声被当成真实速度输入激发了位置漂移。解决把R_vel适当调大。如果静态 GNSS 速度噪声标准差是 0.05 m/s方差就是 0.0025 m²/s²那R_vel至少给到 0.01。同时可以在代码里加一个简单静态检测当 GNSS 速度持续低于 0.2 m/s 时把速度观测权重视为 0只更新位置。方式是在更新前判断if norm(gnss_vel) 0.2 [X, P] update(X, P, z, H, R); else % 进入近静态只更新位置不更新速度 end5.4 现象四Matlab 脚本慢到怀疑人生现象数据只有 30 分钟循环跑了很久CPU 满载但内存不涨。原因循环里出现inv(S)、实时绘图、或者在每步都对 15×15 矩阵重新做数值积分。Matlab 对大型稀疏矩阵的优化已经很好但 15×15 矩阵如果反复用inv会拖慢整体速度。解决把inv(S)改成/把循环内的plot全部挪到循环结束统一画图。F矩阵中不随时间变化的重力相关项可以直接预计算不用每步重建。还有一个小技巧把主循环写成for而不是while并预分配所有输出矩阵Matlab 执行速度会有明显提升。5.5 现象五位置终于收敛但始终有个固定偏置现象滤波曲线很漂亮RMSE 也不大但轨迹整体偏移了几米和真实轨迹不重合。原因坐标系基准不一致。GNSS 给的是 WGS-84 经纬高IMU 机械编排在 ENU 局部坐标系如果程序包在转换时用了错误的投影原点固定偏置就会出现。解决先确认程序是全局坐标还是局部坐标。松组合程序通常选第一个 GNSS 点为 ENU 原点所有 GNSS 位置都转换到这个原点IMU 的初始位置也必须设为同一原点。转换代码里还得注意椭球参数要用 WGS-84 的a6378137、f1/298.257223563一旦选错偏置就是几十米量级。6. 把松组合程序和验证工具焊在一起加一个误差统计函数跑通松组合只是第一步真正能在项目汇报里拿出手的是一组可解释的定位精度指标。我习惯在程序包里加一个独立脚本统计 NED 三轴 RMSE、最大误差和 PE95 分位数。function stat compute_ned_stat(err_ned) % err_ned: N x 3 矩阵每行表示 N/E/D 误差 if isempty(err_ned) error(误差序列为空); end stat.rmse sqrt(mean(err_ned.^2, 1)); stat.mean mean(err_ned, 1); stat.max max(abs(err_ned), [], 1); stat.pe95 quantile(abs(err_ned), 0.95, 1); fprintf(RMSE: N %.3f m, E %.3f m, D %.3f m\n, ... stat.rmse(1), stat.rmse(2), stat.rmse(3)); fprintf(PE95: N %.3f m, E %.3f m, D %.3f m\n, ... stat.pe95(1), stat.pe95(2), stat.pe95(3)); end逻辑说明abs(err_ned)再取quantile(..., 0.95)得到的PE95表示 95% 的误差都小于该值比只报 RMSE 更能反映真实工程边界。有了这个统计函数还要有足够可信的参考基准。如果数据采集中同时记录了真实值或高精度 RTK 轨迹那误差序列就是组合结果减去参考值。如果只有 GNSS 单点解可以用滤波后的 GNSS 平滑结果做相对评估但不要把它当绝对真值。参数说明导入误差序列前要先剔除粗差和无效 GNSS 段。比如程序里检测到卫星失锁或定位状态码低于 4 的时段就直接从误差序列里去掉。否则统计结果会被个别跳变点拉高。验证工作中我最推荐一个小技巧恒定延迟扫描。把 GNSS 时间戳从 -0.2s 到 0.2s 以 0.05s 为步长平移每次都跑一遍松组合滤波记录水平 RMSE画出“延迟-误差”曲线。曲线最低点所对应的延迟就是当前设备链路的实际输出延迟。某次调试模拟项目 X 时GNSS 延迟 120ms我用延迟扫描把水平 RMSE 从 2.1m 降到了 0.8m效果立竿见影。从那以后每次拿到新的 GNSS/INS 数据我都会强制走一遍延迟扫描再开始调 Q 和 R这能替我省掉一半“滤波器参数玄学”的时间。希望这套松组合 Matlab 程序的拆解思路能帮你更快跑通自己的定位工程。本文还有配套的精品资源点击获取