简介这份资源面向导航算法学习者与工程实践者提供一套基于MATLAB的SINS与GPS组合导航仿真项目用于解决单一惯性导航随时间漂移、GPS在遮挡环境下易受干扰的定位精度问题。包内共7个文件约675KB以m脚本与asv备份文件承载核心融合算法mat数据文件提供仿真所需原始数据doc与txt则记录程序说明与位置组合结果便于对照理解。项目围绕卡尔曼滤波展开涵盖SINS加速度与角速率处理、GPS伪距信息融合、位置组合解算等环节运行后可生成位置轨迹对比与误差统计等分析图直观评估定位精度、收敛速度与稳定性。目前已有223人学习适合希望掌握数据融合技术、提升MATLAB编程能力并积累导航系统设计经验的读者参考。1. 从一组跑偏的轨迹说起SINS/GPS 组合导航到底在解决什么如果你把一套纯惯性导航SINS的数据静置在桌上跑十分钟再对比 GPS 输出的位置大概率会看到两条曲线从重合慢慢张开最后差出几百米甚至上公里。这不是程序写错了而是陀螺零偏和加速度计零偏随时间积分累积的必然结果——惯导短时精度极高、长时发散GPS 长时稳定、短时跳变且易受遮挡。组合导航要干的事就是用卡尔曼滤波把两者的优点缝在一起用 GPS 的位置/速度去持续校正 SINS 的姿态、速度和位置误差同时用 SINS 的高频输出填补 GPS 的更新间隙。这套东西在车载、无人机、水下航行器、农机自动驾驶里都是标配。标题里说的「内包含程序、需要的数据、运行后的分析图」本质就是一套可复现的 MATLAB 工程仿真轨迹生成、IMU/GPS 数据构造、卡尔曼滤波解算、误差曲线绘制。适合正在做毕设、课程设计或者要把算法从论文搬到工程里的同学。下面按「数据怎么造 → 滤波怎么写 → 图怎么看 → 坑在哪」的顺序拆开讲。2. 先搭仿真环境轨迹、IMU 和 GPS 数据怎么造出来真机数据难拿、成本高绝大多数组合导航验证都从仿真起步。核心思路是先设计一条已知的参考轨迹真值再正向推导出理想 IMU 和 GPS 观测最后人为叠加噪声和零偏得到「带误差的传感器数据」。这样你手里同时有真值和观测才能定量评估滤波效果。2.1 参考轨迹与地球参数初始化轨迹一般用东北天ENU坐标系描述姿态用欧拉角或四元数。地球参数别自己瞎填用 WGS-84 的标准值否则后续重力、地球自转补偿会系统性偏。% 地球与重力参数WGS-84 glv.g0 9.7803267714; % 赤道重力加速度 m/s^2 glv.Re 6378137.0; % 长半轴 m glv.f 1/298.257223563; % 扁率 glv.e2 glv.f*(2-glv.f); % 第一偏心率平方 glv.we 7.2921151467e-5; % 地球自转角速度 rad/s % 初始位置纬度、经度、高度 pos0 [34.25*pi/180, 108.95*pi/180, 400]; % 初始速度与姿态静止起步 vel0 [0; 0; 0]; att0 [0; 0; 0]; % 横滚、俯仰、航向 % 采样设置 ts 0.01; % IMU 采样 100Hz T 120; % 仿真时长 120s t 0:ts:T-ts; N length(t);这段是整个工程的底座。ts0.01对应 100Hz是常见 IMU 输出频率GPS 通常 1~10Hz后面单独降频。pos0用弧度制MATLAB 三角函数全部按弧度算混用角度是新手第一个翻车点。2.2 正向推导理想 IMU 输出给定参考轨迹通过微分反推比力加速度计输出和角速度陀螺输出。简化场景下可以先做二维平面运动再扩展到三维。% 设计一条加速-匀速-转弯的参考轨迹简化先做一维加速 vel_ref zeros(3, N); pos_ref zeros(3, N); acc_ref zeros(3, N); for k 2:N if t(k) 20 acc_ref(1,k) 1.0; % 前 20s 东向加速 1 m/s^2 elseif t(k) 80 acc_ref(1,k) 0; % 匀速 else acc_ref(1,k) -0.5; % 减速 end vel_ref(:,k) vel_ref(:,k-1) acc_ref(:,k)*ts; pos_ref(:,k) pos_ref(:,k-1) vel_ref(:,k)*ts; end % 理想陀螺/加速度计输出静止水平忽略地球自转补偿的简化版 gyro_ideal zeros(3, N); acce_ideal zeros(3, N); acce_ideal(1,:) acc_ref(1,:); acce_ideal(3,:) glv.g0; % 天向承受重力反力acce_ideal(3,:)填g0是关键加速度计静止时测到的是支撑力等于重力大小。很多人忘了这一项导致天向速度一直往下掉。真实工程里还要叠加地球自转、哥氏加速度、向心加速度补偿仿真阶段可以先简化但心里要清楚这是简化。2.3 叠加零偏、随机游走与 GPS 噪声理想数据没法验证滤波必须加误差源。陀螺零偏、加速度计零偏、角度随机游走、速度随机游走这四类是最基本的。rng(2024); % 固定随机种子保证可复现 % 零偏常值 慢变 gyro_bias (0.5*pi/180/3600) * ones(3,N); % 0.5 deg/h acce_bias 100e-6 * glv.g0 * ones(3,N); % 100 ug % 随机游走噪声 arw 0.01*pi/180/60; % 角度随机游走 vrw 0.001; % 速度随机游走 gyro_noise arw/sqrt(ts)*randn(3,N); acce_noise vrw/sqrt(ts)*randn(3,N); gyro_meas gyro_ideal gyro_bias gyro_noise; acce_meas acce_ideal acce_bias acce_noise; % GPS1Hz位置噪声 5m速度噪声 0.1m/s gps_idx 1:100:N; gps_pos pos_ref(:,gps_idx) 5*randn(3,length(gps_idx)); gps_vel vel_ref(:,gps_idx) 0.1*randn(3,length(gps_idx));rng(2024)这行别省否则每次跑出来的误差曲线都不一样调参时根本分不清是参数变了还是噪声变了。零偏单位换算要小心0.5*pi/180/3600是 0.5 度每小时转成 rad/s。GPS 用1:100:N降频到 1Hz和 100Hz 的 IMU 形成典型的「高频低频」组合。3. 卡尔曼滤波落地15 维状态怎么摆、量测怎么接组合导航的滤波核心是误差状态卡尔曼滤波间接法。直接对姿态、速度、位置做滤波会遇到姿态非线性问题误差状态法把状态定义成「真值减估计值」线性化更干净工程上几乎都用这套。3.1 状态量与误差方程15 维状态姿态误差3、速度误差3、位置误差3、陀螺零偏3、加速度计零偏3。零偏建模成随机游走能在线估计并补偿。% 状态datt(3) dvel(3) dpos(3) gbias(3) abias(3) % 连续时间误差方程离散化简化版忽略地球自转耦合项 F zeros(15,15); F(1:3, 1:3) -skew(gyro_meas(:,1)); % 姿态误差受陀螺影响 F(1:3, 10:12) -eye(3); % 陀螺零偏耦合 F(4:6, 1:3) skew(acce_meas(:,1)); % 速度误差受比力影响 F(4:6, 7:9) -eye(3); % 位置耦合 F(4:6, 13:15) eye(3); % 加计零偏耦合 F(7:9, 4:6) eye(3); % 位置误差 速度误差积分 % 离散化 Phi eye(15) F*ts; % 过程噪声简化对角阵 Q diag([ (arw)^2*ones(1,3), (vrw)^2*ones(1,3), ... (0.1)^2*ones(1,3), (1e-5)^2*ones(1,3), (1e-4)^2*ones(1,3) ]) * ts;skew()是反对称矩阵函数MATLAB 没有内置需要自己写三行。F(1:3,10:12)-eye(3)表示陀螺零偏会直接污染姿态误差这是零偏可观测性的来源。Q 矩阵的对角元素对应各状态的过程噪声强度调参时主要动这里。3.2 量测更新GPS 位置速度怎么进滤波GPS 提供位置和速度量测方程是线性的直接取状态里的位置、速度误差即可。H zeros(6,15); H(1:3, 7:9) eye(3); % 位置量测 H(4:6, 4:6) eye(3); % 速度量测 R diag([5^2*ones(1,3), 0.1^2*ones(1,3)]); % 与 GPS 噪声一致 % 量测残差GPS 观测 - 惯导推算 z [gps_pos(:,1) - pos_ins; gps_vel(:,1) - vel_ins]; % 卡尔曼增益与更新 K P * H / (H*P*H R); dx K * z; P (eye(15) - K*H) * P;R必须和 2.3 节里 GPS 噪声设置一致否则滤波增益会失真——R 设太小滤波过度信任 GPS轨迹跟着 GPS 噪声抖R 设太大GPS 校正力度不够惯导误差压不下去。z是残差工程上要监控它的均值长期不为零说明模型有系统偏差。3.3 闭环校正与反馈时机误差状态估计出来后要反馈回惯导解算否则误差越积越大。反馈时机有两种每个 IMU 周期都反馈闭环或者只在 GPS 更新时反馈半闭环。% 闭环反馈每个 IMU 周期用当前误差估计修正 att_ins att_ins - datt_est; vel_ins vel_ins - dvel_est; pos_ins pos_ins - dpos_est; gyro_meas gyro_meas - gbias_est; acce_meas acce_meas - abias_est; % 反馈后误差状态清零协方差保留 dx_est zeros(15,1);反馈后dx_est清零但P不清零这是标准做法。如果连 P 一起清零滤波器会「失忆」下次更新时增益异常。零偏反馈到测量值上相当于在线标定这是组合导航比纯惯导精度高的关键机制之一。4. 避坑与排查跑不出正确曲线时先看这几条仿真跑完发现轨迹发散、误差曲线不收敛、或者结果和论文对不上八成是下面几个问题。按「现象 → 原因 → 解决」逐条排查。现象一位置误差随时间线性发散GPS 完全拉不回来。原因通常是量测更新没生效或者 H 矩阵维度/位置写错。检查H(1:3,7:9)是否对应位置误差状态以及 GPS 更新循环是否真的在gps_idx时刻触发。另一个隐蔽原因是残差符号搞反z gps - ins和ins - gps会导致滤波往反方向修正。现象二姿态曲线高频抖动噪声比纯惯导还大。多半是 R 设得过小滤波过度信任 GPS。GPS 位置噪声 5m 对应 R 里应该是 25不是 5。另外检查过程噪声 Q 是否过小Q 太小会让滤波器认为模型极准拒绝接受量测修正。现象三零偏估计不收敛一直漂。零偏可观测性依赖机动。如果轨迹全程匀速直线陀螺零偏和姿态误差耦合在一起数学上不可分。解决办法是在轨迹里加入转弯和加减速机动让零偏暴露出来。这也是为什么标定要「转起来」而不是静置。现象四曲线整体偏移一个常值形状对但数值不对。检查单位。角度/弧度混用、度每小时和 rad/s 混用是最常见的。0.5*pi/180/3600这类换算建议写成带注释的表达式别直接写结果数字。重力补偿漏掉或符号反了也会造成天向常值偏移。现象五每次运行结果都不一样无法复现。忘了rng()固定种子或者噪声生成顺序在循环里被打乱。把所有随机数生成集中到数据构造阶段滤波阶段不再调用randn保证「数据固定、算法可复现」。5. 把分析图读透误差曲线、协方差与一致性检验图不是画出来好看就行每张图都要能回答一个具体问题。位置误差曲线看收敛速度和稳态精度速度误差看动态响应姿态误差看可观测性协方差曲线看滤波器是否「自信得合理」。5.1 三类核心分析图的画法与判读% 图1位置误差东/北/天 figure; plot(t, pos_err(1,:), r, t, pos_err(2,:), g, t, pos_err(3,:), b); xlabel(时间 (s)); ylabel(位置误差 (m)); legend(东向,北向,天向); grid on; title(位置误差曲线); % 图2姿态误差 figure; plot(t, att_err(1,:)*180/pi, t, att_err(2,:)*180/pi, t, att_err(3,:)*180/pi); xlabel(时间 (s)); ylabel(姿态误差 (deg)); legend(横滚,俯仰,航向); grid on; % 图3协方差 3-sigma 包络 figure; sigma 3*sqrt(squeeze(P_hist(7,7,:))); % 东向位置标准差 plot(t, pos_err(1,:), b, t, sigma, r--, t, -sigma, r--); xlabel(时间 (s)); ylabel(东向位置误差 (m)); legend(实际误差,3\sigma 包络); grid on;图1 判读初始几十秒误差应该快速下降然后趋于平稳稳态值在 GPS 噪声量级附近几米。如果一直下降不收敛说明过程噪声偏大如果平稳值远大于 GPS 噪声说明校正不足。图2 判读航向误差通常比横滚俯仰收敛慢因为航向可观测性最弱需要机动激励。如果航向误差一直不收敛回到第 4 章「现象三」检查轨迹机动性。图3 是很多人忽略的一致性检验实际误差应该大部分落在 3σ 包络内。如果实际误差频繁冲出包络说明 P 被低估滤波器过度自信需要调大 Q 或 R如果包络宽得离谱说明 P 被高估滤波器太保守。5.2 用均方根误差做定量对比曲线看趋势RMSE 看数字。做算法对比或调参时RMSE 是最直接的指标。% 稳态段后 60sRMSE idx t 60; rmse_pos sqrt(mean(pos_err(:,idx).^2, 2)); rmse_vel sqrt(mean(vel_err(:,idx).^2, 2)); rmse_att sqrt(mean((att_err(:,idx)*180/pi).^2, 2)); fprintf(位置 RMSE (m): E%.3f N%.3f U%.3f\n, rmse_pos); fprintf(速度 RMSE (m/s): E%.3f N%.3f U%.3f\n, rmse_vel); fprintf(姿态 RMSE (deg): R%.4f P%.4f Y%.4f\n, rmse_att);取后 60s 是为了避开初始收敛段否则 RMSE 会被前几十秒的大误差拉高反映不出稳态性能。对比不同 Q/R 组合时固定同一段数据、同一个随机种子只改滤波参数这样 RMSE 的差异才归因于参数。5.3 我调这套代码时踩出来的两个习惯第一个习惯先跑纯惯导再跑组合。把 GPS 更新关掉看纯惯导误差怎么发散心里有个基准线。如果组合后的曲线比纯惯导还差问题一定在滤波而不是数据。第二个习惯把残差序列单独画出来。z的均值应该接近零方差应该和 R 匹配。残差有趋势说明模型缺项残差方差远大于 R 说明 R 设小了。这个图比误差曲线更早暴露问题我一般把它放在调试第一屏。这套仿真工程的价值不在于跑出一条漂亮曲线而在于你能改一个参数、立刻看到它对误差和协方差的影响把「玄学调参」变成有依据的迭代。真机数据接入时把 2.3 节的仿真数据换成实测 IMU/GPS滤波框架一行不用改这才是先做仿真的意义。希望帮到你。本文还有配套的精品资源点击获取