简介这份资源面向导航算法学习者与工程实践者提供一套基于MATLAB的SINS与GPS组合导航完整仿真项目用于解决单一惯性导航随时间漂移、GPS在遮挡环境下易受干扰的定位精度问题。包内共7个文件约675KB以m脚本与asv备份文件承载核心融合算法mat数据文件提供仿真所需原始数据doc与txt则记录程序说明与位置组合结果便于对照理解。项目围绕卡尔曼滤波展开涵盖SINS加速度与角速率处理、GPS伪距信息融合、位置组合解算等环节运行后可生成位置轨迹对比与误差统计等分析图直观评估定位精度、收敛速度与稳定性。目前已有223人学习适合希望掌握数据融合技术、提升MATLAB编程能力并积累导航系统设计经验的读者参考。1. 拿到一套 SINS/GPS 组合导航 MATLAB 程序先别急着 run如果你手上只有 IMU 和 GPS 两路数据却要输出一条平滑、可用、能过验收的轨迹纯惯导积分几分钟就飘到姥姥家纯 GPS 又跳得没法看——SINS/GPS 组合导航就是干这个的。这套 MATLAB 资源把程序、配套数据、运行后的分析图打包在一起核心是一条完整的松组合解算链路惯性解算 → 卡尔曼滤波 → 误差反馈校正 → 轨迹与误差曲线出图。它适合正在做毕设、课程设计或工程预研的人尤其是搜「GPS SINS matlab」找现成可跑代码的。但我要先泼盆冷水这类资源最容易翻车的地方不是算法而是坐标系、单位、时间对齐这三件事90% 的「跑出来是条直线」都栽在这。下面按我拆包复现的顺序讲。2. 先看懂这套程序在算什么SINS 解算与 15 维卡尔曼的对应关系2.1 惯导解算这条主线姿态、速度、位置怎么递推SINS 的核心就三组微分方程姿态用四元数或方向余弦矩阵更新速度靠比力旋转到导航系后扣掉重力再积分位置由速度积分得到。这套程序里通常能看到attitude_update、velocity_update、position_update三个函数或者揉在一个sins_mechanization里。你要先确认它用的是哪种姿态表示——四元数还是欧拉角。四元数没有万向锁工程上更常见但如果你要直接读横滚俯仰航向得在输出端转欧拉角。关键参数是解算周期dt。它必须和 IMU 采样率严格对应比如 IMU 是 100Hz那dt 0.01。我见过有人数据是 200Hz 却写dt0.01结果速度直接差一倍轨迹尺度全错。地球参数也别乱填地球自转角速度wie 7.292115e-5 rad/s重力加速度随纬度变化程序里如果用了g 9.8常数短时间还行长时间会有常值偏差。% SINS 单步解算骨架松组合常见结构 dt 0.01; % 解算周期必须等于 1/IMU采样率 wie 7.292115e-5; % 地球自转角速度 rad/s g 9.793; % 当地重力按纬度查表更准 % 1) 姿态更新四元数 q 由角速度积分 q quat_update(q, gyro - Cbn*wn_ie_n, dt); % 2) 速度更新比力旋转到导航系扣重力、哥氏项 fn Cbn * acc; % 比力转到导航系 vn vn (fn - 2*cross(wn_ie_n, vn) - gn) * dt; % 3) 位置更新经纬高积分 pos pos [vn(2)/(Rmh); vn(1)/((Rnh)*cos(pos(1))); -vn(3)] * dt;逻辑说明Cbn是机体系到导航系的旋转矩阵由四元数实时算出wn_ie_n是地球自转在导航系的投影哥氏项在低动态短时场景可以忽略但程序里保留说明作者是按完整模型写的。参数说明gyro、acc单位必须是 rad/s 和 m/s²如果你拿到的数据是 deg/s 和 g进解算前必须换算这是第一个高频坑。2.2 卡尔曼滤波的 15 维状态每个量对应什么物理误差松组合的标准做法是 15 维状态3 维姿态误差、3 维速度误差、3 维位置误差、3 维陀螺零偏、3 维加计零偏。程序里会有一个F矩阵状态转移和一个H矩阵量测。GPS 提供位置和速度时H就是从中挑出位置速度那 6 维量测噪声矩阵R按 GPS 的定位精度填比如水平 1.5m、高程 3m。这里最容易被忽略的是Q矩阵过程噪声。它代表你对 IMU 噪声水平的信任程度。填太小滤波器不信任 GPS轨迹还是飘填太大滤波器过度依赖 GPSGPS 一跳轨迹就抖。常见做法是先用 Allan 方差标定出陀螺和加计的随机游走系数再折算进Q。没有标定数据时陀螺零偏稳定性按0.01 deg/h量级、加计按100 ug量级试然后看新息序列是否白噪声来微调。% 15维卡尔曼状态顺序 [dphi dpitch droll dvn dve dvd dlat dlon dalt bgx bgy bgz bax bay baz] F zeros(15); % 姿态误差受速度误差和陀螺零偏驱动具体元素由导航系比力构成 F(1:3,4:6) skew(fn); % 比力反对称阵 F(1:3,10:12) -Cbn; % 陀螺零偏对姿态的影响 F(4:6,1:3) skew(Cbn*acc); % 姿态误差对速度的影响 F(4:6,13:15) Cbn; % 加计零偏对速度的影响 F(7:9,4:6) diag([1/(Rmh), 1/((Rnh)*cos(lat)), -1]); H zeros(6,15); H(1:3,7:9) eye(3); % GPS位置观测 H(4:6,4:6) eye(3); % GPS速度观测 Q diag([zeros(1,6), (1e-5)^2*ones(1,3), (1e-4)^2*ones(1,6)]); R diag([1.5^2 1.5^2 3^2 0.1^2 0.1^2 0.1^2]);逻辑说明F里姿态和速度的耦合项是组合导航能收敛的关键缺了它滤波器就退化成两个独立系统。Q和R的比值决定信任分配调试时先固定R调Q。参数说明skew()是求反对称矩阵Rm、Rn是子午圈和卯酉圈曲率半径随纬度变化程序里一般有现成函数。2.3 反馈校正闭环还是开环差一个字结果差很多组合导航分反馈校正闭环和输出校正开环。闭环是把滤波估计的误差直接反馈回去修正姿态、速度、位置和零偏下一历元用修正后的值继续解算开环只修正输出内部解算照飘。这套程序大概率是闭环因为闭环精度更高。你要在代码里找到反馈那几行确认零偏有没有被真正扣掉——有些程序估了零偏但忘了反馈结果就是零偏一直累积。% 闭环反馈估计值直接修正导航状态和IMU零偏 pos pos - dx(7:9); vn vn - dx(4:6); q quat_multiply(q, euler2quat(-dx(1:3))); % 姿态误差反馈 gyro_bias gyro_bias dx(10:12); acc_bias acc_bias dx(13:15);逻辑说明注意姿态反馈的符号误差定义方向不同会导致越修越偏这是调试时最玄学的地方。参数说明dx是卡尔曼更新后的状态估计反馈后一般把dx清零避免重复扣除。3. 把程序跑起来数据准备、时间对齐与首次出图3.1 数据文件怎么读IMU 与 GPS 的字段和单位核对资源里的数据通常是.mat或.txt。.mat直接load.txt用load或importdata。拿到数据第一件事不是跑是whos看变量名和维度再打印前几行确认列顺序。IMU 常见列顺序是[时间 陀螺x 陀螺y 陀螺z 加计x 加计y 加计z]GPS 是[时间 纬度 经度 高程 北向速度 东向速度 天向速度]。单位必须逐个核对陀螺是 rad/s 还是 deg/s加计是 m/s² 还是 g经纬度是度还是弧度。load(imu_data.mat); % 假设变量名 imuN×7 load(gps_data.mat); % 假设变量名 gpsM×7 % 单位核对与换算 if max(abs(imu(:,2:4))) 10 imu(:,2:4) imu(:,2:4) * pi/180; % deg/s - rad/s end if max(abs(imu(:,5:7))) 5 imu(:,5:7) imu(:,5:7) * 9.793; % g - m/s^2 end % 经纬度转弧度 gps(:,2:3) gps(:,2:3) * pi/180;逻辑说明用阈值判断单位是工程上常用的土办法因为正常车载陀螺不会超过 10 rad/s加计不会超过 5g。参数说明换算系数pi/180和9.793按当地重力微调。这一步做完建议把 IMU 静止段的前几百个样本拿出来算均值和方差均值就是零偏粗估值方差反映噪声水平和后面Q矩阵对照。3.2 时间对齐GPS 是 1Hz、IMU 是 100Hz 怎么处理GPS 和 IMU 采样率不同必须做时间对齐。标准做法是以 IMU 时间为基准每个 IMU 历元都做一次惯导递推只有 GPS 时刻才做卡尔曼量测更新。程序里通常有个if判断当前 IMU 时间是否到达下一个 GPS 时刻。你要确认这个判断用的是时间戳而不是索引否则数据有丢帧时直接错位。gps_idx 1; for k 1:size(imu,1) t imu(k,1); % 惯导递推每个IMU历元都做 [q, vn, pos] sins_step(q, vn, pos, imu(k,2:7), dt); % 到达GPS时刻才做量测更新 if gps_idx size(gps,1) t gps(gps_idx,1) [dx, P] kalman_update(pos, vn, gps(gps_idx,2:7), P, H, R); [q, vn, pos, gyro_bias, acc_bias] feedback(...); gps_idx gps_idx 1; end traj(k,:) [pos vn]; end逻辑说明t gps(gps_idx,1)用大于等于而不是等于是为了容忍时间戳的微小抖动。参数说明dt从 IMU 时间列相邻差值算比固定值更稳。如果 GPS 有丢星导致时间跳变这里要加保护否则gps_idx会越界。3.3 首次出图三条曲线判断解算是否正常跑完先出三张图轨迹对比纯惯导 vs 组合 vs GPS、姿态角、速度误差。正常结果应该是组合轨迹平滑地贴在 GPS 点附近纯惯导逐渐发散。如果组合轨迹和纯惯导一样飘说明量测更新没生效如果组合轨迹跟着 GPS 剧烈抖动说明R太小或Q太大。figure; plot(gps(:,3)*180/pi, gps(:,2)*180/pi, k., MarkerSize, 4); hold on; plot(traj(:,2)*180/pi, traj(:,1)*180/pi, r-, LineWidth, 1.2); legend(GPS, SINS/GPS); xlabel(经度(deg)); ylabel(纬度(deg)); title(轨迹对比);逻辑说明先画散点再画线能直观看出组合结果是否被 GPS 拉偏。参数说明经纬度转度只为出图好看内部解算始终用弧度。4. 避坑与排查跑不通、跑飞、跑出来是直线的五条血泪记录4.1 现象轨迹是一条直线或一个点原因位置更新里经纬高增量算错最常见是经度增量忘了除以cos(lat)或者dt单位是毫秒却当秒用。解决打印前 10 个历元的位置增量和vn*dt手算结果对比差一个cos(lat)或 1000 倍就是它。4.2 现象滤波器发散误差越来越大原因P初始值给太小滤波器「自信」过头新息被压制或者F矩阵离散化用了IF*dt但dt太大。解决P初始按diag([1e-4 1e-4 1e-4 1 1 1 10 10 10 ...])给足F离散化改用expm(F*dt)或二阶近似。4.3 现象GPS 一跳轨迹就抖原因R填得太小滤波器把 GPS 跳变当真实运动。解决R按 GPS 实际定位精度的 2~3 倍填水平至少2^2高程5^2速度0.2^2。同时检查有没有对 GPS 做异常值剔除没有的话加一个 3σ 新息门限。4.4 现象姿态角在静止时缓慢漂移原因陀螺零偏没估准或没反馈或者地球自转补偿符号反了。解决静止段单独跑看姿态漂移速率反推零偏检查wn_ie_n的符号北向分量应是wie*cos(lat)天向是wie*sin(lat)。4.5 现象程序报错维度不匹配原因H矩阵列数和状态维数不一致或者 GPS 数据某时刻缺速度只有位置。解决size(H)和length(dx)对齐GPS 缺列时把对应H行和R元素删掉别硬凑。5. 进阶用新息序列自检滤波健康度以及一套可复用的调参顺序跑通只是及格能判断滤波器健不健康才是分水岭。我习惯在量测更新后把新息nu z - H*x_pred存下来跑完画新息序列和它的自相关。健康的新息应该是零均值白噪声如果出现缓慢趋势说明Q偏小、滤波器跟不上如果自相关有明显尖峰说明R偏小或量测有系统性偏差。% 新息序列自检 nu z - H * x_pred; figure; subplot(2,1,1); plot(nu); title(新息序列); grid on; subplot(2,1,2); autocorr(nu, 50); title(新息自相关);逻辑说明autocorr是 MATLAB 自带函数滞后 50 个点足够看短时相关性。参数说明如果新息均值不为零先查 GPS 和 SINS 的位置基准是否同一坐标系WGS84 和 CGCS2000 在米级应用可以混用但高程基准差几十米很常见。调参顺序我固定走四步第一步静止段标定零偏和噪声填Q初值第二步固定Q把R从大到小调直到轨迹既不抖也不飘第三步固定R微调Q看新息是否白化第四步全程跑一遍检查闭环反馈有没有引入数值不稳定。这套顺序能避免两个参数同时动导致的「玄学调参」。还有一个容易被忽略的点这套程序如果带分析图图里的误差曲线是评估依据。你要确认误差定义是「组合减 GPS」还是「组合减真值」。如果只有 GPS那误差曲线里包含 GPS 自身误差别把它当成绝对精度。真值通常来自更高精度的参考系统资源里如果没有就用 GPS 平滑后轨迹做近似参考但结论要打折扣。从那以后我每次拿到组合导航程序都强制先跑静止段看零偏和姿态漂移再跑一段直线看尺度最后才上完整轨迹。这三步走完基本不会出现「跑出来不能用」的尴尬。希望帮到你。本文还有配套的精品资源点击获取