简介：这份资源面向学习组合导航算法的高年级本科生、研究生及工程技术人员，提供GPS/INS位置组合导航的MATLAB实现方案，帮助理解惯性导航与卫星导航数据融合的核心流程。压缩包共10个文件，约682KB，以m脚本为主，辅以doc说明文档、asv备份、mat数据文件和txt程序说明，覆盖GPS信号模拟、INS动态模型、扩展卡尔曼滤波融合及结果可视化等环节。已有3938人学习下载，说明该方向具有较高的关注度。读者可从中获得可运行的组合导航仿真代码、滤波器设计思路、误差与均方根误差等性能评估方法，以及GPS单独、INS单独与组合后轨迹的对比结果，便于快速搭建实验环境并深入掌握卡尔曼滤波在导航状态估计中的应用。
1. GPS/INS组合导航的MATLAB实现：从单点解算到松耦合滤波的完整路径
做无人车或飞行器定位的工程师，迟早会撞上同一个问题：GPS单点解算在开阔路段看着还行，一进城市峡谷或树荫遮挡，轨迹就开始跳，速度曲线像心电图。而IMU短时精度高、更新率快，却会随时间积分漂移，几分钟后位置误差能到几十米。GPS/INS组合导航MATLAB实现这件事，本质上就是让这两个传感器互相补位——用GPS的长期稳定性去约束IMU的漂移，用IMU的高频输出填补GPS失锁时的空白。这套方案适合已经会写基础卡尔曼滤波、手头有实测或仿真GPS与IMU数据、想把定位精度从米级压到分米级的从业者。下面按数据准备、单点解算、松耦合滤波、调参与排错的顺序，把可复现的MATLAB路径拆开讲。
2. 数据准备与坐标系对齐：组合导航的第一道门槛
2.1 为什么坐标系不统一会让后续滤波全盘失效
组合导航的输入是两路数据：GPS给出的是ECEF或经纬高下的位置速度，IMU给出的是载体坐标系下的加速度和角速度。如果直接把IMU的比力当作导航系下的加速度去积分，得到的轨迹会完全偏离真实路径。常见做法是先把IMU数据从载体系转到导航系（NED或ENU），再扣除重力，最后积分得到位置速度。这一步的旋转矩阵由姿态角决定，而姿态角又来自陀螺积分，所以姿态初始化误差会直接传播到位置。
我一般会在数据准备阶段做三件事：统一时间基准、统一坐标系、检查数据完整性。时间基准不统一是新手最容易翻车的地方——GPS通常是1Hz或10Hz，IMU可能是100Hz或200Hz，如果直接按索引对齐，滤波会发散。正确做法是按时间戳插值或最近邻匹配，把两路数据对齐到同一时间轴。
2.2 用MATLAB做时间对齐与坐标转换的最小代码
下面这段代码演示如何把IMU数据从载体系转到导航系，并按GPS时间戳对齐。假设IMU数据已经做了零偏补偿，姿态角由外部提供或由陀螺积分得到。
% 假设 imu_data 为 N×7 矩阵：[time, ax, ay, az, gx, gy, gz] % gps_data 为 M×4 矩阵：[time, lat, lon, alt] % 姿态角 roll, pitch, yaw 为 N×1 向量，单位弧度 % 1. 按GPS时间戳对齐IMU数据 gps_time = gps_data(:,1); imu_time = imu_data(:,1); aligned_imu = interp1(imu_time, imu_data(:,2:7), gps_time, 'linear', 'extrap'); % 2. 载体系到导航系的旋转矩阵（NED） N = length(gps_time); acc_ned = zeros(N,3); for k = 1:N r = roll(k); p = pitch(k); y = yaw(k); % 旋转矩阵 R_b2n = Rz(y)*Ry(p)*Rx(r) Rx = [1 0 0; 0 cos(r) sin(r); 0 -sin(r) cos(r)]; Ry = [cos(p) 0 -sin(p); 0 1 0; sin(p) 0 cos(p)]; Rz = [cos(y) sin(y) 0; -sin(y) cos(y) 0; 0 0 1]; R_b2n = Rz * Ry * Rx; acc_ned(k,:) = (R_b2n * aligned_imu(k,1:3)')'; end % 3. 扣除重力（NED下重力指向天，即第三轴为负） g = 9.80665; acc_ned(:,3) = acc_ned(:,3) + g;这段代码的逻辑是：先按GPS时间戳对IMU数据做线性插值，保证两路数据在同一时间轴上；然后用欧拉角构造旋转矩阵，把比力从载体系转到NED系；最后在NED的第三轴加上重力分量，得到纯运动加速度。参数上需要注意：插值方法用linear还是previous取决于IMU噪声水平，如果IMU噪声大，previous更稳；重力值按当地纬度微调，赤道和极地差约0.5%，对长时积分有影响。
2.3 数据完整性检查的三个必看指标
对齐之后别急着跑滤波，先看三个指标：时间戳是否单调递增、是否有重复时间戳、IMU的采样间隔是否均匀。时间戳回退通常意味着数据记录有误，重复时间戳会导致插值报错，采样间隔不均匀则说明IMU有丢包。我一般用diff和histogram快速扫一遍，发现问题就回到原始数据段截取干净区间，不要试图用滤波去掩盖数据缺陷。
3. GPS单点解算：从伪距到位置速度的MATLAB实现
3.1 最小二乘解算的迭代逻辑与收敛条件
单点解算的目标是解出接收机的位置（x,y,z）和钟差（dt）。观测方程是伪距等于几何距离加钟差加电离层对流层误差加噪声。由于几何距离是位置的非线性函数，需要用泰勒展开线性化，然后迭代求解。常见做法是给一个初始位置（比如上一次的解或地心），计算雅可比矩阵，解最小二乘，更新位置，直到位置变化小于阈值（比如1e-3米）或迭代次数超过10次。
MATLAB里可以用lsqnonlin或手写迭代。手写的好处是能看清每一步的残差和收敛过程，方便排查为什么解算结果跳变。下面给出手写版本的核心片段。
% 输入：sat_pos 为 n×3 卫星ECEF位置，pseudo 为 n×1 伪距 % 输出：pos 为 1×3 接收机ECEF位置，bias 为钟差 function [pos, bias] = spp_solve(sat_pos, pseudo, init_pos) pos = init_pos; bias = 0; for iter = 1:10 n = size(sat_pos,1); H = zeros(n,4); y = zeros(n,1); for i = 1:n dx = sat_pos(i,:) - pos; rho = norm(dx); H(i,1:3) = -dx / rho; H(i,4) = 1; y(i) = pseudo(i) - rho - bias; end % 最小二乘解 delta = (H' * H) \ (H' * y); pos = pos + delta(1:3)'; bias = bias + delta(4); if norm(delta(1:3)) < 1e-3 break; end end end这段代码的关键在雅可比矩阵H的构造：前三列是接收机到卫星的单位向量取负，第四列是钟差系数1。每次迭代用当前估计的位置算几何距离，更新位置和钟差。收敛条件用位置增量小于1e-3米，实际数据里如果伪距噪声大，可以放宽到1e-2米。注意初始位置不能给得太离谱，否则泰勒展开可能不收敛，我一般用所有卫星位置的均值作为初值。
3.2 卫星位置计算与地球自转修正
卫星位置不能直接用广播星历的粗略值，需要按星历参数计算，并做地球自转修正。信号传播时间内地球转过的角度会让卫星位置在ECEF系里偏移，修正量约为传播时间乘以地球自转角速度。MATLAB里可以用ecef2lla和lla2ecef做坐标转换，但要注意这些函数对高度和纬度的定义。
常见做法是：先算信号发射时刻的卫星位置，再算传播时间，然后用传播时间修正卫星位置，迭代两次即可。如果忽略这一步，单点解算的平面误差可能到几米，对后续组合导航的观测更新有影响。
3.3 单点解算精度评估：DOP值与残差分析
解算完位置后，别只看结果，要看DOP值和残差。DOP由H矩阵的迹决定，几何分布差的时候DOP能到5以上，位置精度会成倍恶化。残差则反映伪距观测的质量，如果某颗卫星残差明显大于其他卫星，可能是多路径或星历异常，应该在组合导航前剔除。
我一般会画三张图：卫星天空图、DOP随时间变化、残差直方图。天空图看几何分布，DOP曲线看哪些时段不适合做观测更新，残差直方图看有没有系统性偏差。这三张图能提前暴露大部分数据问题，比直接跑滤波再回头找原因省时间。
4. 松耦合滤波：15维状态卡尔曼滤波的MATLAB落地
4.1 状态方程与观测方程的维度选择
松耦合是组合导航里最常用的结构：GPS和IMU各自独立解算，然后把GPS的位置速度作为观测量，去更新IMU的导航解。状态向量一般取15维：位置误差（3）、速度误差（3）、姿态误差（3）、加速度计零偏（3）、陀螺零偏（3）。状态方程由IMU的误差传播模型给出，观测方程则是GPS位置速度与IMU位置速度之差。
为什么选15维而不是9维或21维？9维不含零偏，滤波发散快；21维含比例因子，对普通MEMS IMU没必要，反而增加可观测性差的维度。15维是精度和复杂度的平衡点，适合大多数车载和机载场景。
4.2 状态转移矩阵与噪声矩阵的构造
状态转移矩阵F由IMU的比力和姿态决定，噪声矩阵Q由IMU的噪声密度和采样时间决定。下面给出F的核心构造代码，假设导航系为NED。
% 输入：fn 为导航系比力 3×1，C_b2n 为载体系到导航系旋转矩阵 % 输出：F 为 15×15 状态转移矩阵 function F = build_F(fn, C_b2n, dt) F = eye(15); % 位置误差对速度误差 F(1:3,4:6) = eye(3) * dt; % 速度误差对姿态误差（比力的反对称矩阵） F(4:6,7:9) = -skew(fn) * dt; % 速度误差对加速度计零偏 F(4:6,10:12) = C_b2n * dt; % 姿态误差对陀螺零偏 F(7:9,13:15) = -C_b2n * dt; % 其余项由误差传播模型给出，此处省略高阶小量 end function S = skew(v) S = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end这段代码里，skew函数构造反对称矩阵，用于把比力叉乘写成矩阵形式。F矩阵的稀疏结构决定了滤波的计算量，MATLAB里用稀疏矩阵存储能提速。Q矩阵一般取对角阵，对角线元素是各状态噪声的方差，加速度计和陀螺的噪声密度从IMU手册查，采样时间用实际间隔。
4.3 观测更新与卡尔曼增益的数值稳定性
观测更新是滤波里最容易出问题的地方。GPS位置速度的噪声协方差R如果设得太小，增益会很大，滤波结果被GPS跳变带偏；设得太大，IMU的漂移又压不住。我一般先用GPS单点解算的残差统计来估计R，位置噪声取2到5米，速度噪声取0.1到0.5米每秒，具体看接收机型号和天线环境。
数值稳定性方面，协方差矩阵P在更新时容易失去对称正定性。常见做法是每次更新后做一次P = (P + P')/2，或者用Joseph形式更新。MATLAB里可以用chol检查正定性，如果报错就说明P已经病态，需要减小Q或增大R。
% 卡尔曼更新核心片段 K = P * H' / (H * P * H' + R); x = x + K * (z - H * x); P = (eye(15) - K * H) * P; P = (P + P') / 2; % 强制对称这段代码里，H是观测矩阵，松耦合下取单位阵映射位置速度。z是GPS位置速度减去IMU位置速度。K是增益，x是状态误差估计。更新后强制对称是血泪经验，不做这一步，跑几千步后P会不对称，滤波结果开始震荡。
4.4 反馈校正与闭环实现
滤波估计出状态误差后，要反馈到导航解里：位置速度直接减误差，姿态用误差角修正旋转矩阵，零偏则累加到IMU原始数据上做补偿。闭环还是开环取决于应用场景：闭环能抑制零偏漂移，但实现复杂；开环简单，适合短时实验。我一般做闭环，因为MEMS陀螺的零偏不稳定，开环跑几分钟就偏了。
反馈时注意姿态误差的修正顺序：先修旋转矩阵，再修零偏，最后清空状态误差。顺序错了会导致姿态和零偏耦合，滤波收敛变慢。
5. 避坑与排查：组合导航MATLAB实现里最容易翻车的五个点
5.1 滤波发散：现象是位置误差随时间增大而非收敛
现象是跑了几百步后，位置误差曲线开始上扬，协方差P的迹持续增大。原因通常是Q设得过大或R设得过小，导致增益异常，滤波把噪声当信号。解决方法是先用GPS单点解算的残差估计R，再把Q降到IMU手册噪声密度的量级，跑一遍看P的迹是否稳定。如果还发散，检查F矩阵的符号，尤其是速度误差对姿态误差那一项，符号错了会正反馈。
5.2 GPS失锁后轨迹跳变：现象是失锁期间IMU单独积分，重捕后位置突跳
现象是GPS信号遮挡几十秒后恢复，滤波结果出现几十米的跳变。原因是失锁期间IMU零偏漂移累积，重捕时GPS观测与IMU预测差太大，增益把状态拉过去。解决方法是失锁期间增大Q，让滤波更信任IMU；重捕时先用新息做卡方检验，剔除异常观测，再逐步恢复R。我一般会设一个失锁计数器，超过阈值就切换滤波模式。
5.3 时间戳不同步：现象是滤波新息有周期性波动
现象是新息序列出现与GPS更新率一致的周期波动。原因是IMU和GPS的时间戳没有严格对齐，或者插值引入了相位延迟。解决方法是检查两路数据的时间戳来源，确保用同一个时钟；插值时用interp1的linear方法，避免nearest带来的阶跃。如果波动还在，画新息的自相关函数，看周期是否与GPS更新率一致。
5.4 姿态初始化误差：现象是初始几分钟位置误差大，之后收敛
现象是滤波刚开始时位置误差大，几分钟后慢慢收敛。原因是初始姿态角不准，导致比力投影到导航系时有误差，积分后位置偏。解决方法是先用GPS速度航向或磁力计做初始对准，静止时用加速度计测横滚和俯仰。如果初始对准做不了，就在滤波初期增大姿态误差的P，让滤波快速修正。
5.5 数值精度问题：现象是长时间跑后协方差矩阵报错
现象是跑几万步后MATLAB报chol或inv错误，提示矩阵非正定。原因是浮点累积误差让P失去对称性。解决方法是每次更新后强制对称，或者用平方根滤波。MATLAB的single精度不够，全程用double。如果还报错，检查Q和R是否量级差太大，量级差超过1e6时数值条件数会恶化。
6. 进阶技巧：用新息序列做滤波健康度在线评估
跑通基本滤波后，怎么知道当前滤波是否健康？我一般看新息序列。新息是观测值减去预测观测值，理论上应该是零均值白噪声。如果新息有偏或自相关，说明模型或噪声参数有问题。MATLAB里可以实时算新息的均值和方差，画滑动窗口曲线。
% 新息健康度评估 innov = z - H * x; % 新息 window = 50; innov_mean = movmean(innov, window); innov_std = movstd(innov, window); % 如果 innov_mean 偏离零超过 3*innov_std，说明滤波异常这段代码里，movmean和movstd做滑动统计，窗口取50步，对应GPS更新率下的几秒。如果均值偏离零超过三倍标准差，说明观测模型有偏或GPS有异常，应该触发故障检测。这个技巧在实测数据里特别有用，能提前发现多路径或星历异常，比等滤波发散再回头找原因省事。
另一个进阶方向是自适应滤波：根据新息统计在线调整R。新息方差大时增大R，降低GPS权重；新息方差小时减小R，提高GPS权重。实现上可以用R = alpha * R + (1-alpha) * innov * innov'，alpha取0.95到0.99。注意R不能太小，否则滤波会被噪声带偏。
我自己的习惯是每次跑完滤波，先看新息序列，再看协方差迹，最后看位置误差曲线。三张图都正常，才认为这次组合导航是可信的。如果新息有周期性波动，先查时间同步；如果协方差迹上扬，先查Q和R；如果位置误差在GPS失锁后跳变，先查失锁期间的Q设置。这套排查顺序帮我省了很多后悔药。希望帮到你。
本文还有配套的精品资源，点击获取
