简介：这套用Matlab编写的全球定位系统与惯性导航系统组合导航（即卫星/惯导位置组合）程序，面向导航工程、测绘遥感以及无人机、车辆定位方向的初学者和研发人员，用于解决城市峡谷或信号干扰下全球定位系统易失锁、而纯惯性导航误差随时间累积的工程问题。资源包共5个文件，包括两个M脚本（一个位置组合演示主程序，一个基于卡尔曼滤波的全球定位系统与惯性导航位置融合实现）、一份MAT数据文件、一份TXT程序说明和一份DOC结果分析文档，压缩包整体仅670KB，结构简洁，便于按需取用。目前已有1758人学习下载。程序完整演示了卫星信号正常时用全球定位系统校正惯性导航初值、信号丢失后由惯性导航继续推算直至卫星恢复的典型流程；对照说明文档可掌握松组合位置融合的代码框架、卡尔曼滤波参数设置和误差校正方法，再利用数据文件与结果文档进行验证分析，还能帮助理解组合导航中数据同步与状态估计的关键细节，为后续扩展速度、姿态组合或紧组合研究提供清晰的起点。 车子刚钻进高架下的阴影，导航上的位置点就开始“漂”到旁边的辅路上，等驶出隧道那几秒，轨迹直接断成了折线——跑过复杂路段的人多半见过这画面。单纯靠GPS定位的车载导航，在城市里从来都是勉强够用，卫星信号一弱，误差随随便便几十米。而把GPS和INS（惯性导航系统）组合在一起解决这种场景，正是导航领域里最经典、也最实用的一招。这篇内容就围绕“GPS+INS组合导航”展开，聊聊它的基础原理、工程化落地的关键点，以及实测中容易踩的坑，适合正在做定位方案选型、搞导航算法入门、或者对高精度定位好奇的开发者作为参考。
1. 为什么单独的GPS靠不住：从一次城市路测说起
先说一个实际测试结果：在某城市中心路段，同一台设备、同一部手机，用纯GPS模式记录轨迹，就能看到明显的跳变和漂移。最夸张的一段，位置点偏离真实道路大约40米，且方向完全不对——车子明明在桥下直行，轨迹线却拐到了旁边的楼宇区。这就是GPS单独工作的典型问题，不是硬件差，而是信号环境本身就决定了它的短板。
GPS定位的原理是接收多颗卫星的电磁波信号，计算信号传输时间，再用三边测量法解算出接收机坐标。问题在于，卫星信号从两万公里外传来，到达地面的实际功率非常弱，很容易受到环境干扰，具体区分有这几类：
- 多径效应：城市里高楼、立交桥、隧道墙壁都会反射卫星信号。接收机收到的既有直射波，又有反射波。反射波路径更长，到达时间更晚，解算时相当于引入了一个“假距离”，位置自然就偏了。
- 卫星几何分布（DOP值）：定位精度取决于卫星在天空中的分布状态。分布越“散”，几何结构越理想，精度越高；如果几颗卫星挤在天空同一侧，水平精度因子（HDOP）就会变大，即使信号没问题，位置解的误差也会放大数倍。
- 遮挡和失锁：隧道、地下停车场、高架桥下方，卫星信号直接消失，GPS下面就是裸奔状态——要么输出上一次的定位结果，要么干脆无解。
GPS还有个固有缺陷：更新频率低，民用接收机一般到10Hz，运动稍快一点，相邻两个定位点之间的轨迹就会变得很粗糙。单独用GPS做事，本质上是把一个有“瞬时误差”且“间断性失效”的传感器，当成唯一位置来源，结果自然不靠谱。
那INS在这时候就能派上用场了。它完全靠自身搭载的加速度计和陀螺仪，测算载体相对初始状态的加速度和角速度，通过积分推算速度和位置。车载导航里常用的六轴IMU方案，就是一个三轴加速度计加一个三轴陀螺仪，不需要任何外部信号，也不会被遮挡干扰，更新频率可以轻松做到100Hz以上。单独看INS，它在短时间内推算出的轨迹平滑度很好，但时间一长，积分的误差会像滚雪球一样累积——加速计零偏漂移一点，速度积分就偏了，再积分一次，位置误差直接变成随时间平方增长的量级，十几分钟就能漂出几公里。
一个短期准但不稳定，一个长期稳但瞬时乱，两个传感器就像一对互补的搭档：GPS负责把INS的长期漂移拉回来，INS负责在GPS被遮挡或跳变时撑住短时精度，这就是组合导航最根本的动机。
2. 组合导航的底层逻辑：卡尔曼滤波如何把两套系统拧成一股绳
组合导航在算法层有一个近乎统一的主架构：卡尔曼滤波（Kalman Filter）。它做的事情可以理解为“加权融合”——把GPS和INS各自的输出，按照不确定度大小进行加权，得到一个比任何单一传感器都更接近真实值的结果。
卡尔曼滤波的运作可以拆成两个阶段，对应它的两个核心方程：
- 预测（Time Update）：利用INS在上一时刻的导航状态（位置、速度、姿态），通过IMU的加速度和角速度积分，推算出当前时刻的状态预测值。预测的同时，系统也会更新状态协方差矩阵——协方差越大，代表对这个预测结果越不确信。
- 更新（Measurement Update）：GPS输出一帧有效定位结果后，拿它和预测值做差，得到“新息”。新息乘以一个卡尔曼增益K，作为修正量加到预测值上，得到当前时刻的最优估计。
卡尔曼增益K的取值，本质上是一个置信度分配问题：如果GPS的观测噪声协方差R很小，说明GPS这次定位可信度高，K就会变大，融合结果更倾向于GPS；如果INS的预测协方差P很大，说明纯推算漂移已经积累得比较厉害，K同样会变大，让GPS来纠偏。
这里用个相对直观的类比：你蒙着眼睛在一片空旷但偶尔有标记桩的场地上走。身体的步幅感知相当于INS，能让你短时间内维持对位置的感觉，但步幅估计总有一两点偏差，走得越远，你对自己位置的判断越没底；而标记桩相当于GPS，每看到一次就相当于拿到一次绝对位置参考。卡尔曼滤波干的事，就是在“我凭感觉走到的位置”和“标记桩告诉我的位置”之间，根据两者的可信度快速算出最优落点。
实际工程中，组合导航系统通常会建立一组更完整的状态向量，包含位置误差、速度误差、姿态误差、陀螺和加计的零偏等，在滤波过程中持续估计和修正IMU的误差参数。这正是组合导航比简单“GPS主、INS备”强的地方——它不止用GPS来更新位置，还能反过来在线校准INS的漂移。比如车辆在直线行驶时，GPS信号能帮着把航向角误差的估计收敛出来，这个修正后的航向角，再被用于INS的解算，整个系统就会越跑越准。也正因如此，启动时GPS表现正常的时段，对后续长时间精度有决定性影响。
3. 三种组合架构：松组合、紧组合、深组合到底差在哪
组合导航不是只有一种接法。根据GPS接收机内部输出的数据粒度和与INS融合的深度，工程上一般分成三个层级：松组合（Loose Coupling）、紧组合（Tight Coupling）和深组合（Deep Coupling）。
3.1 松组合（Loose Coupling）
松组合是最好理解的方案：GPS接收机自己先把定位解算完成，输出位置和速度；INS独立做惯性解算，输出另一份位置和速度；然后卡尔曼滤波器把两者的差值当观测值，估计INS的误差并进行修正。公共汽车上常见的车机导航，很多就是这种逻辑。
优点是实现简单，不需要改动GPS接收机内部的跟踪环路，硬件选型灵活，普通U-blox模块加一个六轴IMU就能跑起来。缺点是整个系统的性能上限已经被GPS接收机锁死——GPS内部的解算已经做了一次噪声平滑，丢失了一部分原始观测信息，而且一旦GPS失锁或输出数据异常，组合系统只能退化为纯INS模式，等待GPS回归。
3.2 紧组合（Tight Coupling）
紧组合不再使用GPS接收机解算后的位置结果，而是直接拿原始观测量：伪距、伪距率（多普勒频移），甚至载波相位。在同一个卡尔曼滤波器里，不仅估计导航状态，还同时估计GPS接收机的钟差和钟漂。
这么做的好处非常明显：GPS在视野仅有3~4颗卫星时，仍然可以输出原始观测量，紧组合利用INS对接收机位置和速度的先验估计，能辅助解算并稳定输出位置结果。城市高架、峡谷路段，卫星数波动剧烈，紧组合的抗干扰能力就体现出来了。代价是算法复杂度上了一个台阶——需要处理GPS原始数据协议（如U-blox的UBX-RXM-RAWX），需要建立伪距和载波相位的观测模型，还得应对周跳检测等工程难题。
3.3 深组合（Deep Coupling）
深组合通常用在军用或高抗干扰场景。INS不仅参与导航解算，还直接把预测的多普勒频移反馈到GPS接收机的跟踪环路里，辅助载波环和码环维持信号锁定。极端情况如强干扰下，GPS信号信噪比掉到常规跟踪门限以下时，深组合系统仍能通过INS辅助把信号锁住一段时间。消费级和多数工业级项目基本用不到这么深，选型时不建议为了追概念而选深组合——成本和可靠性的负担都会明显增加。
从实际项目选型的角度看，我的建议是：能用松组合起步就不要直接上紧组合。先跑通完整的数据链路，再根据实际测试结果判断是否需要紧组合。很多场景下，松组合加一个好的IMU型号，配合严谨的标定流程，已经能覆盖绝大多数驾驶场景的精度需求。
4. 工程落地的头号难题：时间同步与坐标系对齐
如果说卡尔曼滤波是组合导航的“大脑”，那时间同步和坐标系对齐就是它的“神经”。这个环节做得不好，再好的滤波器也是白搭。
4.1 时间同步
GPS输出位置信息的时间戳，和IMU采样时刻之间存在一个微妙的时间差。GPS接收机从收到信号到解算出位置需要一定时间，输出延迟可能达到几十到一百毫秒；而IMU的数据则是按固定中断时刻采样，基本没有延迟。如果直接拿两个带时间错位的量喂给滤波器做“观测更新”，等于把两个不同时刻的状态硬凑在一起，产生的误差不可忽视。
工程上常见的做法是：用GPS接收机的PPS（Pulse Per Second）秒脉冲信号，对齐系统时钟。每个PPS上升沿对应GPS时间的整秒时刻，系统在主控里捕获PPS硬中断，记录本地时钟对应的计数器值，从而实时修正主控的本地时钟偏差。IMU每次采样时打上本地时钟时间戳，GPS定位结果输出时同样打上本地时间戳，这样一来，即便两个数据源异步到达，滤波器也能根据时间戳做插值或外推，把观测量调整到同一个时间基准上。
有朋友做过一个对比实验：同一套硬件，时间同步误差约50ms时，动态场景下的组合导航轨迹偏差比时间同步误差小于5ms的情况大了近两倍。原因在于，车辆在高速行驶时50ms的位置变化已经有一米多，而滤波器看到的位置差里混入了这部分时间错位产生的“伪误差”，修正方向自然就偏了。
4.2 坐标系对齐
GPS输出的坐标系是ECEF（地心地固系）或经纬度高程（LLA），而INS解算时通常工作在北东地（NED）导航系或站心坐标系下，IMU测量的又是在载体系（通常为右-前-上）下的加速度和角速度。三者之间要靠一个姿态矩阵（方向余弦矩阵DCM）串起来。
这里最麻烦的不是公式本身，而是矩阵初值的正确性。如果IMU安装时没有严格对准车体前进方向，装出一个比如5度的航向偏角，那么车辆直行时，IMU输出的加速度会被错误地分解到东西、南北两个方向，时间一长就会积累出一个纯几何偏差。这个问题在低速起步和停停走走的路况下尤其明显。因此有经验的工程师在安装车载组合导航设备时，会优先保证IMU轴线与车体中轴线基本平行，残余的小角度偏差则在标定环节通过确定性航向安装角标定做纠正。
4.3 杆臂效应补偿
杆臂效应是另一个容易被忽略的工程细节。GPS天线通常安装在车顶，而IMU安装在车内某个位置，两者之间存在一个固定空间距离（杆臂）。车辆转弯或颠簸时，车顶天线位置和车内IMU位置会因为角运动产生不同的线加速度，如果直接拿两者的位置做差，就会在观测方程里引入一一对应的杆臂误差。
解决办法是提前量出杆臂在载体系下的三个分量，在GPS观测位置换算到IMU位置时做一步刚性平移补偿。固定安装的设备一般只需要标定一次，但如果设备被移动过天线位置，需要重新标定，没有例外。
5. IMU选型与标定：一只脚没站稳，整个系统都会晃
很多入门者把组合导航的重点放在滤波算法上，把大量的精力放在研究扩展卡尔曼滤波、粒子滤波，结果实车一跑，误差大得离谱。问题多半不在算法，而在IMU本身。
5.1 IMU等级怎么选
IMU的核心指标之一是零偏稳定性（Bias Stability），它决定了在无GPS修正时，纯惯性推算的位置漂移速度。零偏稳定性在1°/h以上的消费级IMU，纯惯性导航的漂移可能在每分钟几米到几十米；而工业级的10°/h到50°/h的IMU，虽然短期内差不了太多，但失锁时间一长就开始裸露原型——几秒钟的桥洞尚能应付，一分钟以上的隧道就得靠阈值判断重新初始化。
项目选型时，我会先用一个简单公式估算需求：隧道或遮挡路段的极限持续时间。假设车辆时速60km/h，需要纯惯性支撑20秒才能穿过隧道，那么需要保证位置误差在几米量级内。结合零偏稳定性去推算积分漂移，能初步判断IMU是否达标。别迷信“高精度=高价格”，消费级IMU在短期遮挡场景下往往已经够用，关键是把零偏、比例因子这些误差标定干净。
5.2 零偏标定：六面静态的两个重要细节
IMU的零偏标定，通常是把设备按六个面分别静止一段时间（常见做法是每个面静置1~2分钟），利用重力矢量在载体系三个轴上的投影变化，标出加计的零偏和比例因子。陀螺仪的零偏，则在静止状态下采集角速度输出，做均值统计得出。
标定过程中有两个细节特别影响结果：
- 地面隔振：标定平台如果放在桌子上，周围有人走动或车辆经过，地面微振会污染陀螺仪数据。一个可行的做法是把标定平台放在地下室的混凝土底座上，或者用重物压实地垫，尽可能减少外部振动干扰。
- 温控环节：MEMS器件的零偏随温度变化明显，常温下标定的值到了夏天车内的60多摄氏度环境下会出现可观的漂移。靠谱的做法是在静态标定前后各做一次温箱循环测试，观察零偏的温度曲线，必要时在算法里做温度补偿。即使不建完整温补模型，也要至少记录设备在正常工作温度范围两端的数据，确认漂移量和系统容忍度之间的关系是否可控。
5.3 初始对准
组合导航设备开机后，并不能立即输出正确姿态，需要先完成初始对准。最常见的是静态对准——设备静止数秒到数分钟，利用加速度计感知重力方向确定横滚和俯仰角，利用陀螺仪感知地球自转角速率来确定航向。问题在于，消费级MEMS陀螺仪的噪声远大于地球自转信号，静态航向对准精度通常只能到1度到几度级别。
如果需求对启动后的航向精度要求高，有几种变通办法：
- 车辆起步前做直线行驶校准：利用GPS在车辆直线运动时输出的一系列位置点，拟合出航向角，校正初始航向误差。
- 双天线GNSS测向：通过两天线间的载波相位差直接输出航向，能达到0.1度级别的精度，但硬件成本和安装体积都会上一个台阶。
- 磁力计辅助：缺点是极易受车辆电气系统和城市磁场干扰，只能作为粗辅助，不能作为长期航向源。
6. 实测效果：丢星、过桥洞、高架下的真实表现
这套组合导航方案，我跑过好几组真实路测，挑几个有代表性的场景说下表现。
场景一：GPS正常，城市快速路。松组合状态下，位置轨迹基本稳定贴合车道线，路口转弯时的圆弧轨迹比纯GPS平滑得多。同一台设备关掉INS，只保留GPS时的轨迹在大楼密集区会有明显的锯齿状跳变，与真实道路偏差最大时超过20米；打开组合导航后，同一路段的轨迹跳变基本消失，最大偏差缩小到几米级别。
场景二：连续高架下穿行。车速约70km/h，连续经过三个高架桥洞，每个桥洞遮挡时长约2~3秒。纯GPS在这段路上的表现是：位置点在高架正下方时突然向旁边路侧“弹跳”十几米，驶出后再跳回，轨迹线看起来就像被拧了一把。而组合导航只在桥洞最中心位置出现了不足1米的横向偏差，驶出后1~2秒内立即恢复正常，原因是INS在GPS失锁瞬间接管了解算任务，维持了一个连续平滑的轨迹。
场景三：长隧道。一个总长约1.2公里的城市隧道，纯惯性模式下（切断GPS修正）走完全程，最终位置误差大约在30到50米之间，主要来自航向漂移——隧道内没有转弯，航向只要偏0.5度，一公里下来横向偏差就能达到8到10米。那组测试的教训是：长隧道运营环境下，不能指望纯惯性撑全程，需要配套车辆轮速信号，或者提前把卫星信号引入隧道（比如隧道口放置中继天线），才能真正满足导航需求。
场景四：停车并起步。车辆静止一段时间后重新启动，GPS定位结果可能出现整段位置的渐变跳变（收敛过程）。松组合系统在静止检测逻辑下，会先停止对IMU的位置更新，GPS收敛完成后，再把位置修正平滑地应用到航迹上。如果这个平滑处理做得不好，轨迹就会在起步瞬间画出一个明显的“台阶”。这也是很多自行搭建实验平台的开发者容易忽略的点——静止检测和跑动检测阈值需要结合IMU噪声底噪去标定，不能简单套用出厂默认值。
7. 实测中容易踩的几个坑
组合导航的工程坑，好多都要实际跑设备才能遇到，这里集中说四个我自己踩过的。
坑一：GPS定位更新频率不够还不自知。很多消费级GPS模块默认配置是1Hz输出，组合导航里观测更新的频率直接被拉低到1秒一次。这种情况下，卡尔曼滤波的修正节奏被拖慢，INS长时间得不到修正，位置误差会周期性地在两次更新之间累积变大，轨迹出现“锯齿状恢复”。排查方法很简单：看GPS模块的NMEA句子里GGA和RMC的输出间隔，如果确实是1Hz，确认模块是否支持10Hz配置，并在启动脚本里显式设置。
坑二：IMU没有和车辆刚性固定。一些测试者担心车载震动会损坏IMU，就用双面胶或者软胶垫把IMU贴在车内台面上。结果转弯时IMU跟着软胶垫有微小倾斜，陀螺仪数据里混入了额外的角速度误差，组合导航反馈出来的姿态和车体真实姿态间总有几度偏差。正确做法是用金属支架刚性固定，接合面尽量平整紧贴，必要时用螺丝锁死。
坑三：把GPS天线放错位置。GPS天线紧挨着行车记录仪屏幕或手机支架，干扰源一样不少，包括视频信号、电磁辐射、供电电源噪声。受干扰的接收机会出现明显的载噪比降低，定位误差会随机放大。我自己的经验是，GPS天线务必放在车顶或后视镜基座上方，尽量远离金属遮挡和电子设备，并且天线地平面要可靠接车体金属（或使用自带地平面的天线）。
坑四：直接拿实验室算法上实车，忘记针对动态场景做滤波参数微调。静态仿真和跑台架上表现良好的卡尔曼增益，到了真实车辆振动、颠簸、转弯侧倾场景下往往偏大或偏小。痛点在于，增益偏大时系统过于信任GPS，把多径误差当成真实位置修正；增益过小时系统又过度依赖INS，GPS修正效果不明显。建议跑真实道路测试时，多录制几组不同路况的数据做离线回放，用相同算法调参数对比轨迹误差，找到在大多数路段表现相对均衡的一组参数，再固化到在线系统里。
8. 两个提升整体效果的小建议
如果时间允许，可以在组合导航跑通后顺手做两件事。
一是把车辆轮速（或车速）信号接入系统，作为速度观测源。在GPS遮挡的隧道场景里，轮速信号不依赖卫星，也不像IMU那样随积分时间漂移，可以显著提升长隧道内的位置预测稳定性。接入方式通常是通过CAN总线读取，有的车载导航主控也支持用脉冲信号直接接轮速传感器。这样做好处很大，代价只是在算法里增加一个一维观测更新。
二是记录一周各地的原始数据。不要只跑单位门口固定路线，尽量涵盖高架、隧道、林荫道、老城区窄路、高速等不同特征环境。每次测试都完整保存原始IMU数据、GPS观测值、滤波器中间状态。后续如果发现某个路段的定位表现异常，可以直接离线重放数据，定位是哪类干扰引起的，而不是等到下一次外出测试才排查。这个习惯，比多调一个滤波参数，可能更能帮你真正把组合导航调明白。
本文还有配套的精品资源，点击获取
