简介：这份资源面向学习组合导航算法与多源数据融合的本科生、研究生及工程技术人员，围绕GPS与INS互补定位这一核心问题，提供一套可直接运行的Matlab位置组合程序。压缩包共5个文件，约670KB，以m脚本为主，辅以mat数据文件、txt程序说明和doc结果文档，分别承担算法实现、输入数据、使用指引与结果展示，便于读者对照理解代码逻辑与运行流程。程序涵盖GPS初始化INS参数、卡尔曼滤波数据融合、实时状态更新、GPS失锁时切换INS模式及误差校正等环节，可帮助读者掌握松组合导航的基本框架，并借助示例数据复现位置组合结果、观察滤波前后误差变化。已有1766人学习下载，适合作为课程设计、科研入门或算法验证的参考素材。

1. 组合导航：为什么单靠 GPS 或 INS 都会翻车

跑过车载定位或者无人机导航的开发者大概都遇到过这种玄学现象：车过隧道时轨迹突然飘出去几十米，出隧道后好几秒才拉回来；或者无人机悬停时位置看着挺稳，一旦快速横滚，GPS 报出来的位置就开始画圈。这不是设备坏了，而是 GPS 和 INS 各自都有无法自愈的短板，单打独斗必然在某个工况下翻车。GPS 长期精度稳定、误差不随时间累积，但输出频率低、易受遮挡和多径影响，动态响应差；INS 靠陀螺仪和加速度计积分推算，短时精度极高、输出频率可达几百赫兹，但积分会累积漂移，几分钟就能偏出可用的范围。组合导航要解决的核心问题，就是让两个「各有死穴」的传感器互相补位——用 GPS 的长期稳定性去约束 INS 的漂移，用 INS 的高频动态去填补 GPS 的更新间隙和失锁时段。搞懂这套互补机制为什么能 work、滤波器到底在估什么、失锁时系统靠什么撑住，是从「会调参」走到「敢上车」的分水岭。

2. 松耦合与紧耦合：两套融合架构到底差在哪

组合导航的架构选择，决定了系统在卫星信号变差时的退化行为。常见的实现思路分两条线：松耦合和紧耦合。理解它们的差别，比记住「紧耦合更好」这个结论重要得多。

2.1 松耦合：把 GPS 当黑匣子，只取它的位置速度解

松耦合是最直观的做法。GPS 接收机自己先解算出位置和速度，组合导航滤波器把这份「成品」和 INS 推算的位置速度做差，作为量测输入去修正 INS。结构上像两个独立模块对接，GPS 内部怎么选星、怎么解算，滤波器完全不关心。

这种架构的优点是实现简单、模块解耦，GPS 换型基本不影响滤波器代码。但它的致命弱点在于：GPS 输出的位置速度是已经「加工过」的结果，接收机内部各颗卫星的原始观测信息（伪距、伪距率、载波相位）在解算时被压缩掉了。一旦可见卫星少于四颗，接收机自己就解不出位置，松耦合滤波器直接失去量测，只能纯惯导外推。换句话说，松耦合的可用性下限被接收机的解算门槛卡死了。

2.2 紧耦合：把原始观测直接喂进滤波器

紧耦合不取 GPS 的成品解，而是把伪距、伪距率这些原始观测和 INS 推算的对应量做差。滤波器状态里除了 INS 的误差状态，还要估计接收机钟差、钟漂这类参数。量测方程本质上是「用 INS 推算的位置反算卫星到接收机的几何距离，再和实测伪距比对」。

这样做的直接好处是：哪怕只剩一颗、两颗卫星，只要还有观测，就能形成约束，系统不会瞬间退化成纯惯导。卫星数不足四颗时，松耦合已经瞎了，紧耦合还能靠一两颗星把漂移压住一部分。代价是滤波器维数变高、观测模型和接收机原始数据格式强绑定，工程上调试量明显更大。

2.3 为什么工程上不是无脑选紧耦合

紧耦合理论优势明显，但落地时有几道坎。第一是数据接口：不是所有接收机都开放原始观测，很多低成本模块只吐 NMEA 语句，拿不到伪距。第二是时间同步：伪距观测的时刻必须和 INS 推算时刻严格对齐，差几毫秒在高速运动下就是米级误差，这要求硬件层面有精确的秒脉冲对齐。第三是观测质量：原始伪距里的多径、电离层误差没有经过接收机内部滤波，直接进滤波器反而可能引入野值。所以常见的工程取舍是——高动态、遮挡频繁的场景（如城市峡谷、无人机）倾向紧耦合，普通车载导航用松耦合加完好性监测就够。选架构前先问自己：我的场景里卫星失锁有多频繁、失锁时对精度要求多高。

3. 卡尔曼滤波在估什么：15 维状态背后的物理意义

组合导航的灵魂是卡尔曼滤波，但很多人调了半天参数，其实没搞清楚状态向量里每一项代表什么物理量、为什么这么排。把状态定义讲透，后面的调参才有依据。

3.1 状态向量的构成与排列逻辑

典型的 INS/GPS 组合滤波器采用误差状态卡尔曼滤波（ESKF），估计的不是绝对状态，而是「真实值减去惯导推算值」的误差。常见 15 维状态排列如下：

# 15 维误差状态向量定义（导航系下） # 顺序：位置误差(3) 速度误差(3) 姿态误差(3) 陀螺零偏(3) 加计零偏(3) dx = np.zeros(15) # dx[0:3] -> 纬度/经度/高度误差，单位 m # dx[3:6] -> 东/北/天速度误差，单位 m/s # dx[6:9] -> 姿态失准角（小角度近似），单位 rad # dx[9:12] -> 陀螺零偏误差，单位 rad/s # dx[12:15] -> 加速度计零偏误差，单位 m/s^2

为什么用误差状态而不是直接估绝对状态？因为姿态是旋转量，直接在线性滤波器里表示会破坏正交性，而误差是小量，可以用小角度近似做线性化，数值上更稳。位置、速度、姿态这 9 维是「导航解」，后 6 维零偏是「传感器内参」——滤波器在估计导航误差的同时，顺带把陀螺和加计的零偏也估出来，这样下次积分时能把零偏扣掉，漂移速度就慢下来了。这是组合导航比单纯「GPS 修正位置」高明的地方：它修正的不只是结果，还有产生误差的源头。

3.2 状态转移：误差是怎么随时间传播的

误差状态的传播由惯导的误差方程描述，核心是姿态误差会耦合进速度误差、零偏会耦合进姿态和速度。离散化后的状态转移矩阵大致长这样：

# 简化版状态转移（仅示意姿态-速度耦合项） F = np.eye(15) # 姿态失准角对速度误差的耦合：f x phi（比力叉乘） F[3:6, 6:9] = -skew(accel_nav) * dt # 姿态误差自身的传播：-omega_ie x phi F[6:9, 6:9] = -skew(omega_nav) * dt # 零偏对姿态/速度的耦合 F[3:6, 12:15] = C_bn * dt # 加计零偏 -> 速度 F[6:9, 9:12] = -C_bn * dt # 陀螺零偏 -> 姿态

skew()是反对称矩阵，C_bn是机体系到导航系的旋转矩阵。这段代码的物理含义是：加速度计测到的比力如果带着姿态误差，就会被错误地投影到导航系，产生速度误差；陀螺零偏会让姿态持续缓慢转动，进而污染速度。滤波器正是靠这些耦合关系，从「位置速度的偏差」反推出「姿态和零偏错在哪」。过程噪声矩阵 Q 则描述这些误差传播的不确定性，Q 给太小滤波器过度自信、给太大又跟着噪声跑，这是调参的第一道坎。

3.3 量测更新：GPS 观测如何拉回漂移

松耦合下量测方程很直接：GPS 位置减 INS 位置、GPS 速度减 INS 速度，得到量测残差，再乘卡尔曼增益去修正状态。

# 松耦合量测更新（位置+速度，6 维观测） H = np.zeros((6, 15)) H[0:3, 0:3] = np.eye(3) # 位置观测对应位置误差状态 H[3:6, 3:6] = np.eye(3) # 速度观测对应速度误差状态 y = gps_pv - ins_pv # 量测残差 S = H @ P @ H.T + R # 新息协方差 K = P @ H.T @ np.linalg.inv(S) dx = K @ y # 误差状态估计 P = (np.eye(15) - K @ H) @ P

R是 GPS 观测噪声协方差，它决定了滤波器多信任 GPS。R 设小了，GPS 一有野值就把整个状态带偏；R 设大了，漂移压不住。工程上常用新息协方差做自适应：当残差持续偏大，说明 GPS 可能被多径污染，临时放大 R 降低权重。修正完dx后，要把误差反馈回惯导解算，姿态修正要用旋转矩阵相乘而不是直接加，否则正交性会被破坏——这是新手最容易踩的坑之一。

4. 失锁之后靠什么撑住：惯导递推与零速修正

GPS 失锁是组合导航最真实的考验。隧道、地下车库、城市高楼之间，卫星说没就没。这时候系统能不能撑住，取决于惯导递推的质量和辅助约束的设计。

4.1 纯惯导递推的误差增长曲线

失锁瞬间，滤波器停止量测更新，只剩状态预测。此时位置误差大致按时间的平方增长（速度误差积分成位置），姿态误差按时间线性增长。以中等精度 IMU 为例，陀螺零偏 1°/h 量级，失锁 30 秒姿态可能漂零点几度，位置漂几米到十几米；失锁几分钟，纯惯导基本就不可用了。所以失锁期间的第一原则是：尽量缩短失锁时间，或者引入其他约束。

常见的实现思路是在失锁时切换滤波器模式——不再做量测更新，但继续用之前估出来的零偏做补偿，让递推尽量干净。同时把过程噪声适当放大，表示「我现在不确定度在涨」，这样一旦 GPS 恢复，滤波器会用较大的增益快速拉回，而不是因为过度自信而修正缓慢。

4.2 零速修正：停车时白捡的约束

车辆等红灯、进地库停车，速度真值为零，这是一个不需要任何外部传感器就能用的约束。零速修正（ZUPT）把「速度为零」当成一个虚拟观测喂进滤波器：

# 零速修正：检测到静止时，用零速度作为量测 if is_stationary(accel, gyro): H_zupt = np.zeros((3, 15)) H_zupt[0:3, 3:6] = np.eye(3) # 观测速度误差 y_zupt = np.zeros(3) - ins_vel # 残差 = 0 - 当前速度 # 用 y_zupt 做一次标准量测更新

is_stationary通常靠加速度模长接近重力、陀螺模长接近零、加计方差小这几个条件联合判断。零速修正的妙处在于：它能在 GPS 完全失锁时持续约束速度误差，进而间接约束姿态和零偏。地库里停几分钟，靠 ZUPT 就能把漂移压在一个可接受的范围，出场后 GPS 一恢复就能快速收敛。这套机制在车载组合导航里几乎是标配，血泪经验是静止检测的阈值要按实际振动环境调，阈值太松把行驶误判成静止，滤波器会被错误观测带偏。

4.3 非完整性约束：车辆不会横着走

轮式车辆还有个天然约束：正常情况下不会侧向滑移、不会腾空，所以车体系下的侧向速度和垂向速度近似为零。非完整性约束（NHC）把这两个方向的速度当成虚拟观测，同样能约束航向漂移。它和零速修正互补——ZUPT 管静止，NHC 管行驶，两者结合能在 GPS 弱的时候显著延缓退化。注意 NHC 在车辆打滑、急转弯、越野工况下会失效，需要检测侧向加速度或轮速差来动态关闭，否则反而引入错误约束。

5. 避坑与排查：组合导航落地时最容易翻车的几处

理论看着顺，工程上翻车的地方往往在细节。下面几条是实际调试中反复出现的现象、原因和解法。

5.1 出隧道后轨迹画大圈

现象：车辆出隧道后，轨迹先往一个方向甩出去再慢慢拉回，画出一个明显的圆弧。原因：失锁期间姿态误差累积，尤其是航向漂移，导致恢复瞬间 INS 推算位置和 GPS 位置差出一大截，滤波器用大增益一次性修正，姿态被猛拉，轨迹就出现回环。解决：失锁期间放大过程噪声、启用 ZUPT/NHC 约束，恢复时不要一次性全信 GPS，可以对新息做限幅或分几步渐进修正。

5.2 静止时位置缓慢爬行

现象：车停着不动，输出位置却以每秒几厘米的速度持续偏移。原因：零速修正没生效，或者静止检测阈值没覆盖当前振动水平；也可能是加计零偏没被估出来，递推一直在积分一个恒定偏差。解决：检查静止检测的加计方差阈值，确认 ZUPT 量测确实进了滤波器；观察零偏状态的收敛曲线，若零偏估计一直不收敛，检查 Q 矩阵里零偏对应的过程噪声是否给得太小。

5.3 GPS 恢复瞬间姿态跳变

现象：GPS 从失锁恢复，姿态角突然跳几度，甚至短暂震荡。原因：误差状态反馈时姿态修正用了直接相加而非旋转矩阵相乘，或者修正量过大导致线性化假设失效。解决：姿态反馈统一走旋转矩阵更新，大修正量时分多次小步反馈；同时检查量测噪声 R 是否设得过小，导致增益过大。

5.4 高速时位置系统性滞后

现象：车速越快，组合输出位置比真实位置越「拖后」。原因：GPS 观测和 INS 推算的时间戳没对齐，存在固定延迟，高速下延迟乘以速度就是固定位置偏差。解决：核对秒脉冲对齐逻辑，确认量测更新用的是同一时刻的 INS 状态；必要时在滤波器里对 GPS 延迟做补偿。

5.5 滤波器协方差发散

现象：位置误差越来越大，协方差矩阵 P 的对角元持续增长不收敛。原因：系统模型和实际不符，比如量测矩阵 H 写错、观测和状态的对应关系搞反，或者过程噪声 Q 严重偏大导致滤波器完全不信任模型。解决：先打印新息序列，正常应接近零均值白噪声，若持续有偏说明模型有问题；再检查 H 矩阵每一项对应的物理量是否匹配。

6. 精度、成本与鲁棒性的三角取舍

组合导航做到底，绕不开一个现实问题：精度、成本、鲁棒性三者很难同时拉满。高精度光纤陀螺配紧耦合能做出很好的动态性能，但成本和调试复杂度都上去了；低成本 MEMS 配松耦合够用，可一旦失锁就原形毕露。我一般的判断顺序是：先看场景里卫星可见性有多差、失锁有多频繁，这决定架构选松还是紧；再看动态有多剧烈，这决定 IMU 等级；最后才在剩余预算里抠滤波器调参的细节。反过来先纠结滤波器参数，往往是本末倒置。

验证一套组合导航是否真的可靠，别只看跑通的那段轨迹。有效的做法是人为制造退化：把 GPS 数据掐掉几十秒看漂移曲线，注入几个位置野值看滤波器会不会被带偏，把车速拉到高速看时间对齐有没有问题。这些「破坏性测试」比正常工况更能暴露设计缺陷。还有一个容易被忽略的点——滤波器的可观测性。静止时航向其实不可观测，靠 GPS 位置变化才能约束航向，所以长时间静止后起步，航向往往需要一段行驶才能收敛，这个特性要在系统行为上提前设计好，而不是等用户发现轨迹歪了再补救。

从那以后我每次搭组合导航，都强制先把失锁场景和野值注入跑一遍再谈精度指标，因为正常工况下的漂亮曲线说明不了任何问题，退化工况下的表现才是这套系统真正的底线。希望帮到你。

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