简介：这份资源面向导航、控制与信号处理方向的学生及工程人员，提供GPS/INS组合导航的MATLAB实现方案，帮助理解两种导航方式互补融合的完整流程。压缩包共10个文件，约682KB，以m脚本与asv备份文件为主，另含doc说明文档、mat数据文件和txt程序说明，覆盖GPS信号模拟、INS动态模型、扩展卡尔曼滤波融合及结果可视化等环节。已有3938人学习下载，说明其在教学与课程设计中具有较高参考价值。读者可借助其中的位置组合程序，动手搭建滤波器、分析定位误差与均方根误差，并对照文档理解各模块的输入输出关系，从而掌握数据融合的建模思路与调参方法，为后续组合导航项目或论文复现打下基础。
1. GPS/INS组合导航matlab实现：从单点解算到松组合的工程落地路径
做车载定位或无人机飞控的工程师，大概率都遇到过这样的场景：GPS模块在开阔路段跑得好好的，一进隧道或城市峡谷，轨迹立刻开始飘，速度曲线出现尖刺，姿态角跟着抖。单靠GPS，输出频率低、易受遮挡和多路径影响；单靠INS，陀螺和加计零偏随时间积分发散，几分钟后位置误差就能到百米级。GPS/INS组合导航matlab实现，要解决的就是把两者优势拼起来——用GPS的长期稳定性去约束INS的短期发散，用INS的高频输出和抗遮挡能力去填补GPS的空白。这套方案适合做算法验证的学生、做样机调试的嵌入式工程师，以及需要快速评估组合导航收益的从业者。Matlab不是最终产品，但它是把卡尔曼滤波、误差状态方程、时间同步这些环节跑通的最短路径。
2. 松组合与紧组合怎么选：误差状态方程和观测量的工程取舍
2.1 松组合、紧组合、深组合的边界在哪里
组合导航按耦合深度通常分三类。松组合（Loosely Coupled）把GPS和INS各自算出的位置、速度作差，作为卡尔曼滤波的观测量，结构简单，GPS失锁时INS独立递推，工程上最容易落地。紧组合（Tightly Coupled）直接用GPS的伪距、伪距率作为观测量，即使可见星少于四颗也能更新，城市峡谷下优势明显，但需要拿到原始观测量，接收机配置和接口调试成本高。深组合（Ultra-Tightly Coupled）把INS信息反馈到接收机跟踪环路，属于接收机内部改造，一般团队碰不到。
我一般建议：第一次做GPS/INS组合导航matlab实现，从松组合入手。原因是观测量干净、调试链路短、出错时容易定位是INS递推问题还是GPS观测问题。等松组合跑稳，再考虑紧组合。
2.2 误差状态方程：15维还是18维
松组合最常用的是15维误差状态向量：
δx = [δp(3), δv(3), δψ(3), δba(3), δbg(3)]分别是位置误差、速度误差、姿态误差、加速度计零偏、陀螺零偏。如果用的是低精度MEMS，零偏不稳定，可以扩到18维，加入零偏变化率。但维数越高，可观性分析越麻烦，P阵调参越玄学。新手先用15维。
连续时间误差状态方程写成：
% 15维误差状态转移矩阵 F 的构造（简化示意） % 状态顺序: dp(3) dv(3) dpsi(3) dba(3) dbg(3) F = zeros(15,15); F(1:3,4:6) = eye(3); % 位置误差对速度误差 F(4:6,7:9) = skew(fb); % 速度误差对姿态误差, fb为比力 F(4:6,10:12) = Cbn; % 速度误差对加计零偏 F(7:9,7:9) = -skew(omega_ie); % 姿态误差自身 F(7:9,13:15) = -Cbn; % 姿态误差对陀螺零偏这里skew()是反对称矩阵，Cbn是机体系到导航系的旋转矩阵。离散化用一阶近似Phi = I + F*dt，dt是IMU采样间隔。如果IMU是100Hz，dt=0.01s，一阶近似够用；如果dt大于0.05s，建议用二阶或精确离散化。
2.3 观测方程：位置速度观测量怎么摆
松组合的观测向量通常是GPS位置和速度与INS推算值之差：
% 观测矩阵 H, 观测量为 [dpos(3); dvel(3)] H = zeros(6,15); H(1:3,1:3) = eye(3); % 位置观测直接对应位置误差 H(4:6,4:6) = eye(3); % 速度观测直接对应速度误差如果GPS只给位置不给速度，H取前3行。观测噪声R按GPS模块手册给的位置标准差和速度标准差设，一般位置1~3米，速度0.1~0.5米每秒。R设大了滤波收敛慢，设小了GPS跳点会把INS带偏。
注意：GPS输出的位置是天线相位中心，INS推算的是IMU中心，两者之间有杆臂。杆臂不补偿，转弯时位置观测会有米级偏差。杆臂向量在车体坐标系下量出来，转到导航系后从观测里扣掉。
3. 在Matlab里把松组合跑起来：数据准备、时间同步与滤波主循环
3.1 数据准备：IMU和GPS的格式对齐
假设你手上有两路数据：IMU的比力和角速度，GPS的位置和速度。常见做法是存成mat文件，每列一个量。IMU数据至少包含时间戳、三轴加计、三轴陀螺；GPS数据包含时间戳、经纬高、东北天速度。
% 加载数据, 假设 imu_data 为 N×7, gps_data 为 M×7 load('imu_gps_data.mat'); t_imu = imu_data(:,1); % IMU时间戳, 单位秒 fb = imu_data(:,2:4)'; % 比力, 3×N omega = imu_data(:,5:7)'; % 角速度, 3×N t_gps = gps_data(:,1); pos_gps = gps_data(:,2:4)'; % 纬度经度高程或局部坐标 vel_gps = gps_data(:,5:7)'; % 东北天速度如果GPS给的是经纬高，先转成局部东北天坐标，参考点取第一条GPS的经纬度。转换用geodetic2enu或自己写椭球公式。注意IMU和GPS时间戳可能不同源，先检查时间基准是否一致。
3.2 时间同步：最近邻插值和对齐的坑
IMU通常100Hz以上，GPS通常1Hz或10Hz。滤波主循环按IMU频率走，每到GPS时刻做一次量测更新。最简单的是最近邻匹配：
gps_idx = 1; for k = 2:length(t_imu) dt = t_imu(k) - t_imu(k-1); % INS机械编排递推 [pos_ins, vel_ins, Cbn, fb_n, omega_n] = ins_mechanization(... pos_ins, vel_ins, Cbn, fb(:,k-1), omega(:,k-1), dt); % 时间更新 [Phi, Qd] = discretize_FQ(F, Q, dt); P = Phi * P * Phi' + Qd; % 判断是否到GPS时刻 if gps_idx <= length(t_gps) && t_imu(k) >= t_gps(gps_idx) % 量测更新 [pos_ins, vel_ins, P] = measurement_update(... pos_ins, vel_ins, pos_gps(:,gps_idx), vel_gps(:,gps_idx), P, H, R); gps_idx = gps_idx + 1; end end这里ins_mechanization是机械编排函数，包含姿态更新、速度更新、位置更新。姿态更新用四元数或旋转矩阵，速度更新要扣掉哥氏力和重力。discretize_FQ把连续F和Q离散化。
时间同步的坑在于：GPS时间戳可能是整秒，IMU时间戳从零开始，两者差一个常数偏移。不校准这个偏移，量测更新会引入系统性误差。做法是用一段静止数据对齐，或者用GPS的PPS信号做硬件同步。纯软件对齐时，最近邻匹配在动态场景下会有半个采样周期的误差，车速快时位置误差能到分米级。
3.3 卡尔曼滤波主循环：预测和更新的参数怎么设
Q阵是过程噪声协方差，决定滤波器对INS模型的信任程度。Q设大了，滤波器更信GPS，INS零偏估计收敛快但噪声大；Q设小了，滤波器更信INS，GPS跳点被平滑但零偏估计慢。
% 过程噪声Q的构造, 按误差状态顺序 Q = zeros(15,15); Q(1:3,1:3) = eye(3) * (0.1)^2; % 位置过程噪声 Q(4:6,4:6) = eye(3) * (0.05)^2; % 速度过程噪声 Q(7:9,7:9) = eye(3) * (0.01*pi/180)^2; % 姿态过程噪声 Q(10:12,10:12) = eye(3) * (100e-6*9.8)^2; % 加计零偏噪声 Q(13:15,13:15) = eye(3) * (10*pi/180/3600)^2; % 陀螺零偏噪声这些数值不是拍脑袋，是按器件手册的噪声密度和零偏稳定性折算的。加计零偏噪声单位是米每二次方秒，陀螺零偏噪声单位是弧度每秒。实际调的时候，先按手册值设，跑一遍看新息序列是否白噪声。新息有偏或自相关，说明Q或R不匹配。
P阵初始值按初始对准误差设。位置误差设10米，速度误差设0.1米每秒，姿态误差设1度，零偏按器件零偏设。P初始值不影响稳态，但影响收敛速度。
4. 避坑与排查：组合导航调试中最容易翻车的五个地方
4.1 现象：轨迹整体偏移，滤波收敛后仍有固定偏差
原因：杆臂没补偿，或者GPS天线和IMU的坐标系定义不一致。GPS位置是天线相位中心，INS推算的是IMU中心，两者差一个杆臂向量。转弯时这个偏差在观测里表现为系统性误差。
解决：量出IMU到天线的杆臂向量，在机体系下表示，转到导航系后从GPS位置里扣掉。杆臂补偿公式：
% lever_arm_b 为机体系杆臂, Cbn为机体系到导航系旋转矩阵 lever_arm_n = Cbn * lever_arm_b; pos_gps_compensated = pos_gps - lever_arm_n;4.2 现象：GPS失锁后重新捕获，轨迹跳变
原因：GPS失锁期间INS递推，位置误差累积。重新捕获时，GPS观测和INS推算值差得远，量测更新把状态硬拉过去，轨迹出现尖刺。
解决：失锁期间把R调大，或者直接跳过量测更新，只做时间更新。重新捕获后先用几个历元的小权重更新，让滤波器慢慢收敛。判断失锁用GPS的可见星数和定位质量因子。
4.3 现象：姿态角缓慢漂移，静态下也漂
原因：陀螺零偏没估计准，或者地球自转角速度补偿漏了。低精度MEMS陀螺零偏能到几十度每小时，不估计的话姿态几分钟就飘走。
解决：检查误差状态里陀螺零偏是否可观测。静态下姿态误差和陀螺零偏强耦合，需要机动才能分开。如果一直静止，零偏估计不准是正常的。另外检查机械编排里是否扣了地球自转和运输率。
4.4 现象：滤波发散，P阵越来越大
原因：Q设太大，或者观测模型有系统性误差。Q太大时P在时间更新中膨胀，量测更新压不住。观测模型有偏时，新息不白，P也会异常。
解决：先看新息序列，画出来看均值和自相关。新息有偏说明H或观测值有问题；新息自相关说明Q或R不匹配。把Q调小一个量级试，或者用自适应滤波在线估Q。
4.5 现象：Matlab跑得慢，数据量大时卡死
原因：循环里频繁调函数，或者P阵更新用了低效写法。Matlab的for循环本身不慢，慢在函数调用开销和内存分配。
解决：把机械编排和滤波更新写成向量化形式，预分配所有数组。P阵更新用Phi*P*Phi'，Matlab会自动优化。如果还慢，把核心循环写成mex函数，或者用parfor分段跑。
5. 进阶技巧：用新息序列验证滤波一致性，以及一条我踩过的坑
滤波跑完不算完，得验证结果可信。最直接的方法是看新息序列——观测值减去预测观测值：
% 新息序列 innovation = pos_gps(:,gps_idx) - pos_ins_pred; % 新息协方差 S = H * P_pred * H' + R; % 归一化新息平方 NIS = innovation' * inv(S) * innovation;NIS应该服从卡方分布，自由度等于观测维数。如果NIS均值远大于自由度，说明滤波器过度自信，P设小了或R设小了；远小于自由度，说明滤波器不够自信。把NIS画出来，看是否在置信区间内。
另一个技巧是看零偏估计的收敛曲线。加计零偏和陀螺零偏在机动后应该收敛到稳定值。如果一直飘，说明可观性不够，需要更多机动或更准确的观测。
我踩过的一个坑：早期做松组合时，GPS速度观测直接用了接收机输出的地速，没注意那是二维地速，垂直速度是另外给的。结果垂直通道的观测噪声设错了，高度方向滤波发散，轨迹在垂直方向来回跳。后来把垂直速度单独拿出来，按接收机手册重新设R，问题才解决。血泪经验是：GPS输出的每一个量都要确认定义和坐标系，别想当然。
还有一个习惯：每次改完Q或R，先跑一段静态数据看收敛，再跑动态数据看跟踪。静态看零偏估计和姿态稳定性，动态看位置速度跟踪误差。两段都过了，再上实测数据。这个习惯帮我省了很多后悔药。
希望帮到你。
本文还有配套的精品资源，点击获取
