卡尔曼滤波在组合导航中的优化应用：从INS/GNSS融合到三维立体定位

前阵子和一个做低速无人车的朋友聊到一个很常见的现象：在高架桥下测试时，卫星定位的轨迹直接穿过了桥墩，而IMU推算的位置反倒一直贴着路面。两台传感器各报各的数，谁也说服不了谁。这个场景估计搞过定位的人都遇到过——单靠卫星信号，遮挡环境下会漂到让你怀疑人生；单靠惯导，十分钟后误差也能让你怀疑人生。把两者融合起来，就是我们常说的组合导航。这篇文章想把INS与卫星信号的融合算法从头拆开讲清楚，重点说说基于卡尔曼滤波技术的优化应用是怎么落地的，以及中间那些文档里很少写清楚的坑。

适合的读者主要有两类：一类是刚接触组合导航，正在搭第一个滤波器的学生或工程师；另一类是已经能跑通基本流程，但发现精度、稳定性总是差口气，想系统排查问题的同行。我也会把传感器选型、耦合方式、参数调优、异常保护这些内容都串进来，尽量做到看完能直接用来指导自己的项目。

1. 单传感器各自的软肋，恰恰是对方的机会

1.1 卫星信号不是不行，而是太“脆”

GNSS定位的原理其实很直白：接收机测量到多颗卫星的伪距，代入最小二乘或卡尔曼解算出位置。开阔天空下，实时动态差分（RTK）可以做到厘米级，普通单频也可以做到米级，而且误差不会随时间累积。这一点是惯导永远比不上的。

但这个系统的短板也很明显。第一，它对环境极其敏感。高楼峡谷、隧道、高架下、浓密树荫，信号被遮挡后定位精度断崖式下降，甚至直接失效。第二，它怕多径。城市里到处是玻璃幕墙和金属护栏，反射信号混进来产生十几米的偏差，而且这个偏差是正负震荡的，并不是单纯加个固定的偏移。第三，更新率通常只有10～20Hz，中间一两百毫秒的轨迹全靠插值，在急转弯或快速变道时会产生明显延迟。

有个现象很能说明问题：在城市里停车等红灯时，静止状态下的卫星定位轨迹往往会在两三米范围内飘来飘去，像喝醉酒一样。这是因为载噪比下降、多径和大气噪声都在起作用。你要是拿这种位置直接做控制，车辆会一路“画龙”，完全没有办法自动驾驶。

所以卫星导航的结论是：长期精度靠谱，短期连续性差；环境好时能当基准，环境差时直接抓瞎。

1.2 惯性导航很自主，但误差会“滚雪球”

惯性导航依靠IMU里的加速度计和陀螺仪，分别测量比力和角速度。加速度积分得到速度，速度再积分得到位置；角速度积分得到姿态。整个推算过程完全自主，不需要任何外部信号，无遮挡问题，更新率可以到200Hz甚至1000Hz，输出也是毫秒级连续。

但这个系统有个致命的毛病：误差是积出来的。陀螺仪哪怕只有1°/h的零偏，看着很小，姿态误差却会随时间线性增长。姿态一旦有了角度误差，重力分量就会被错误地分离到水平加速度上，产生水平位置误差；这个误差再经二次积分，位置漂移会以时间的平方级扩大。加速度计零偏更直接，0.1mg的零偏持续作用，理论上60秒纯积分就能漂出好几米。

我做过一个粗略估算：一个中等精度的MEMS IMU，零偏稳定性在10°/h级别，不加任何修正，90秒后位置漂移可以接近百米的量级。这在绝对定位眼里是完全不可接受的。

惯性导航的结论是：短期精度和连续性极好，绝对位置会随时间越来越离谱。

1.3 互补的本质：一个管“长期不漂”，一个管“短时精准”

把两个传感器的特点放到一起看，方向就非常明显了：卫星系统给惯导提供一个稳定的绝对参考，把随时间累积的漂移拽回来；惯导在卫星信号被遮挡、多径、低更新率的间隙里，提供高频、连续的相对运动和姿态信息。这就是组合导航最底层的动机，也是这个领域存在的基础。

卡尔曼滤波在这里的核心贡献，不是简单地把两个位置取平均值，而是根据各自的误差特性动态决定“谁的话更可信”。开阔天空下，卫星观测噪声小，滤波器会更信任卫星的绝对位置；进入高架下卫星明显飘的时候，通过观测残差可以把它的权重降下来，转而依靠惯导的短时推算。两者交替补位的效果，比任何一个单传感器都稳定得多。

组合导航输出的也不只是“一条位置轨迹”，而是一个完整的三维运动状态：三维位置、三维速度、三维姿态，以及陀螺和加速度计的零偏估计。这就是标题里的“三维立体组合导航”的实质——它在严格的数学框架里估计的是一个立体的运动状态，而不只是二维平面上的x、y坐标。

2. 卡尔曼滤波如何把两个不完美的信号“拧”成一条轨迹

2.1 从贝叶斯直觉说起：预测和更新就是“猜”加“改”

很多讲卡尔曼滤波的资料一上来就列公式，然后大家记住了公式却不知道它在干嘛。用大白话来说，卡尔曼滤波干的事情就是两件：先用系统模型预测当前状态，再用观测数据对预测结果做修正。预测和修正各自带一个不确定性，最后的结果是两者按不确定性加权折中出来的最优估计。

你可以把它想成两个人估计一件事情：一个人是根据经验和历史在猜，另一个人拿仪器在测。如果猜的那个人历史表现很稳定，而仪器今天噪声特别大，你就更愿意相信猜的结果；反过来，仪器今天很准、猜的那个人一直在抖动，你就要让测量的权重大一点。卡尔曼滤波里的协方差矩阵，干的就是这个“判断谁更可信”的活。

这也是它比互补滤波、加权平均高明的地方。加权平均只能给固定权重，没办法根据信号质量实时变化；卡尔曼滤波通过协方差传播，能把“我有多相信这个估计”这件事随时算出来，并且直接用协方差计算出最优权重矩阵——也就是卡尔曼增益。搞懂这一点，后面的所有公式都只是在实现这个思路。

2.2 组合导航中最常见的15维状态模型

组合导航里最经典的模型不是只在二维平面里做位置融合，而是把一个完整的三维运动状态作为估计对象。用得最多的状态向量是15维：

有人会加入更多维度，比如陀螺和加速度计的标度因数、安装失准角、GNSS接收机时钟偏差，以及杆臂误差等，状态量可以扩到21维甚至更高。但初学者从15维起步最合适：维度少，可观性好，参数容易调，也足以支撑松耦合和大部分紧耦合应用。

状态方程的来源就是惯性导航的机械编排。把IMU输出的比力和角速度作为输入，用姿态矩阵把加速度折算到导航坐标系，再积分得到速度和位置。陀螺零偏和加速度计零偏在模型里通常建模成随机游走，也就是它们的值会缓慢漂移，需要用GNSS观测不断修正。状态方程的矩阵形式可以写成：

x_k = F * x_{k-1} + G * u_k + w

其中x是15维状态，F是状态转移矩阵，u是IMU输入，w是过程噪声。这一步做的是“我根据上一时刻的状态和IMU读数，猜一下现在大概在哪里”。

2.3 核心公式就这么几步，但每一步都有物理含义

卡尔曼滤波的标准五步是预测两步、更新三步，全部列出来也就几行：

预测： x̂_k = F * x̂_{k-1} + B * u P_k = F * P_{k-1} * Fᵀ + Q 更新： K = P_k * Hᵀ * (H * P_k * Hᵀ + R)⁻¹ x̂_k = x̂_k + K * (z - H * x̂_k) P_k = (I - K * H) * P_k

逐个说它们的物理含义。P是状态协方差矩阵，对角线元素可以粗略地理解为“当前每个状态估计的不确定程度”；Q是过程噪声矩阵，表示你对惯性导航模型本身的信任程度，Q越大说明你认为模型误差越大；R是观测噪声矩阵，表示你对GNSS等外部观测的信任程度，R越大说明你认为测量越不可靠。

K是卡尔曼增益，它是P、H、R共同作用的结果。当R相对小、P相对大的时候，K会变大，滤波器更偏信观测；反过来K变小，滤波器更偏信模型预测。z减H乘以x的那个括号，数学上叫新息，是“实际观测和预测观测之间的差别”，也就是预测和测量不一致的地方在哪里。

代入组合导航的场景看：高架桥下卫星信号飘了十几米，此时R应该被放大，K就会降下来，滤波依然信任惯导的推算，卫星观测对位置的影响变小。开阔处卫星信号稳，R小，K相应变大，惯导的漂移就被实时拽回来。这套机制不需要人为写一堆复杂的判断逻辑，它自己就能根据不确定性去平衡。

3. 松、紧、深三种耦合架构怎么选

3.1 松耦合：最适合作为第一套方案

松耦合的架构是“先解算，后融合”。GNSS接收机内部自己完成卫星信号的解算，输出一组位置和速度；导航滤波器拿到这组位置速度作为观测值，和INS的机械编排结果做卡尔曼滤波融合。所有导航功能都在外部处理器上实现，接收机只负责把结果递出来。

优点是模块化程度非常高。换一个GNSS接收机、换成RTK、换成双天线测向，对融合算法的影响很小，只需要调整R矩阵里的噪声参数。调试也直观，每个环节的错误都能单独定位：先看GNSS单独输出的位置好不好，再看INS单独积分的漂移大不大，最后才看滤波融合出的结果。

缺点是它把GNSS接收机内部解算出来的结果当成了“不可再分”的最终观测。一旦可见卫星少于4颗，接收机就出不了定位结果，此时松耦合系统拿不到任何更新，只能靠INS自行推算。哪怕此时还有3颗星，理论上还能给滤波提供有用的伪距约束，松耦合也利用不上。

如果是刚起步的项目，建议直接采用松耦合。先把卡尔曼滤波的预测更新、坐标系转换、时间同步跑通，有一个能稳定工作的基础组合导航系统，再考虑要不要升级到紧耦合。这一步走稳了，后面换架构只是改观测模型的问题。

3.2 紧耦合：少一颗星也能用的关键在于改“原料”

紧耦合不再使用接收机解算出的位置速度，而是直接用接收机输出的伪距、伪距率（多普勒观测）作为观测值。接收机对每一颗可见卫星单独输出一个观测，导航滤波器自己统一处理这些原始测距信息。

这个架构最大的价值体现在卫星部分遮挡的场景。正常定位要求至少4颗星才能解算出完整的三维位置和时钟偏差；但在紧耦合结构里，哪怕只有1～3颗星，只要把这些原始伪距放进观测方程，滤波器照样可以结合INS的状态预测，获得一部分位置约束。结果就是，信号环境差的时候，紧耦合比松耦合更能“扛”。

代价也很实在。第一，接收机必须支持输出RTCM或厂商私有格式的原始观测数据，选型受限。第二，伪距观测方程里必须额外建立接收机时钟偏差模型，把钟差当成一个状态量或者用已知参数消掉，这一步处理不好会直接破坏滤波器的精度。第三，可见卫星数量在复杂环境里变化很快，滤波器的观测维度会动态变化，代码实现和异常处理都更复杂。

用一句话概括：松耦合换的是成熟与简洁，紧耦合换的是在恶劣环境下多出来的可用信息。

3.3 深耦合：硬件级的最终方案，别轻易尝试

深耦合是INS信息和GNSS接收机内部跟踪环路之间的深度融合。导航滤波器的输出会直接反馈到接收机的载波环路、码环路，辅助接收机在弱信号下保持跟踪；接收机内部更早的积分观测又被送回导航解算。这种结构在高动态飞行器、强干扰环境里有优势，信号动态应力和抗欺骗能力都强。

但它几乎不是一个软件层面能完全解决的问题，通常需要专用的紧耦合板卡或一体化的导航芯片支持，工程复杂度、硬件成本和调试难度都高出一个量级。对于绝大多数创业项目、科研原型、低速无人车或者无人机应用，深耦合都不是必要的。我更建议把精力放在松耦合到紧耦合的过渡，以及后面要讲的滤波细节优化上。

4. 滤波器能跑起来只是开始，这些工程细节决定精度

4.1 时间同步：GNSS数据到你手里时已经“过期”了

这是新手最容易忽略、却对精度影响极大的问题。GNSS接收机从接收射频信号到做完解算，再把结果通过串口发出来，中间存在几十到几百毫秒的延迟。你在应用层收到的那组位置，实际上是“过去某个时刻”的位置，而不是当前时刻的位置。

如果车辆正好在高速运动，把这个过期位置当作当前观测直接和INS融合，会产生与速度乘以延迟量相当的系统误差。车速18km/h、延迟200ms时，位置就有1m的偏差；车速72km/h时这个偏差到4m，足以让融合结果看起来“拖尾”。

工程上的处理办法有几类。最常见的是先把INS机械编排的时间基准对齐到GNSS时间戳，在卡尔曼预测阶段把状态预测到GNSS数据的有效时刻，再执行更新。另一种做法是利用GNSS的PPS秒脉冲硬件对齐时间，配合时间戳做精确打点。对于已有GNSS原始观测但延迟不固定的情况，可以把延迟量本身扩到状态向量里实时估计，这属于进阶方案，但很值得做。我早期做项目时曾偷懒没处理延迟，结果在转弯路段融合轨迹明显比实际晚半拍，查了快两天才发现是这个问题。

4.2 坐标系和姿态初值：融合前先把“坐标系语言”统一

GNSS解算得到的位置通常在ECEF地心地固坐标系里，而惯导的机械编排一般在地理坐标系NED或ENU里进行。滤波之前必须做一步坐标转换，否则两个传感器说的根本不是一个坐标系里的位置，融合结果就是乱的。

坐标转换本身不难：用经纬度和高程把ECEF转成当地的NED或ENU，关键要保证一致。初值更麻烦。滤波器的初始位置可以由GNSS第一帧提供，这个简单；但初始姿态如果给错了，系统会以这个错误姿态为基础不断积分，位置误差前几分钟就会出现明显发散。

静止初始化时，可以用加速度计测量重力方向来估计横滚和俯仰：重力方向是竖直的，加速度计三轴读数里的分量分配就能反推角度。航向角在静止时没法用加速度计确定，所以要么用磁力计（但要注意磁场畸变），要么在运动后借助GNSS速度方向来确定水平航向，更稳妥的是使用双天线GNSS测向系统直接给定航向初值。多花几分钟做好初始化，比一切参数调好之后再来排查“为什么起飞头几十秒就歪了”便宜得多。

4.3 野值和多径：学会对异常观测说不

GNSS在城市环境下的观测值里经常混入异常。这种异常不像高斯噪声那样均匀分布，而是表现为突然跳到几米外、或者持续一段时间的偏移。如果滤波器把这类异常当作正常观测去更新，结果就是轨迹突然抽风。应对的关键不是调小R，而是主动检测并拒绝异常观测。

最常用的方法是基于新息的卡方检验。新息就是实际观测和预测观测之差，理论上服从均值为零的高斯分布，其协方差可以推导为S = H * P * Hᵀ + R。通过计算新息马氏距离d = z⁻¹ * S⁻¹ * d，再和卡方分布的分位数比较。对三维位置观测，自由度是3，99%置信水平的阈值大约是11.34；六维观测则取约16.81。超过阈值的观测直接判定为异常，用一个极大的R值替代正常R，相当于让这次观测在更新中失去作用。

不过阈值不能设得太死。设得太严，正常观测也会被频繁拒绝，导致滤波器“饿死”；设得太松，异常观测又会混进来。我的习惯是在实际路测中统计新息分布，把阈值设为理论值和实测值的折中。城市峡谷里宁可保守一点，错的数据比没有数据更伤。

4.4 Q和R的调参经验：看着轨迹调参数是不行的

滤波器能不能收敛、能不能稳住，很大程度上取决于Q和R怎么取。但这组参数没有标准答案，只能根据实际传感器特性和运行环境摸索。一个好的出发点是：Q尽量依据IMU器件的Allan方差等指标来设，R依据GNSS在开阔天空下的实测定位精度来设，比如水平1m、垂直2m。

调参的正确顺序应该是先固定R，只调Q。观察GNSS可用时的整体位置误差和速度误差，观察GNSS失锁后系统位置漂移速率。如果滤波器在失锁后很快漂走，说明Q偏小，模型过于自信，你对惯导的信任放得太多；如果轨迹被GNSS噪声拖得毛糙、高架桥下还跟着飘，说明R偏小，滤波器对观测的信任过度。

比看轨迹更有说服力的是看新息序列。正常工作的新息应该是均值接近零、方差与S的预测值相近的白噪声。如果新息明显存在趋势性，说明状态模型有系统误差，比如加速度计零偏没建模好；如果新息均值不为零但方差正常，大概率是观测有固定偏差，比如杆臂或者时间同步没对齐。通过新息序列判断滤波健康状态，比“看着轨迹还不错”可靠得多，能帮你把问题定位到具体环节。

5. 从“能跑”到“好用”：卡尔曼滤波的优化应用方向

5.1 自适应滤波：让滤波器自己学会调整噪声水平

固定Q和R的组合导航在“信号环境恒定”的场景里没问题，但真实环境往往是开阔地和城市峡谷交替出现。开阔地里GNSS精度高，R应该小；峡谷里多径严重，R应该大。用固定R的滤波器，要么在开阔地平滑不到位，要么在峡谷里被异常观测带偏。

自适应滤波的思路是让滤波器根据新息的现时统计情况，动态修正R或Q。常见做法是维护一个滑动窗口，统计一段时间内新息的实际协方差，再和理论协方差S比较。如果实际协方差明显大于理论值，说明观测噪声被低估了，就适当放大R；如果实际值接近理论值，保持现有R不动。

还可以配合渐消因子，在检测到模型异常时主动放大P或者Q，让滤波器重新“醒过来”，恢复对观测的跟踪能力。自适应方法容易在强多径时被异常观测牵着走，所以必须有保护机制。我通常给R设置一个下限，通常是标称观测噪声的0.1倍左右，防止滤波器自信过度、完全不跟着GNSS走了。

5.2 多传感器接入：里程计、气压计、视觉和激光怎么融进来

卡尔曼滤波这个框架的好处是，多一个传感器只是多一组观测方程，无需撼动整体架构。实际项目里，公里计的引入价值最大。轮式或履带式设备安装里程计后，轮速乘以标度因子即为前进方向的速度观测；还可以加转弯约束，比如低速无人车通常假设后轴侧向速度接近零。这类非完整约束在地下停车场、隧道这些GNSS完全失效的场合，可以把有效定位时间从几十秒拉长到几分钟，非常实用。

气压计对应的是垂直通道。GNSS垂直观测噪声一般比水平差2~3倍，惯导的垂直通道又天然不稳，所以纯GNSS/INS系统的高度波动比较大。气压计的短时差分可以当作高度变化率的观测，比直接用绝对高度更稳，因为气压计容易受天气和温度影响产生零点漂移，但短时差分能把这个漂移大部分消掉。

视觉或者激光接入时，本质上分两类：如果提供的是结构化地图匹配的绝对定位，那和GNSS松耦合的高度观测没有区别，直接作为位置观测；如果提供的是视觉里程计那样的相对位姿，则需要把它作为增量式约束加入，通常会在状态模型里增加传感器相对主IMU的外参参数，同时考虑延迟对齐。这一步经常成为最终精度瓶颈，因为外参标定误差会导致两套传感器在大动态转弯或颠簸时互相矛盾，前端看起来都“挺准”，融合结果却在抖动。

5.3 “三维立体”到底怎么落地：垂直通道和高程稳定

标题里强调“三维立体组合导航”，很多人会以为只是把z坐标加进来算均值。实际上，三维空间里的核心难点在垂直通道。卫星测量高程的能力受卫星几何限制，GDOP恶化时垂直误差往往远大于水平误差；惯性导航的垂直通道则更加糟糕——高度误差会通过重力补偿误差反馈进去，形成不稳定的正反馈回路。

处理办法是层次化接入垂直辅助。第一层是GNSS的高程观测，基础框架必须有；第二层是气压计的高度变化率辅助，把绝对高度的缓慢漂移约束住；第三层再进阶一点，如果载体在地面行驶，可以引入路面约束，把高度锁定在一个平滑变化的通道附近。对飞行器来说，气压计加GNSS的组合几乎是标准配置；对无人车来说，路面约束或者轮式里程计的垂向约束有很大帮助。

这块做好的直观收益是：坡道检测更加平滑、上立交桥和地面路的切换不抖动、悬停无人机不会莫名其妙往下掉高度。三维立体导航的价值，正是在这些不太显眼但实际每天都在发生的工况里体现出来的。

5.4 性能评估建议：别只看轨迹贴不贴地图

跑通以后最容易犯的错，是觉得“轨迹贴在地图上、没有明显飞点”就是系统做好了。实际上，定性地看看不出系统误差、延迟和协方差是否真实可信。我比较习惯的一套评估流程是这样的：先在开阔处沿直线往返几个来回，用RTK或者高精度后处理参考轨迹做基准；再进入高架桥下、树荫、立交桥等遮挡环境，多跑几趟，记录GNSS原始观测。

统计指标上，至少要算水平位置误差RMS、95分位点误差，垂直方向单独统计；速度误差和姿态误差也要抽样统计，尤其要关注转弯和加减速阶段。还要专门测试GNSS失锁后的表现：选定一个区域，人为切断GNSS观测30秒、60秒、120秒，看混合导航的位置相对真实轨迹的漂移量，这个数值直接反映惯导质量和滤波模型是否靠谱。

在调试工具上，务必保留原始GNSS观测数据、IMU原始数据和IMU的数据，用统一时间戳回放，对排查问题非常关键。做调参对比时，把同一段数据的滤波结果拉出来对比，分左右屏观察位置误差曲线和新息序列变化，比单纯看地图直观得多。

6. 几个我很少在文档里写的小经验

做这套系统几年，踩过不少坑，有几个细节很少在技术文档里写明，但对实际效果影响很大。

一是滤波器的鲁棒性往往比峰值精度更值钱。很多团队花大力气把开阔环境下的误差从10cm压到5cm，结果一到城市峡谷就散架。工程上一个可以接受的目标不是“每种环境下都最优”，而是“95%的时间误差小于1米，任何环境下都不会飞掉”。把异常观测保护、时间同步、姿态初值做好，优先保障底线，再去追求峰值。

二是参数不是越多越好。状态扩维确实能提高理论精度，但维度越高，可观测性问题和调参难度也越大。很多场景里，15维状态、松耦合、配合良好的野值处理，已经能交出很漂亮的答卷。真正让系统变好用的是对时间同步和坐标系的严谨，不是堆算法。

三是别忽略仿真环境的对照。我在真实跑路之前，习惯用一段录制的原始IMU和GNSS数据做离线回放，把每次修改参数后的结果用同一组数据对比。这样既能快速回归测试，也能避免在真车上反复试错消耗大量时间。把一段“问题数据”留作回归用例，排查新问题时会非常安心。

组合导航是一个把多个不完美信号拧成一条可信轨迹的工程，卡尔曼滤波给了框架，剩下的功夫都在理解和处理各种误差源上。希望这篇总结能帮你少走几个弯路，尤其是那些我当年调了很久才发现的问题。