简介这是一份面向导航技术研究者、研究生及自动驾驶、航空航天等领域工程师的GNSS/INS松组合导航Matlab程序用来解决单一GNSS易受遮挡、INS存在累积漂移的问题涵盖GNSS与惯性导航原理、数据模拟、卡尔曼滤波融合、惯导解算与组合结果分析等完整流程。压缩包共30个文件以26个m脚本为主另有2个mat数据文件、1个imu数据文件及1个asv备份整体仅6.49MB。m文件按功能分为轨迹生成、误差模拟、姿态/四元数运算、滤波更新及主程序等模块mat和imu数据可直接用于跑通仿真验证。目前已有2483人学习下载。通过学习代码可掌握松组合导航的数据融合策略、惯导解算更新步骤及误差抑制方法还能了解四元数、等效旋转矢量等姿态运算细节适合作为论文复现或组合导航算法入门的基础工具。1. GNSS/INS松组合导航不是跑通就完事很多同学拿到这套Matlab程序第一步都是赶紧跑仿真、看曲线结果一上来就被坐标系和噪声参数搞懵东向位置误差飘到几十米还以为是代码错了。这套松组合导航程序真正值钱的地方是它把惯导机械编排、GNSS量测更新和扩展卡尔曼滤波EKF三件事串成了一个闭环能让你在Matlab里直接复现“GNSS每秒钟给一个位置、INS以100Hz推算姿态速度”的工程场景。适合正在做组合导航课程设计、论文仿真或者刚接触捷联惯导的从业者。我自己拆完这套源码后最大的感受是程序能跑只算第一步能把它调稳才是本事。2. 先拆清楚松组合的链路从IMU数据到状态估计2.1 为什么工程上首选松组合而不是紧组合GNSS/INS融合在工程上最常见的有两派做法松组合和紧组合这套Matlab程序实现的是松组合。松组合的含义很直白GNSS接收机先自己在内部完成伪距定位输出位置和速度一般是经纬高和东北天速度然后组合导航滤波器把这组输出当作量测去修正INS递推出来的导航解。紧组合则是把伪距、伪距率原始观测直接送进滤波器。工程上首选松组合核心原因是它把两个系统解耦了。GNSS部分不需要知道INS的状态方程INS部分也不关心GNSS内部用了什么定位算法两边各出各的在滤波层做“最后一次融合”。这种结构天然适合快速集成GNSS接收机换一个品牌只要输出协议是NMEA或者厂商二进制位置、速度接口不变组合导航程序基本不用重写。而紧组合要和接收机内部打交道得拿到原始观测数据不少接收机是不开放伪距输出的所以紧组合在这套资源里不属于首选方案也不适合初学者第一版去实现。从计算量上说松组合的状态向量一般是15维姿态误差3维、速度误差3维、位置误差3维、陀螺零偏3维、加计零偏3维量测向量只有6维位置3维、速度3维。这套程序里的卡尔曼滤波在100Hz机械编排后做1Hz量测更新Matlab跑一遍完整仿真耗时和内存占用都在可控范围这也是很多课程设计愿意选择松组合的原因。2.2 状态方程怎么建15维状态到底在估计什么这套程序的状态方程建立在惯导误差传播模型上也就是常说的psi角误差模型。姿态误差用失准角来定义不是直接用欧拉角好处是误差方程在小失准角假设下可以线性化这是卡尔曼滤波能够使用的前提。% 状态向量定义15维误差状态 % [失准角(3); 速度误差(3); 位置误差(3); 陀螺零偏(3); 加计零偏(3)] % 状态转移矩阵 F 的关键子块示意 F_aa -skew(omega_ie_n); % 姿态误差与地球自转角速度耦合 F_av eye(3); % 姿态误差对速度误差的影响 F_vv -(skew(vel_n) * skew(phi)); % 速度误差与比力/位置耦合 F_pp [0 1/(R_Mh) 0; 1/((R_Nh)*cos(lat)) 0 0; 0 0 -1];这段代码描述的是系统矩阵中三个最容易被写错的子块。F_aa里的skew操作是把向量转换为反对称矩阵也就是叉乘的矩阵形式姿态误差随时间传播时会和地球自转角速度耦合。F_pp用的是当地曲率半径R_M和卯酉圈曲率半径R_N这两项决定了经纬度位置误差和速度误差之间的几何关系。程序里这两个曲率半径的计算用的是WGS-84椭球模型不是简单地把地球当球体处理这一点比很多教材里给的球形近似要准确。参数上R_M取的是子午圈曲率半径R_N是卯酉圈曲率半径对应位置状态里经度和纬度误差的不同传播路径两者混用会导致北向和东向位置误差的耦合关系出错。2.3 量测方程怎么配先看程序处理的是NED还是ECEF松组合程序最容易翻车的地方之一是GNSS量测到底在哪个坐标系下的。这套Matlab程序里GNSS输出的经纬高先被转成NED北东地坐标系下的位置增量然后量测矩阵H对应的是位置误差和速度误差。NED和ECEF的区别在于ECEF是地心地固系NED是载体当地的切平面坐标系。组合导航里INS输出一般习惯放在NED下导航解算是用方向余弦矩阵从载体系投影到NED系因此GNSS量测也要投影到同一个NED系里做差。% 量测矩阵 H位置量测 速度量测对应NED坐标系 H zeros(6, 15); H(1:3, 7:9) eye(3); % 位置误差直接可观测 H(4:6, 4:6) eye(3); % 速度误差直接可观测 % 注意程序中GNSS位置先用geodetic2ned转成NED增量再进入滤波量测矩阵的设置逻辑很清楚位置量测对应状态里的位置误差项速度量测对应速度误差项。程序里GNSS的经纬高和INS推算的经纬高先做差再转到NED下这一步的顺序会影响H矩阵的写法。如果GNSS位置直接以经纬高形式进入滤波器H矩阵就需要经过一个坐标转换矩阵和直接使用NED增量是不同的。使用这套程序时必须先确认GNSS数据的预处理输出是NED增量否则量测更新时量测新息会被单位或坐标系搞错表现出来就是滤波发散。3. 跑通这套组合导航从文件结构到IMU数据模拟3.1 程序包里需要关注的脚本和数据结构拿到这套程序后先别急着运行主函数把文件结构扫一遍。通常来说一套完整的松组合Matlab程序会包含IMU数据模拟器、GNSS轨迹生成器、捷联惯导机械编排函数、卡尔曼滤波器函数和主脚本。主脚本负责把数据流串起来IMU模拟器输出加速度和角速度机械编排函数做姿态解算滤波器做误差修正。project_root/ ├── main_loose_gnss_ins.m % 主脚本设置仿真参数并循环运行 ├── ins_mechanization.m % 捷联惯导机械编排函数 ├── ekf_update.m % 卡尔曼滤波量测更新 ├── ekf_predict.m % 卡尔曼滤波状态预测 ├── imu_simulator.m % 模拟IMU输出加计陀螺 ├── gnss_simulator.m % 模拟GNSS位置速度输出 ├── trajectory_gen.m % 生成参考轨迹直线/转弯/爬升 └── config_params.m % 参数配置文件这个文件结构是这类程序的常见分工方式。IMU模拟器里会生成加速度计和陀螺仪的原始输出模拟时会在真实比力和角速度上叠加强度可调的噪声和零偏GNSS模拟器根据参考轨迹叠加高斯白噪声模拟接收机的位置和速度输出。主脚本里会设置IMU的数据率通常是100HzGNSS的数据率通常是1Hz到10Hz而滤波器在量测到达的历元才做一次更新这也是松组合和紧组合在计算流程上的一个差别。3.2 机械编排里最容易写错的姿态更新机械编排是INS的核心这套程序里姿态更新用的是四元数方法不是方向余弦矩阵直接积分。四元数相比欧拉角最大的好处是没有奇异性在俯仰角接近正负90度时不会出现万向锁问题相比方向余弦矩阵四元数的计算量更小每次更新只需要几次乘加运算。function [Cbn_new, q_new] attitude_update(Cbn_old, q_old, gyro, dt) omega_ib_b gyro; % 载体系角速度 omega_in_n earth_rate_n nav_rate_n; % 导航系相对惯性系角速度 omega_bn_b omega_ib_b - Cbn_old * omega_in_n; % 等效旋转矢量 delta_theta omega_bn_b * dt; % 角增量 q_delta [cos(norm(delta_theta)/2); sin(norm(delta_theta)/2) * delta_theta / norm(delta_theta)]; q_new quat_multiply(q_old, q_delta); Cbn_new quat2dcm(q_new); end姿态更新分三步先算出载体系相对导航系的角速度再转成等效旋转矢量最后用四元数乘法完成更新。程序里用到了地球自转角速度在导航系的分量earth_rate_n这一项是必须保留的它描述了地球自转对姿态的影响在长时间导航时尤其明显舍不得加这项会导致航向角缓慢漂移。nav_rate_n是载体运动引起的导航系旋转来源于载体在地球表面移动时的曲率效应。两个分量合起来才是完整的“导航系相对惯性系旋转角速度”。如果只保留地球自转项而不加运动项在车辆以较高速度行驶时航向误差会有可见的累积这种情况在车载组合导航中经常遇到。3.3 IMU数据模拟噪声参数决定了你的仿真可信度这套程序里IMU模拟器的噪声模型一般由两个部分组成白噪声和零偏稳定性。白噪声用均值为零、方差为参数的高斯随机数生成陀螺白噪声的单位是deg/sqrt(h)加计白噪声的单位是ug/sqrt(Hz)。零偏则用一个一阶高斯马尔可夫过程来模拟或者直接用常数零偏再加随机游走两种做法对应不同精度等级的惯性器件。% 陀螺仪模拟参数 gyro_noise_density 0.01; % deg/sqrt(hr) gyro_bias_instability 0.5; % deg/hr gyro_bias gyro_bias_instability * pi / 180 / 3600; % 转成 rad/s % 加计模拟参数 accel_noise_density 100; % ug/sqrt(Hz) accel_bias 0.3 * 1e-3 * 9.8; % 0.3mg 转成 m/s^2噪声参数的单位换算是一个高频雷区。陀螺零偏0.5deg/hr不能直接用必须换算成rad/s加计零偏0.3mg也必须换算成m/s^2。很多同学仿真结果出现明显位置漂移不是因为算法写错了而是因为参数单位没有预处理。这套程序如果提供了参数配置文件你需要自己核对一遍单位转换是否完整。IMU数据模拟的质量直接决定了后续滤波效果的“真实上限”——模拟器生成的IMU数据噪声过小滤波器会表现出过度自信噪声过大滤波结果和纯惯导比没有改善这两种情况都无法说明你的组合导航算法正确。4. 调参实战把位置误差从几十米压到几米4.1 过程噪声和量测噪声到底取多少卡尔曼滤波在松组合里取得好坏过程噪声和量测噪声的比值是关键。过程噪声反映你对IMU误差模型的信任程度量测噪声反映你对GNSS输出的信任程度。这两个噪声矩阵各差一个量级滤波结果就会有不同的特性。% 过程噪声连续系统需要离散化 Q_gyro_noise (gyro_noise_density * pi / 180 / 3600)^2 * eye(3); Q_accel_noise (accel_noise_density * 1e-6 * 9.8)^2 * eye(3); Q diag([zeros(3,1); zeros(3,1); zeros(3,1); diag(Q_gyro_noise); diag(Q_accel_noise)]); % 量测噪声 R_pos (3^2) * eye(3); % GNSS位置标准差 3米 R_vel (0.05^2) * eye(3); % GNSS速度标准差 0.05 m/s R blkdiag(R_pos, R_vel);过程噪声只加在陀螺和加计的零偏状态上姿态误差、速度误差、位置误差通过状态转移矩阵自然传播不需要额外加噪声这是常规做法。量测噪声的取值参考GNSS接收机的标称精度普通单频接收机位置误差在3米左右速度误差在0.05米每秒左右如果程序模拟的是差分GNSS位置误差可以压到0.5米速度误差压到0.02米每秒。把这组参数输入滤波器后位置的稳态误差应该能控制在几米范围内如果跑出来误差还是几十米那问题大概率不在噪声参数上而是出在状态转移矩阵或量测矩阵中。4.2 协方差初值给滤波器一个合理的起点滤波器的初始协方差矩阵P0代表你对初始状态误差的信任程度。程序里如果所有初始误差都为0P0可以取一个很小的值。但在实际场景中GPS/INS组合导航在启动时初始姿态误差往往是未知的特别是航向角可能出现几度的误差因此P0需要给得偏保守。% 初始协方差 P0 zeros(15, 15); P0(1:3, 1:3) (1 * pi / 180)^2 * eye(3); % 初始失准角1度 P0(4:6, 4:6) (0.1)^2 * eye(3); % 初始速度误差0.1 m/s P0(7:9, 7:9) (10)^2 * eye(3); % 初始位置误差10米 P0(10:12, 10:12) (0.5 * pi / 180 / 3600)^2 * eye(3); % 陀螺零偏 P0(13:15, 13:15) (1e-3 * 9.8)^2 * eye(3); % 加计零偏初始航向误差给1度是一个合适的值。如果P0给得太小滤波器会以为初始状态是完美的航向误差回到真实值要经过很长时间如果P0给得太大滤波初期会出现一个明显的振荡位置误差快速波动。两种情况的折中方案就是按照器件特性给P0。程序里如果能够在跑完一圈后检查P矩阵对角线元素是否收敛就能直观看到哪些状态可观测性强哪些状态是“弱可观测”的。4.3 量测更新是1Hz怎么和100Hz机械编排衔接程序里机械编排是100HzGNSS量测是1Hz这意味着大多数导航解是无量测更新的纯惯导推算结果只有每秒一次的量测更新来修正误差。这段衔接代码是整套程序里最容易因为时序问题出错的地方。for k 2:length(t_imu) % 机械编排每个IMU周期都执行 [Cbn, vel_n, pos] ins_mechanization(Cbn, vel_n, pos, imu_data(k, :), dt_imu); % 判断当前时刻是否有GNSS量测 if mod(t_imu(k), dt_gnss) 0 % 有量测则执行滤波更新 [state, P] ekf_update(state, P, z_gnss, H, R); % 反馈修正机械编排结果 [Cbn, vel_n, pos, state] feedback_correction(Cbn, vel_n, pos, state); end end这个流程的关键在于反馈修正。每次量测更新后滤波器估计出的姿态误差、速度误差、位置误差要反馈到机械编排的结果上把INS的推算值“拉回”到真实值附近然后状态向量中对应的误差项清零。这套程序里如果省略了反馈修正这一步滤波器估计出的误差会不断累积最后位置误差不是收敛而是振荡。反馈修正的频率和量测更新一致都是1Hz。反馈修正还有一个细节陀螺零偏和加计零偏的估计值也要反馈把IMU原始输出中的零偏扣除后再进入下一轮的机械编排。这会改变IMU数据的等效输入。两段式反馈结构是工程上的标准做法状态反馈给导航结果零偏反馈给IMU原始数据。5. 避坑指南这套松组合程序的五个常见问题5.1 航向角持续漂移GNSS修正后还是回不来现象位置和速度误差都收敛了航向角误差却一直没有收敛或者收敛得非常慢最终保持在1到2度的残差上。原因GNSS位置和速度量测对航向角的可观测性偏弱特别是在车辆直线行驶、没有转弯机动时。GNSS速度虽然包含航向信息但它只能确定水平速度方向不能直接确定载体航向与速度方向之间的侧滑角。直线行驶时航向误差和速度误差之间存在耦合滤波器无法区分。解决让模拟轨迹包含转弯和加速减速段这是提高航向可观测性的最有效手段。另外检查R矩阵中的速度量测噪声是否偏大速度量测噪声如果太大航向修正量被削弱。这套程序里默认轨迹如果几乎是直线航向不收敛是正常的不要误判为代码问题。5.2 滤波发散P矩阵对角线暴涨现象运行一段时间后位置误差跑到几百米协方差矩阵的某些对角线元素越来越大没有收敛趋势。原因通常是过程噪声Q设置偏大或者状态转移矩阵F的离散化出了问题。Q过大意味着滤波器对IMU模型非常不信任每一轮状态预测都大幅增加不确定性量测更新无法把协方差压回来最后协方差爆炸。解决先检查F矩阵离散化是否使用了正确的矩阵指数计算方式F*dt直接替代矩阵指数在高动态场景下误差被放大。程序里如果只用了一阶泰勒展开试着把IMU频率降低或者把场景改成低动态看看是否有所改善。Q矩阵里陀螺零偏和加计零偏的噪声量级需要重新评估对照IMU数据手册确认。5.3 机械编排几秒钟就飞掉纯惯导都稳不住现象不开启GNSS量测更新时惯导推算的位置和姿态在几秒内就出现明显漂移甚至直接发散。原因初始姿态没有对齐特别是初始失准角太大。捷联惯导系统本质上是一个积分器初始姿态误差会导致比力投影错误加速度积分到速度速度积分到位置误差以三次方时间多项式增长。解决用一个静止初始化算法对准初始姿态。常见的做法是取前几秒静止数据的平均比力利用比力方向反推初始俯仰和横滚角航向角则提供初值。程序中如果能跑一个静止时段的预对准纯惯导的稳定性会有明显改善。不少课程设计省略了这一步直接用给定值的姿态启动结果就是几秒内发散。5.4 GNSS量测更新跳变时位置出现锯齿现象位置误差曲线呈现锯齿状GNSS量测更新时误差突然跳回零点附近而过了一段时间误差又慢慢漂开下一个量测到来时再次跳变。原因这属于正常现象不是程序问题。松组合在两次GNSS量测之间没有绝对参考INS的误差随时间累积GNSS更新时被强制修正所以误差曲线看起来像锯齿。解决如果想观察真实的滤波平滑效果检查反馈修正策略。如果程序采用全状态反馈也就是位置、速度、姿态误差都反馈并清零锯齿会明显一些如果只反馈位置和速度保留姿态误差在滤波器状态中曲线会更平滑代价是姿态误差和位置误差之间会出现延迟。推荐观察位置误差的RMS和最大值不要单独看某一次量测附近的瞬时表现。5.5 使用的是某个厂商的IMU参数但程序的模拟参数不匹配现象换了自己的IMU参数后滤波结果比默认参数差很多位置误差反而增大了。原因默认参数可能是高精度惯导参数换成低精度MEMS后噪声和零偏都大一个数量级量测噪声不变的情况下滤波器对GNSS的信任权重过高导致过度修正。解决换IMU时同步调整Q矩阵中的陀螺和加计噪声项以及P0中的初始零偏不确定度。低精度IMU和GNSS融合时可以适当调大量测噪声R让滤波对量测的修正不会过于激进。这套程序的边界在于它假设GNSS和IMU时间同步已经处理过如果实际使用中GNSS和IMU的时间戳有偏差需要先在数据预处理阶段做对齐程序本身没有提供这一层处理。6. 用Allan方差验证你的惯性器件参数而不是拍脑袋填数调参这件事最忌讳凭空写一组陀螺和加计噪声然后期望滤波结果好看。工程上一个相对靠谱的做法是用Allan方差分析IMU的实测数据得到陀螺零偏稳定性和随机游走系数再把这两个参数反填到滤波器的Q矩阵中。虽然这套Matlab程序自带IMU模拟器不需要外接真实数据但这个验证思路仍然值得在仿真中复现。Allan方差的核心逻辑是把IMU静态输出按不同的积分时间分组计算每一组均值的方差然后对积分时间取对数作图。典型的Allan方差曲线分为几段斜率判断噪声类型斜率-1对应量化噪声斜率-0.5对应角速率随机游走斜率0对应零偏不稳定性斜率0.5对应速率随机游走。在导航应用中最关注的是随机游走系数和零偏不稳定性两项。function [tau, adev] allan_variance(data, fs) % data: IMU静态输出序列单位需保持连续rad/s 或 m/s^2 % fs: 采样频率 max_tau floor(length(data) / 10); tau logspace(0, log10(max_tau), 50); adev zeros(size(tau)); for i 1:length(tau) n round(tau(i) * fs); % 每组样本数 m floor(length(data) / n); if m 2 break; end chunk reshape(data(1:m*n), n, m); avg mean(chunk); % 每组均值 adev(i) sqrt(0.5 * mean(diff(avg).^2)); end end这段代码实现了基础的Allan方差计算。分组时把长度为N的数据切成m个长度为n的块计算每个块的均值再求相邻块均值差平方的均值的一半开方后就是该积分时间下的Allan标准差。tau从1秒取到数据长度的十分之一覆盖时间跨度要足够长才能看到零偏稳定性的“谷底”。仿真IMU数据做Allan方差分析时通常需要至少数小时静态数据才能得到稳定的长周期段短数据切出来的长tau点会跳得很厉害。拿到Allan方差曲线后零偏不稳定性对应曲线谷底的值约等于零偏噪声的等效幅度随机游走系数对应斜率-0.5段的纵轴读数。把这两个值和程序默认参数对比如果差到两倍以上说明模拟器的噪声模型和默认滤波参数不匹配。从那以后我每次拿到一组合导航程序都会把IMU噪声参数先过一遍Allan方差再做状态调参而不是直接跑滤波看曲线少走好多弯路。希望帮到你。本文还有配套的精品资源点击获取