简介《GNSS/INS松组合导航Matlab程序》面向导航、自动驾驶与航空航天方向的学习者和算法验证人员围绕GNSS与INS松组合的数据融合、惯导解算与卡尔曼滤波展开适合已有一定Matlab基础、希望理解组合导航实现流程的读者。压缩包共30个文件约6.49MB以26个.m脚本为核心配合2个.mat数据文件及.imu、.asv等辅助文件覆盖轨迹与IMU数据、滤波初始化、时间更新、量测更新、姿态四元数运算和结果绘图等环节。已有2485人学习说明其在相关课程设计与科研入门中具有一定参考度。读者可据此梳理从GNSS轨迹生成模拟惯导数据、到INS解算和松组合滤波验证的完整链路并借助脚本调试与定制算法积累复杂环境定位问题的排错与实验经验。1. GNSS/INS 松组合导航 Matlab 程序一套能跑通的组合导航验证环境做导航算法的人多半有过这种经历单独跑 GNSS 定位静止时点云乱飘单独跑 IMU 积分几十秒后位置就发散到没法看。把两者拼起来听起来简单实际动手才发现时间同步、坐标系对齐、卡尔曼滤波 Q/R 矩阵调参每一处都能让人卡上一整周。这套 GNSS/INS 松组合导航 Matlab 程序就是把这些环节串成一条可执行链路的工程化脚本从原始观测数据读入、惯导机械编排解算到 15 维误差状态卡尔曼滤波融合再到轨迹与误差曲线输出每个模块都能单独拿出来调试。它不适合当作产品级代码直接上車但作为课程设计、算法验证、论文复现的底稿比自己从零一步步搭要靠谱得多。下面按「这是什么—怎么跑起来—参数怎么设—坑在哪」的顺序拆开讲。2. 松组合的算法骨架为什么是 15 维误差状态卡尔曼滤波2.1 松组合与紧组合的选型分界松组合Loosely Coupled和紧组合Tightly Coupled最本质的差别在观测量的层次松组合拿 GNSS 接收机解算出来的位置、速度当量测也就是把接收机当成一个黑匣子只取它输出的 PVT 结果紧组合则直接吃伪距、伪距率原始观测卫星数少于 4 颗时仍能维持滤波。这套程序走的是前者原因并不玄学——松组合的状态量和量测模型都更干净量测方程是线性的H 矩阵直接就是[I3 0 ...]这种形式新手调试时能一眼看出问题出在滤波还是出在 GNSS。代价也要说清楚松组合的输出精度上限被接收机本身锁死GNSS 失锁瞬间滤波器就退化成纯惯导递推没有原始观测可用在城市峡谷、隧道这类场景下表现不如紧组合。选择它的典型理由是手上只有接收机输出的 NMEA 或 CSV 定位结果拿不到原始观测量或者研究重点是惯导误差模型与滤波结构本身不想被接收机内部环路干扰。这两条满足其一松组合就是合理起点。从工程链路看这套程序包含四个可独立验证的部分模块输入输出验证手段数据解析轨迹文件 / IMU 文件时间戳对齐的数组打印首尾行、检查采样率惯导机械编排陀螺、加计增量姿态、速度、位置递推静置段速度漂移量卡尔曼滤波惯导递推值 GNSS PVT误差状态估计与校正新息序列白噪声检验结果评估融合轨迹 参考轨迹RMSE、轨迹图、误差曲线与纯惯导对比这张表的用处在于分工排查。当融合结果不对时先断掉滤波让惯导单独递推几十秒看速度漂移是否在合理量级消费级 IMU 静置 30 秒速度漂移几十厘米到几米都算正常光纤陀螺应在厘米级如果惯导本身就发散那问题在机械编排或单位换算跟滤波器无关。这一步能省掉大量盲目调 Q/R 的时间。2.2 15 维误差状态的定义与递推程序采用误差状态卡尔曼滤波ESKF状态向量 15 维% 状态向量定义15x1 % x(1:3) 位置误差 (m)东北天坐标系 % x(4:6) 速度误差 (m/s) % x(7:9) 姿态误差 (rad)小角度近似 % x(10:12) 陀螺零偏 (rad/s) % x(13:15) 加计零偏 (m/s^2) x zeros(15,1); % 状态转移矩阵 F 的构建简化示意完整版按误差方程展开 F zeros(15,15); F(1:3,4:6) eye(3); % 位置误差对速度误差 F(4:6,7:9) skew(f_ib_n); % 速度误差对姿态误差f_ib_n 为比力在导航系投影 F(4:6,13:15) C_b_n; % 速度误差对加计零偏 F(7:9,10:12) -C_b_n; % 姿态误差对陀螺零偏 % 其余块按当地重力、地球自转角速度、曲率半径展开 % 离散化一阶近似 Phi eye(15) F * dt;这段代码的核心在F(4:6,7:9) skew(f_ib_n)。姿态误差会通过比力耦合进速度误差而skew()是比力的反对称矩阵function S skew(v) S [ 0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0 ]; end参数说明与常见改法dt必须与 IMU 采样周期严格一致若 IMU 是 100 Hz 而 GNSS 是 1 Hz惯导递推走 0.01 s 步长量测更新每 100 步触发一次。有些实现图省事用一阶离散化在dt较大或载体机动剧烈时会有明显截断误差常见做法是改用expm(F*dt)或者二阶泰勒展开I F*dt 0.5*(F*dt)^2代价只是每步多一次矩阵乘法现代机器上完全感知不到。误差状态的定义顺序不能随意打乱因为后面 H 矩阵、噪声矩阵都按这个顺序填充如果中途改了顺序却没同步改 Q 矩阵的块对角结构结果会表现为「滤波器收敛但收敛到错误值」这是最难查的一类问题。2.3 量测更新与反馈校正GNSS 提供位置和速度时的量测方程% z H*x v量测为 GNSS 位置速度与惯导递推值之差 H zeros(6,15); H(1:3,1:3) eye(3); % 位置量测对应位置误差 H(4:6,4:6) eye(3); % 速度量测对应速度误差 % 新息 z [pos_gnss - pos_ins; vel_gnss - vel_ins]; % 量测噪声阵pos_sigma / vel_sigma 来自接收机指标或实测统计 R diag([pos_sigma^2*ones(3,1); vel_sigma^2*ones(3,1)]); % 标准卡尔曼更新 K P * H / (H * P * H R); dx K * z; P (eye(15) - K*H) * P * (eye(15) - K*H) K*R*K; % Joseph 形式保证正定这里有两个容易被忽略的细节。第一R不用inv()而用左除数值稳定性更好尤其在R接近奇异时某些接收机速度输出精度填得极小。第二协方差更新用了 Joseph 形式而非(I-KH)P后者在长时间递推中会因舍入误差失去对称正定性症状是跑几十分钟后滤波突然崩掉、协方差出现负特征值用 Joseph 形式能把这个隐患压下去。状态反馈有两种流派每次量测更新后立刻把dx反馈到惯导解算值上并清零误差状态闭环校正或者只在输出时做补偿、误差状态持续累积开环。程序默认走闭环反馈代码大致是% 反馈校正注意姿态用旋转矩阵左乘而非直接相加 pos_ins pos_ins - dx(1:3); vel_ins vel_ins - dx(4:6); C_b_n (eye(3) - skew(dx(7:9))) * C_b_n; % 小角度修正 omega_bias omega_bias dx(10:12); acc_bias acc_bias dx(13:15); x zeros(15,1); % 反馈后误差状态清零姿态修正用(I - skew(dx)) * C而不是C dx直接加因为姿态误差是旋转意义上的小量直接加到欧拉角上会在跨 ±180° 时出现跳变这个坑很多人踩过。反馈之后必须清零误差状态否则下一轮量测更新会把已经补偿过的误差再算一遍表现为轨迹在每次 GNSS 更新点出现周期性抖动——如果你看到轨迹像台阶一样一顿一顿的先查这里。3. 从数据到轨迹把程序跑起来的三步操作3.1 数据准备与时间戳对齐程序对输入数据的要求是两条轨迹文件IMU 数据时间、三轴陀螺、三轴加计和 GNSS 数据时间、纬度经度高度、东北天速度。原始数据常见两种来源公开数据集导出的 txt/csv或者自采设备录的二进制包转成的文本。格式不统一是常态所以第一步永远是写一个适配脚本把列对齐。% 读取 IMU 数据假设为逗号分隔首行为表头 imu_raw readmatrix(imu_data.csv, NumHeaderLines, 1); t_imu imu_raw(:,1); % 单位秒从 0 开始 gyro imu_raw(:,2:4); % rad/s acc imu_raw(:,5:7); % m/s^2 % 读取 GNSS 数据 gnss_raw readmatrix(gnss_data.csv, NumHeaderLines, 1); t_gnss gnss_raw(:,1); pos_gnss gnss_raw(:,2:4); % 若原始为经纬高需先转东北天 vel_gnss gnss_raw(:,5:7); % 检查采样率一致性 fprintf(IMU 采样间隔均值: %.4f s\n, mean(diff(t_imu))); fprintf(GNSS 采样间隔均值: %.4f s\n, mean(diff(t_gnss)));跑完先看这两行打印。IMU 标称 100 Hz 却打印出 0.0098 或 0.0104 之类的均值说明设备时钟有抖动直接按固定dt0.01递推会累积时间偏差跑十分钟能差出几百毫秒常见做法是用实际时间戳做变步长递推或者先对时间轴做线性重采样到严格等间隔。GNSS 那边如果出现间隔均值 1.0 但中间夹杂 0.2、3.0 的跳变说明有丢星或重复帧需要在解析阶段剔除。如果原始 GNSS 是经纬高转东北天的参考点要选第一个有效定位点并且全程不能变% 经纬高转东北天局部切平面近似 lat0 deg2rad(lat(1)); lon0 deg2rad(lon(1)); h0 h(1); R_N 6378137; e2 0.00669437999014; Rm R_N*(1-e2) / (1-e2*sin(lat0)^2)^1.5; % 子午圈曲率半径 Rn R_N / sqrt(1-e2*sin(lat0)^2); % 卯酉圈曲率半径 E (deg2rad(lon) - lon0) .* (Rn h) .* cos(lat0); N (deg2rad(lat) - lat0) .* (Rm h); U h - h0;注意Rm、Rn用的是参考点纬度算出的固定值在几公里范围内误差可以忽略但如果轨迹跨度超过几十公里这种平面近似会引入米级系统偏差合格从业者此时会改用完整的大地坐标转 ECEF 再转 ENU 的链路。这段代码没写在程序主线里而是单独放在coord_transform.m就是为了让你方便替换成高精度版本。3.2 初始对准与滤波初始化惯导要有个起点才能递推程序里初始姿态、初始位置速度都从第一帧 GNSS 和一段静置数据获取。静止对准是最简方案% 取前 30 秒静止段做粗对准 idx_static t_imu 30; gyro_bias0 mean(gyro(idx_static,:), 1); % 陀螺零偏初值 f_mean mean(acc(idx_static,:), 1); % 比力均值 % 由比力确定横滚角与俯仰角导航系为东北天 roll0 atan2(f_mean(2), f_mean(3)); pitch0 atan2(-f_mean(1), sqrt(f_mean(2)^2 f_mean(3)^2)); % 航向由 GNSS 双天线或首段运动轨迹确定此处先置零待运动中对准 yaw0 0; C_b_n euler2dcm(roll0, pitch0, yaw0);atan2而不是asin是血泪经验asin在俯仰接近 ±90° 时对输入误差极其敏感且丢失象限信息用atan2配sqrt形式能稳很多。陀螺零偏取静置段均值这一步不能省消费级 MEMS 陀螺零偏常在 0.1~1 °/s 量级不扣掉的话姿态几分钟就漂没影了。滤波初始化几个量% 初始协方差按设备指标填 P diag([1^2*ones(3,1); % 位置 1 m 0.1^2*ones(3,1); % 速度 0.1 m/s (1*pi/180)^2*ones(3,1); % 姿态 1 deg (0.5*pi/180/3600)^2*ones(3,1); % 陀螺零偏 (1e-3*9.8)^2*ones(3,1)]); % 加计零偏 % 过程噪声按噪声密度乘采样时间构造 % 数值需按实际 IMU 的 datasheet 或 Allan 方差分析结果填写 Q diag([zeros(3,1); zeros(3,1); ... (gyro_arw*dt)^2*ones(3,1); ... (gyro_bias_instab*dt)^2*ones(3,1); ... (acc_vrw*dt)^2*ones(3,1)]);P的初值给大了滤波前几十秒会大幅修正、轨迹抖给小了滤波器对新量测不信任、收敛慢。经验做法是先按设备手册的标称值填跑完之后看新息序列——新息均值应接近零、标准差与H*P*HR的对角元同量级如果新息系统性偏大说明P或Q给小了。3.3 主循环组织与结果可视化主循环的骨架决定了整个程序的可调试性。推荐按 IMU 步长推进、在 GNSS 时刻插桩的结构k_gnss 1; for k 2:length(t_imu) dt t_imu(k) - t_imu(k-1); % 1. 惯导机械编排递推 [C_b_n, vel_ins, pos_ins] ins_mechanization( ... C_b_n, vel_ins, pos_ins, gyro(k,:) - omega_bias, ... acc(k,:) - acc_bias, dt); % 2. 协方差传播 F build_F(C_b_n, acc(k,:), vel_ins); % 按 2.2 节结构展开 Phi eye(15) F*dt; P Phi*P*Phi Q; % 3. 到 GNSS 时刻做量测更新 if k_gnss length(t_gnss) t_imu(k) t_gnss(k_gnss) [dx, P] kf_update(P, pos_ins, vel_ins, ... pos_gnss(k_gnss,:), vel_gnss(k_gnss,:)); [C_b_n, vel_ins, pos_ins, omega_bias, acc_bias] ... feedback_correct(C_b_n, vel_ins, pos_ins, ... omega_bias, acc_bias, dx); k_gnss k_gnss 1; end % 4. 记录 traj_est(k,:) [pos_ins, vel_ins]; end这个结构的价值在第 3 步的判断条件t_imu(k) t_gnss(k_gnss)。不少人直接用下标整除判断mod(k,100)0一旦 GNSS 有丢帧或时间戳不均匀就全错位了。用时间戳比较虽然多一次判断但容错性好得多。另外注意滤波更新里传的是当前惯导递推值而不是上一次的缓存值顺序搞反会造成一步的系统性滞后。结果对比建议至少画三张图东北平面轨迹叠加参考轨迹、位置误差随时间曲线、姿态误差曲线。轨迹图能一眼看出整体形状对不对误差曲线看有没有发散趋势姿态曲线看航向是否缓慢漂移——如果航向呈单调漂移那是陀螺零偏没估准或航向不可观测纯 GNSS 位置速度对航向的约束很弱静止或直线运动时尤其如此这时要么引入磁力计要么靠机动转弯让航向变得可观测。4. 参数调不对就白跑Q/R 矩阵与滤波发散的排查4.1 过程噪声与量测噪声的量级设定Q 和 R 的关系决定了滤波器信任谁。一个实用判断法Q反映 IMU 的随机误差强度R反映 GNSS 的位置速度精度。拿常见的消费级 IMU 配单点 GNSS 举例位置精度 2~5 m速度精度 0.1~0.3 m/s那么pos_sigma填 3、vel_sigma填 0.2 就是合理起点如果用的是 RTK 固定解位置精度可以到厘米级pos_sigma填 0.02此时滤波器会大幅依赖 GNSS惯导只起平滑作用。% R 矩阵按场景切换的常见写法 switch gnss_mode case single % 单点定位 pos_sigma 3.0; vel_sigma 0.3; case float % 浮点解 pos_sigma 0.5; vel_sigma 0.1; case fixed % 固定解 pos_sigma 0.02; vel_sigma 0.02; end R diag([pos_sigma^2*ones(3,1); vel_sigma^2*ones(3,1)]);实际数据里 GNSS 精度是时变的固定 R 会让滤波器在信号差的时段过度信任 GNSS。合格做法是利用接收机输出的精度因子HDOP/VDOP或解算状态标志动态缩放 R固定解时段把pos_sigma压到 0.02浮点解放大到 0.5单点解放大到 3 以上。程序里预留了gnss_mode字段的位置但默认是常数需要自己按数据里的解状态列去填。Q 的调节更微妙。调大 Q 表示不信任惯导模型滤波器会更激进地跟随 GNSS轨迹平滑度下降调小 Q 表示信任惯导短期平滑好但 GNSS 长时间失锁后恢复慢。常见翻车是 Q 给小了导致滤波器「忘记」自己漂了重新有 GNSS 时修正缓慢表现为失锁恢复后几十秒内误差都压不下去。判断方法很直接看新息序列如果失锁恢复瞬间新息突然增大到几十米且持续衰减说明 Q 偏小如果新息一直在噪声水平附近但误差照样发散那问题不在 Q 而在机械编排。4.2 姿态误差与航向可观测性纯松组合下航向角的可观测性依赖载体的运动。静止或匀速直线运动时GNSS 位置速度对航向基本没有约束力滤波器里的航向误差协方差降不下来姿态误差曲线会显示航向缓慢漂移而横滚俯仰收敛良好。这不是 bug是系统本身的可观测性问题。缓解手段有三条一是利用载体转弯时的向心加速度让航向变得可观测所以验证数据里最好包含几个转弯段二是在初始阶段做运动对准用一段已知轨迹反推初始航向三是如果硬件上有磁力计或双天线 GNSS直接引入航向量测把H矩阵扩到 7 行或 8 行。程序默认不含磁力计接口但kf_update函数是按通用形式写的扩维只需要改H和R。4.3 时间同步与杆臂补偿两个常被忽略的系统误差源。时间同步方面GNSS 秒脉冲和 IMU 采样往往有几十毫秒的固定延迟在高速运动时 20 ms 的时间错位能造成米级位置误差。程序里没有做硬件同步只按时间戳最近邻匹配这个精度对低速车载够用对无人机或高速场景就不够了需要在数据预处理阶段估计并补偿固定时延。杆臂补偿指 GNSS 天线相位中心与 IMU 敏感中心不重合两者之间有几米到几十米的基线。载体转动时这个基线会带来额外的速度量测偏差% 杆臂补偿lever_arm 为天线相对 IMU 在体系下的位置 vel_gnss_comp vel_gnss C_b_n * cross(omega_ib_b, lever_arm);不做补偿时转弯段的水平位置误差会出现与角速率相关的周期性偏差直线上却看不出来——这是判断是否需要杆臂补偿的一个实用特征。lever_arm一般从安装图纸或标定报告拿没有的话可以用转弯段数据反算让补偿前后误差曲线方差最小。5. 避坑清单五条实际跑数据踩出来的记录5.1 轨迹整体偏移几百米现象融合轨迹形状和参考轨迹一致但整体平移了几百米误差曲线是常数偏置。原因经纬高转东北天时参考点选错或者中途换过参考点。另一种可能是 GNSS 数据本身用了不同的坐标框架比如未做七参数转换的两套基准。解决把参考点固定为第一个有效定位点并写进配置如果误差是旋转加平移的复合形式检查是否需要做基准转换。验证方法是单独画 GNSS 原始点看它和参考轨迹是否重合——如果不重合问题在数据预处理与滤波无关。5.2 滤波跑几百步后协方差报错或轨迹突跳现象程序运行到中途抛出矩阵非正定警告或轨迹在某时刻突然跳变之后持续发散。原因协方差更新用了(I-KH)P的简化形式长时间递推累积舍入误差导致 P 失去正定性。也可能因为某次量测的 R 填得过小K 接近奇异。解决协方差更新改 Joseph 形式对 R 加一个下限保护比如R max(R, 1e-6*eye(6))每次更新后强制对称化P (PP)/2。这几行开销极小能把长时间运行的稳定性拉上来。5.3 每次 GNSS 更新后轨迹出现台阶状抖动现象轨迹在 GNSS 更新时刻出现明显折角像台阶一样两个更新点之间平滑。原因反馈校正后没有清零误差状态下一轮把已补偿的误差又算了一遍或者反馈量符号搞反dx应该是从惯导值里减掉而不是加上。解决检查x zeros(15,1)是否在反馈之后执行检查反馈公式中位置速度用减号、零偏用加号零偏估计值是漂移量补偿时加到偏置修正量上。符号错的表现通常是轨迹迅速发散到无穷很好识别。5.4 航向角持续单向漂移现象横滚俯仰误差在几度以内航向误差随时间线性增长转弯后略有改善但继续漂。原因航向在直线运动下不可观测陀螺零偏的航向分量估不出来。解决验证数据里加入机动段静止段延长以便更准确地估计陀螺零偏初值必要时引入磁力计或双天线航向量测。不要试图靠调 Q 解决可观测性问题不是调参能绕过去的。5.5 静置段速度不为零而是缓慢爬升现象载体静止但融合速度输出从零缓慢增大到 0.1 m/s 以上。原因加计零偏未补偿或者机械编排里的重力补偿用了错误的纬度。重力模型如果直接用常数 9.8在不同纬度会带来 0.01~0.03 m/s² 的误差10 秒就能积累出 0.3 m/s。解决重力用随纬度变化的模型g 9.780327*(10.0053024*sin(lat)^2 - 0.0000058*sin(2*lat)^2)静置检测加零速修正ZUPT检测到静止时把速度量测设为零这一招对低成本 IMU 效果立竿见影。6. 一个实用技巧用新息序列当滤波器的听诊器调这套程序最省时间的习惯是不看轨迹图先看新息序列。新息z pos_gnss - pos_ins是滤波器唯一能「感知」外部世界的通道它的统计特性直接反映 Q/R 是否匹配新息表现含义处理方向均值接近 0标准差与H*P*HR同量级滤波器工作正常不用动均值系统性偏大单方向存在未建模偏差杆臂、时延、坐标偏移查系统误差而非调参标准差远大于理论值Q 或 R 与实际不符通常是 R 给小了先放大 R 试出现孤立尖峰后恢复GNSS 野值加卡方检验剔除长时间接近 0 后突然爆发滤波器过度自信P 塌缩给 P 加下限或调大 Q野值剔除的实现很轻量用新息协方差S H*P*HR算马氏距离超过 3 倍标准差就跳过这次量测S H*P*H R; d2 z / S * z; % 马氏距离平方 if d2 chi2_threshold % 6 维量测99% 置信度阈值约 16.8 K P*H / S; dx K*z; P (eye(15)-K*H)*P*(eye(15)-K*H) K*R*K; else % 跳过本次更新只保留惯导递推 fprintf(t%.2f 剔除野值, d2%.1f\n, t_imu(k), d2); endchi2_threshold按自由度查表6 维量测取 16.8 对应 99% 置信度。注意阈值不能设太小否则连续剔除会让滤波器长时间失去观测量、反而发散实测中如果剔除比例超过 5%说明 R 填小了或者数据质量有问题得回头看上游。我一般会先跑一遍不做野值剔除的版本把新息序列和±3σ包络画在同一张图上野值会明显跳出包络扫一眼就知道数据干净不干净。这个图我在每次换数据集时都强制走一遍比事后对着发散轨迹猜原因高效得多。程序里plot_innovation.m就是干这个的输入只需把每步的z和S存进数组。希望这套脚本能帮你少走几天弯路。本文还有配套的精品资源点击获取