简介：面向导航工程与自动化专业学习者，这份GPS/INS组合导航Matlab仿真源码提供了一套可复现的融合导航实验环境，解决GPS与惯导数据互补融合中的精度与鲁棒性问题。压缩包共6个文件、约673KB，包含m脚本、mat数据、txt说明与doc结果文档：m文件涵盖主仿真入口与卡尔曼滤波实现，mat文件存放预置实验数据，txt指导运行流程，doc展示结果分析。已有27人学习浏览。通过这份资料，读者能直观掌握GPS/INS组合导航的模型搭建、滤波估计与误差修正方法，理解GPS绝对定位与INS连续推算如何通过卡尔曼滤波实现优势互补；得益于完整代码与现成数据，下载后无需额外准备即可直接运行并观察中间结果，适合作为课程设计、毕业设计或组合导航入门研究的起步素材。

1. GPS/INS组合导航不是黑匣子：一套带实验数据的Matlab仿真源码能做什么

组合导航这个方向，论文里看着是十几页公式推导，落到代码上常是一堆跑不通的MATLAB脚本。我见过某同学把一套GPS/INS组合导航源码折腾两星期，最后发现只是IMU和GPS数据的时间戳没对齐，起步就发散。这套仿真源码自带实验数据，跑通之后能直接输出位置、速度和姿态误差曲线，核心是十五维误差状态卡尔曼滤波，走的是最常用的松组合框架。适合两类人：一类是写惯性导航、组合导航方向论文的研究生，需要一套能反复改参数对照结果的基线；另一类是刚接手惯导项目的工程师，想快速搞清楚机械编排和滤波更新在主循环里到底怎么衔接。它不是黑匣子，每层逻辑都能点开看。

2. 松组合还是紧组合：先弄清十五维误差状态和坐标系再谈仿真

2.1 十五维误差状态是怎么来的

这套源码的核心滤波器是扩展卡尔曼滤波，但真正估计的不是位置、速度和姿态的绝对量，而是它们的误差量。用误差状态而不是全状态做融合，是业内默认做法，原因很直接：INS解算出来的位置速度是积分结果，误差模型随时间近似线性，用卡尔曼滤波处理线性化误差比直接处理大动态非线性要稳得多。

标准的十五维误差状态包含五组，向量形式为：

% 十五维误差状态定义 % 索引 1-3 ：北、东、地位置误差 (m) % 索引 4-6 ：北、东、地速度误差 (m/s) % 索引 7-9 ：姿态误差角(失准角) (rad) % 索引 10-12：陀螺零偏 (rad/s) % 索引 13-15：加速度计零偏 (m/s^2) x = zeros(15, 1);

状态向量的维度决定了后面所有矩阵的尺寸：状态转移矩阵是15×15，过程噪声矩阵是15×15，量测矩阵的行数取决于当前可用的GPS观测。写代码前先把这五个分量的单位写进注释里，能省掉后面大量量纲排查时间。很多入门代码跑飞，问题就出在把陀螺零偏当成deg/h填进状态里，而机械编排用的是rad/s。

误差状态模型里最需要注意的是姿态误差方程和速度误差方程之间存在耦合。失准角通过重力矢量的旋转影响速度误差，陀螺零偏又通过姿态误差方程积分进失准角。这个耦合关系决定了为什么不能单独估计位置误差——不估计零偏，滤波更新完，下一拍误差还是会长回来。

2.2 坐标系约定：NED、ENU和载体坐标系必须对齐

拿到这套源码的第一步，不是去跑主脚本，而是确认数据里面的坐标系约定。GPS位置通常有两种存储习惯：一种是转成NED（北东地）或ENU（东北天）的局部坐标，另一种直接存经纬高。IMU输出的加速度和角速度是在载体坐标系下的，这个载体坐标系也分前-右-下和前-上-右两套习惯。

源码里如果直接给的是NED局部位置，那姿态和位置都能对上；如果给的是经纬高，主循环里必然有一段经纬高到NED的转换，这段转换里的基准纬度和基准经度不能随手填零。某开发者踩过一次坑：基准点填成赤道零度经线，算出来的水平位置是几百万米的量级，滤波自然发散。

判断坐标系有一个标准的五分钟检查法，用静止段数据看加速度计：

% 取前500个历元的加速度计静止段均值 acc0 = median(imu(1:500, 5:7), 1); % 判断标准：静止时加速度计测的是比力，约等于负重力 if acc0(3) > 8.0 disp('载体z轴朝下，坐标系可能是NED'); elseif acc0(3) < -8.0 disp('载体z轴朝上，坐标系可能是ENU'); end % 同时检查水平分量应接近0 if norm(acc0(1:2)) > 0.5 disp('警告：静止段水平加速度非零，检查传感器是否水平放置'); end

这段逻辑其实是在验证重力投影方向。NED坐标系中z轴指向地心，静止时加速度计读到的是支撑力，数值上约为+9.8 m/s²；ENU坐标系中z轴指天，静止读数接近-9.8 m/s²。判断完z轴方向，再看水平分量是否接近零，排除数据本身倾斜或存在常数偏置的可能。这套检查应该在调整任何滤波参数之前完成，坐标系错了后面全是无效劳动。

2.3 为什么默认选松组合而不是紧组合

松组合和紧组合的区别不在滤波本身，而在量测构造。松组合把GPS接收机内部PVT解算出的位置速度直接给滤波器当量测，结构简单，状态量只需要十五维。紧组合用的是伪距、伪距率原始观测，量测方程要写卫星几何矩阵，状态量还要加接收机钟差，实现复杂度上了一个台阶。

这套源码带的是位置速度数据，说明设计时就走松组合路线。这个选型合理：松组合的误差模型直观，调试时位置误差和速度误差直接对应GPS量测的物理含义，出了问题容易定位。紧组合的优势在于GPS卫星少于四颗时仍能工作，但仿真验证阶段这个优势体现不出来。

代价也要说清楚：松组合完全依赖GPS的PVT解算，一旦GPS输出跳变，滤波量测也会跟着跳，没有伪距层面的剔除手段。所以源码里如果能在量测更新前加一个新息检验模块，鲁棒性会明显上一个台阶。我一般会检查一下量测新息的3σ门限，后面第5章会专门讲这个坑。

3. 源码结构与主循环：从实验数据到误差状态输出的三步走

3.1 文件组织：主脚本、子函数、实验数据的分工

拿到源码包先看目录结构。这类仿真项目通常不会只放一个main.m，而是把机械编排、滤波预测、滤波更新拆成独立函数，方便单独替换和测试。典型的文件组织方式如下：

先确认存在哪几个文件再动手改代码。主循环、机械编排、滤波函数三者的接口需要对齐：ins_mech输出的是位置向量、速度向量、四元数，kf_predict输入的是上一时刻的状态协方差，这些接口在主脚本中都有注释。源码里把接口注释写全的版本，比那些只有脚本没有说明的要省力得多。

3.2 数据读取与时间对齐

实验数据打包在navdata.mat里，结构体里一般包含IMU数据、GPS观测、参考真值。常见的数据列组织方式如下：

% 读取实验数据 load('navdata.mat', 'imu', 'gps', 'truth'); % imu 矩阵列含义：时间(s), 陀螺x, 陀螺y, 陀螺z, 加速度计x, 加速度计y, 加速度计z % imu数据频率通常为100Hz~200Hz imu_t = imu(:,1); gyro = imu(:,2:4); % 角速度，rad/s acc = imu(:,5:7); % 比力，m/s^2 % gps 矩阵列含义：时间(s), 北位置(m), 东位置(m), 地位置(m), 北速度(m/s), 东速度(m/s), 地速度(m/s) % GPS数据频率通常为1Hz~10Hz gps_t = gps(:,1); gps_pos = gps(:,2:4); gps_vel = gps(:,5:7);

这里最容易被忽略的是IMU和GPS的时间基准。实验数据打包时如果已经对齐到同一根时间轴，直接按行索引即可；如果没有对齐，就先检查时间范围：

% 检查时间戳是否对齐 fprintf('IMU时间范围: %.2f ~ %.2f s\n', imu_t(1), imu_t(end)); fprintf('GPS时间范围: %.2f ~ %.2f s\n', gps_t(1), gps_t(end)); % 如果GPS频率低于IMU，先插值对齐到IMU栅格 if max(abs(diff(gps_t))) > 0.2 gps_pos_i = interp1(gps_t, gps_pos, imu_t, 'linear'); gps_vel_i = interp1(gps_t, gps_vel, imu_t, 'linear'); end

线性插值的前提是GPS数据本身连续无跳变。如果某个时间段GPS信号被遮挡，插值会把遮挡前后的值硬拉出一条直线，生成一组虚假观测。更稳的做法是保留原始GPS时间戳，在主循环里按时间戳匹配量测更新，而不是预先插值到IMU栅格。我一般会把插值后的数据当作备份，主循环里还是按原始GPS采样点触发更新，这样失锁模拟也更方便。

3.3 主循环里发生了什么：预测、更新和机械编排的衔接

主循环的骨架代码通常长这样：

% 初始化位置、速度、四元数（用静止段初始对准结果） pos_n = [0; 0; 0]; vel_n = [0; 0; 0]; quat = [1; 0; 0; 0]; % 初始四元数 % 初始误差状态和协方差 x_est = zeros(15, 1); P_est = zeros(15, 15); N = length(imu_t); for k = 2 : N dt = imu_t(k) - imu_t(k-1); % 1. 机械编排：IMU数据积分更新位置、速度、姿态 [pos_n, vel_n, quat] = ins_mech(pos_n, vel_n, quat, gyro(k,:), acc(k,:), dt); % 2. 误差状态时间更新 Phi = compute_phi(quat, acc(k,:), dt); Qd = compute_qd(Qc, Phi, dt); [x_pred, P_pred] = kf_predict(x_est, P_est, Phi, Qd); % 3. 量测更新：只有当前历元有GPS观测时才执行 if ~isnan(gps_mask(k)) z = [pos_n - gps_pos_i(k,:)'; vel_n - gps_vel_i(k,:)']; [x_est, P_est] = kf_update(x_pred, P_pred, z, H, R); else x_est = x_pred; P_est = P_pred; end % 4. 反馈校正：把估计出的误差量补偿回INS解算结果 pos_n = pos_n - x_est(1:3); vel_n = vel_n - x_est(4:6); quat = quat_compensate(quat, x_est(7:9)); end

注意这里的顺序：机械编排用的是上一历元校正后的状态，预测步骤输出的误差状态在反馈校正前会叠加回本次解算结果。如果不做反馈校正，误差会不停累积，滤波器估计的误差量会越来越大，最后数值溢出。反馈校正把误差状态清零后，下一次循环的预测是在修正后的轨迹上展开的，这套“预测-更新-反馈”的循环就是组合导航的核心节奏。

4. 卡尔曼滤波参数设置：Q、R、P0初值与失锁模拟的实操

4.1 Q矩阵和R矩阵的初值从哪里来

滤波器的Q矩阵和R矩阵是最容易拍脑袋的部分，但初值其实可以从实验数据里估。Q矩阵对应传感器本身的过程噪声，包含了陀螺角度随机游走、陀螺零偏不稳定性、加速度计速度随机游走这几个物理量。所有惯性传感器手册里都会给这些指标，单位通常是deg/sqrt(h)或m/s/sqrt(h)。

从静止段数据可以估算速度随机游走的量级：

% 静止段数据标准差粗估过程噪声 sigma_acc = std(acc(1:500, :), 0, 1); % 单位 m/s^2 Q_acc = diag(sigma_acc.^2 * dt); % 速度随机游走贡献 % 陀螺噪声用Allan方差或直接查手册 sigma_gyro = std(gyro(1:500, :), 0, 1); % 单位 rad/s Q_gyro = diag(sigma_gyro.^2 * dt);

这个方法虽然不如Allan方差严谨，但作为初值已经够用。Q矩阵如果填得过小，滤波器会过度相信INS，量测更新起不到修正作用；填得过大，位置误差曲线会跟着GPS噪声来回跳。一个合理的经验是：Q的数量级先按传感器手册给，再放大一倍看误差曲线是否变平滑，缩小一半看是否出现发散，取两者之间偏保守的值。

R矩阵的量测噪声相对好设，因为GPS的位置和速度噪声可以统计。位置噪声典型值在0.5~2米，速度噪声在0.02~0.1 m/s。R矩阵按下式构建：

% R矩阵：位置与速度量测噪声协方差 sigma_pos = 1.0; % GPS位置噪声标准差 (m) sigma_vel = 0.05; % GPS速度噪声标准差 (m/s) R = diag([sigma_pos^2; sigma_pos^2; sigma_pos^2; sigma_vel^2; sigma_vel^2; sigma_vel^2]);

R矩阵不是越小越好。如果GPS环境差、多路径明显，实际的位置误差远不止1米，把R设成0.1米会让滤波器被一组跳变的伪观测带偏。反过来，R设得太大，更新步骤等于不存在，输出基本就是纯INS结果。仿真数据里GPS噪声通常是理想化的，用前面这个量级一般不会出问题。

4.2 P0初始协方差怎么定

状态协方差矩阵P0的初值决定了滤波启动阶段的收敛速度。P0不能设成零矩阵，也不能设成无穷大。分别对应两个极端：P0为零会让滤波器完全相信初始状态，后续量测更新几乎不起作用；P0太大则起步阶段误差修正过猛，可能出现几百个历元的振荡。

% P0 初始协方差 P0 = diag([ 1.0; % 初始位置误差方差 (m^2) 1.0; 1.0; 0.1^2; % 初始速度误差方差 (m/s)^2 0.1^2; 0.1^2; 0.02^2; % 初始姿态误差方差 (rad^2) 0.02^2; 0.02^2; (0.01*pi/180)^2; % 陀螺零偏初始方差 (rad/s)^2 (0.01*pi/180)^2; (0.01*pi/180)^2; 0.01^2; % 加速度计零偏初始方差 (m/s^2)^2 0.01^2; 0.01^2; ]);

这里的数量级依据是：初始对准后姿态角误差通常在1°以内，对应的弧度约为0.017 rad，方差取0.02²是合理的；陀螺零偏的初始不确定度取一个经验值，后面靠滤波逐步修正。P0的四个分量对应不同物理量，单位写清楚就不会出现位置误差方差和角度的量混淆的问题。

用对角阵做P0就够用，不需要在初始时刻考虑状态之间的相关性。如果初始对准使用静止段的陀螺均值，陀螺零偏的初值可以减掉这个均值，P0对应这一分量再取小一个量级。

4.3 失锁模拟的两种做法

仿真验证里经常要模拟GPS失锁，看纯INS解算能维持多长时间精度。源码里实现失锁有两种常见方式：一种是把GPS观测值赋为NaN，一种是把观测有效标志位置零。

% 方式一：直接把观测置为NaN gps_mask = ones(size(imu_t)); gps_mask(imu_t >= 200 & imu_t <= 260) = NaN; % 方式二：用一个有效标志数组 gps_valid = true(size(imu_t)); gps_valid(imu_t >= 200 & imu_t <= 260) = false;

主循环里判断量测更新条件时，方式一用isnan，方式二直接用布尔索引。推荐第二种，因为后面统计误差时可以复用这个标志数组，把失锁段和正常段分开。

失锁期间的预期表现要心里有数：位置误差会按加速度计零偏的二阶积分量级增长，速度误差按零偏一阶积分增长，姿态角缓慢漂移。松组合模式下失锁60秒，位置误差漂到几十米到几百米都正常，如果只漂几米，说明数据本身的初始对准和零偏估计做得好，或者失锁段恰好没有机动。判断源码合理性时用这个标准：失锁段的误差增长应当近似抛物线，而不是线性。

5. 避坑指南：五个常见翻车现场与排查顺序

5.1 翻车现场一：坐标系错位导致姿态符号全反

现象：跑完主循环，位置误差曲线里水平误差几秒钟内飙到几十米，姿态角在静止段就不是零，而是出现一个明显的常值翻滚角。

原因：IMU的载体坐标系定义和机械编排里假设的姿态转序不一致。最常见的是把前-右-下的坐标系数据按前-上-右处理，导致俯仰和滚转符号反掉。俯仰角反号之后，重力在水平方向的分量会直接进入速度误差方程，滤波更新就算频率再高也压不住。

解决：回到静止段数据，先确定加速度计在无加速状态下三个轴的读数符号和大小。z轴接近+9.8说明载体坐标系z轴朝下，翻滚角和俯仰角的初值应该按这个假设计算。如果数据本身的坐标系和代码注释里的定义不一致，最省事的做法是写一个符号转换矩阵，把加速度和角速度翻到代码约定下，而不是去改机械编排里的姿态转序，改转序容易引入新的出错点。

5.2 翻车现场二：时间戳没对齐导致起步就发散

现象：滤波从第几个历元开始出现振荡，误差在几百毫秒内达到最大值，然后P矩阵对角线开始出现负值，最终整段NaN。

原因：IMU和GPS虽然频率不同，但如果它们的时间戳是各自独立记录的，简单按行号对齐会让GPS观测在时间轴上错位。GPS位置本身有滞后，如果再把IMU第100行当成和第100个GPS历元同一时刻，量测新息就会带着一个系统性偏差。滤波更新步骤里新息均值不为零，协方差越修越小，偏差却始终存在，最后滤波发散。

解决：先打印两边的采样时间范围，确认是否存在时间偏差。用GPS时间戳在IMU时间轴上进行最近邻或线性插值，不要按行号硬对齐。主循环里量测更新改用GPS采样点触发，而不是每个IMU历元都做更新。检查一下新息序列的均值，如果均值长期不等于零，不管P矩阵怎么调，结果都不可信。

5.3 翻车现场三：杆臂效应被忽略导致水平位置偏置

现象：在转弯或加减速段，水平位置误差出现一个与机动相关的周期偏置；直线匀速段误差反而收敛得很好。

原因：GPS天线和IMU安装位置不重合，通常天线装在车顶，IMU装在车内，两者之间有一个固定的杆臂向量。机动时，IMU位置和GPS天线位置之间会产生一个与角速度、向心加速度相关的位置差，这个差值如果被当作量测误差送进滤波器，会表现为与转弯方向相关的系统性偏差。

解决：在量测方程里对GPS位置观测做杆臂补偿。补偿形式是把杆臂向量投影到导航坐标系后加回GPS观测：

% 杆臂补偿：天线相位中心位置 = IMU位置 + 杆臂投影 lever_body = [0.3; 0; -0.5]; % 杆臂向量在载体坐标系下的分量 (m) lever_nav = quatrotate(quat_conj(quat), lever_body'); gps_pos_comp = gps_pos_i(k,:)' + lever_nav;

补偿完再和INS位置做差，量测新息里的周期性偏差会明显减小。这个参数在源码的配置区里一般有预留位置，没有的话自己加上，注意载体坐标系的杆臂方向符号。

5.4 翻车现场四：量测更新频率按错节奏导致速度跳动

现象：速度误差曲线出现阶梯状跳变，每次GPS数据到来时速度被强行拉回，两拍之间速度又按加速度计积分自然漂移，整体看起来像锯齿波。

原因：GPS更新率低于IMU，如果在两次GPS观测之间对同一个GPS观测值做了多次量测更新，或者把GPS的1Hz数据插值后当成10Hz量测，每次更新都在修正相同的量测，速度估计自然出现台阶。

解决：主循环里用一个标志判断当前历元是否有新GPS观测，只在有新增观测的那个历元调用kf_update。插值后的GPS数据只能做绘图用，不能进滤波。量测更新频率匹配好之后，速度误差曲线应该是一条平滑的收敛带，而不是锯齿。

5.5 翻车现场五：P矩阵不守恒导致NaN和黑匣子现象

现象：滤波长时间运行后，估计值看起来正常，但输出结果突然在某一段变成NaN；重新跑一遍，同样的随机种子，发散位置却在另一处。

原因：卡尔曼滤波更新步骤里如果直接写K = P * H' / (H * P * H' + R)，这个除法在矩阵条件数差时会引入数值误差，使得P矩阵失去对称半正定性。失去正定性后，下一次时间更新里的平方根运算或求逆就会出现负值，最终NaN。

解决：所有矩阵除法改写成H * P * H' + R后先做对称化再求逆，每次更新结束后强制执行P = 0.5 * (P + P')。另一个有效做法是更新步骤改成Joseph form，虽然计算量略高，但数值稳定性好很多。记住一个习惯：P矩阵不是理论概念，它是数值对象，每次更新后都要保持对称正定。

注意：排查这五类问题时，顺序很重要。先查时间戳对齐，再查坐标系，最后才动滤波参数。很多人一上来就改Q和R，翻了半天车，回头才发现是数据接口的问题。

6. 仿真结果的验证方法：先分窗统计RMS，再对P矩阵检验一次

6.1 误差曲线怎么读：失锁段和闭合段分开看

拿到位置误差曲线不要急着截图。先把数据按GPS有效、失锁分成两段，分别统计RMS。失锁段的误差增长反映的是INS纯惯性精度和零偏估计质量，闭合段的误差反映的是滤波收敛程度和量测噪声水平。两段混在一起统计，结果会被失锁段的大误差主导，看不出闭合段的真实表现。

% 分窗统计RMS误差 mask_fix = gps_valid; mask_loss = ~gps_valid; err_pos = pos_n - truth_pos; rms_fix = sqrt(mean(err_pos(:, mask_fix).^2, 2)); rms_loss = sqrt(mean(err_pos(:, mask_loss).^2, 2)); fprintf('闭合段水平位置RMS: %.3f m\n', norm(rms_fix(1:2))); fprintf('失锁段水平位置RMS: %.3f m\n', norm(rms_loss(1:2)));

闭合段RMS如果大于量测噪声的3倍，说明滤波器没有充分利用GPS信息，可能是R偏大、量测方程写错，或者新息序列里隐藏着未补偿的系统误差。失锁段RMS如果小到和闭合段一个量级，要么是失锁时间太短，要么是Q矩阵设得太小导致滤波器在失锁段几乎不增长误差，这不代表算法好，反而说明过程噪声模型不可信。

6.2 一个值得养成的习惯：用P矩阵对角线检验一致性

滤波输出的P矩阵对角线代表估计的不确定度，这个值和实际误差曲线应该匹配。把3σ包络和真实误差画在同一个图里，如果误差曲线长期落在3σ之外，说明滤波器对自身不确定度估计过乐观，模型里漏了误差源；如果包络宽到覆盖一切，说明Q和P0设得过大，滤波器接近失效。

% 从滤波历史中提取位置误差的3σ包络 sigma_pos = 3 * sqrt(squeeze(P_history(1,1,:))); figure; plot(t, abs(err_pos_ned(1,:)), 'b', 'LineWidth', 1); hold on; plot(t, sigma_pos, 'r--', 'LineWidth', 1); legend('北向误差', '3σ包络');

在看到这张图之前，不要轻易给参数下结论。真实误差长期超出包络，回头查Q矩阵是否偏小，R矩阵是否低估了GPS噪声，或者是否存在未建模的杆臂、时间不同步。这套检查下来，源码里哪些地方能改、哪些地方不能动，就一目了然了。

从那以后，我每次拿到新的IMU/GPS数据，都强制先跑一遍静止段判断坐标系、估算噪声量级，再进主循环。这个五分钟的习惯，能省掉后面一整天的滤波调参时间。尤其对刚上手这套仿真源码的人，与其对着Q、R一顿乱试，不如先把数据本身摸清楚。希望帮到你。

本文还有配套的精品资源，点击获取