简介：《GNSS与惯性及多传感器组合导航系统原理》电子版资源，面向导航工程、自动驾驶及无人机领域的研发人员与高校学生，系统讲解GNSS与惯性导航（INS）的组合原理及多传感器融合方法。书中以卡尔曼滤波为核心，阐述如何利用GNSS覆盖广、精度高的优势弥补INS长时间运行积累误差的不足，并扩展至地形匹配、航迹推算、磁力计与激光雷达等融合场景，内容涵盖航空、航海、陆地车辆与无人机等主流应用。压缩包大小约13.15MB，便于离线学习。已有1700人学习浏览。配合书中MATLAB代码实例，读者可亲手实践滤波估计与组合导航系统的设计实现，理解不同传感器数据间的不确定性处理与状态更新优化，从理论到工程落地获得完整训练，是掌握鲁棒导航技术的实用参考资料。
1. GNSS与惯性及多传感器组合导航：为什么单一传感器撑不起自动驾驶和无人机
做组合导航的人都知道一句近似玄学的判断：惯性导航短期稳、长期飘，GNSS长期准、短期断。车载无人机在隧道里丢掉卫星信号，纯惯导一分钟就能飘出几十米；反过来，车辆经过高架桥下方时GNSS受多路径影响出现几米跳变，纯卫星定位又会把原本平滑的轨迹撕出锯齿。GNSS与惯性及多传感器组合导航系统，就是把这几种传感器放在同一个状态估计框架里，用各自的优势去压对方的毛病。这套原理不只是写进教科书里的公式堆叠，更是自动驾驶、无人机巡检、测绘级定位真正跑起来的地基。这篇笔记不打算铺开所有推导，而是从「原理怎么立住」讲到「最小代码怎么跑通」，再把时间同步、标定和失效切换这些工程坑一个一个摊开。适合正在做定位方案选型、或者已经拿到IMU和GNSS数据但融合结果发散的工程师。
2. 组合导航系统的核心原理：状态方程、观测方程与可观测性
2.1 从松组合到紧组合：你先要分清这三代架构
组合导航的架构演进，本质上是「GNSS的信息在哪个层级进入滤波器」的问题。松组合最简单：GNSS先单独解算出位置和速度，再把这两个结果当作观测值输入给导航滤波器。紧组合则跳过GNSS接收机的位置解算，直接拿伪距、伪距率这些原始观测参与滤波。深组合通常指接收机内部跟踪环路与导航滤波器闭环，普通应用很少碰。
我一般这样选型：只做车载级辅助定位，松组合足够，因为接收机内部的RTK和PPP解算已经能输出厘米级位置，滤波器的任务是平滑和桥接。但如果你在写无人机或者机器人的紧耦合里程计，松组合的缺点是GNSS丢星后整个滤波器只剩惯导外推，没有任何伪距信息可用，恢复也比较慢。紧组合虽然复杂，但在部分遮挡场景能多撑几十秒，原因是伪距观测比位置观测稀疏得多，一个可见卫星就能提供约束。
有意思的是，很多人把「多传感器融合」等同于「多传感器加权平均」，这是翻车的起点。真正组合导航的核心是一个递归贝叶斯估计问题：维护一个状态向量，用IMU做高频预测，用GNSS、视觉等做低频修正。架构选型决定了你是在状态层面融合还是观测层面融合，这直接影响后面整个代码结构和参数调法。
2.2 状态向量怎么选：姿态、速度、位置、零偏的建模
状态向量的选择是组合导航设计的第一个分水岭。最常见的全状态向量是15维：3维姿态（横滚、俯仰、航向）、3维速度（导航系下）、3维位置（经纬度或局部ENU）、3维陀螺零偏、3维加速度计零偏。实际工程里，还有一个常用变体是把姿态用四元数表示，于是状态向量变成16维，其中四元数4维。
选15维的好处是跟教科书一一对应，协方差矩阵是15×15，调试时能直接看出某个状态的收敛情况。选16维则要处理四元数的模长约束，滤波器输出的四元数每次更新后必须重新归一化，否则误差会累积。我自己的习惯是内部用误差状态表示，也就是ESKF，这样状态向量是误差量，线性化更干净，后面再单独讲。
零偏建模值得多说一句。IMU零偏不是常数，每次上电都不一样，且随温度漂移。滤波器里如果不把零偏纳入状态估计，IMU的积分误差会以二次方增长，GNSS修正后又会迅速归零，输出轨迹出现锯齿。所以状态向量里必须给零偏留位置，并且用随机游走过程描述它的变化。这个过程噪声的大小，直接影响滤波器对零偏的跟踪速度。
2.3 可观测性：为什么静止状态下航向零偏不可观
可观测性是组合导航里最容易被忽视的理论问题。通俗讲，就是「给定一系列观测值，状态向量里的每一项能不能被唯一确定」。一个经典结论是：IMU静止放置时，俯仰和横滚零偏可观，因为重力方向提供了约束；但航向零偏完全不可观，因为没有任何外部信息能区分「陀螺零偏」和「真实航向」。
这个结论的实际影响很大。很多人在静止状态下采集数据做初始对准，发现航向角乱跳，以为是算法写错了。其实不是，是系统本身在这个场景下信息不足。要让航向可观测，必须有外部航向参考（磁力计、双天线GNSS、视觉特征方向），或者让IMU有加速度激励（车辆起步、转弯）。理解了这一点，你就能解释为什么组合导航在车辆静止时偶尔会缓慢旋转，那不是滤波器发疯，而是可观测性矩阵在告诉你「当前数据推不出航向」。
实际工程里，我会用一段「直线加速加转弯」的激励轨迹来初始化航向，比单纯静止采集有效得多。GNSS速度在直线段能约束航向，转弯段能把陀螺零偏和航向解耦，这是从可观测性分析直接推到操作层面的结论。
2.4 多传感器扩展：视觉、里程计、气压计的约束作用
组合导航框架本身是开放的，GNSS和IMU只是最低配。把视觉里程计、轮速里程计、气压计、磁力计、甚至UWB加进来，都是在同一个状态方程上增加新的观测方程。视觉里程计提供的是相对位姿约束（帧间运动），轮速里程计提供的是纵向速度及不可滑移约束，气压计提供的是高度观测，它们的本质都是「辅助修正状态量的某个子空间」。
我常遇到的问题是视觉里程计和GNSS的位置单位不一致、坐标系不一致、甚至更新频率不一致却直接塞进同一个滤波器。正确的做法是给每个传感器独立建模观测噪声，并在滤波器里维护对应的外参。视觉观测的是body frame到相机的位姿变换，GNSS观测的是导航系下的位置，二者中间的变换链条必须完整，否则融合结果比单传感器更差。
另一个容易踩的坑是气压计。气压计对短时气压波动很敏感，过隧道、上下坡、空调风都会影响。我会把气压计当作高度约束的候选，但会用滑动窗口的中位数去判断突跳，而不是直接用原始值。多传感器组合的核心不是「传感器越多越好」，而是「每类传感器的约束子空间要清晰，且噪声模型要准确」—如果两个传感器都在约束同一个状态，其中噪声大的那个会拖累整体。
3. 用卡尔曼滤波把GNSS和IMU融合起来：最小可运行实现
3.1 误差状态卡尔曼滤波(ESKF)为什么比全状态滤波更稳
直接对全状态向量做扩展卡尔曼滤波，理论可行，但工程上问题很多。最典型的：姿态用欧拉角会遭遇万向锁，用四元数要处理模长约束，而且状态量的数值范围差异巨大（位置是米级别，零偏是微小量级），协方差矩阵容易变得病态。误差状态卡尔曼滤波（ESKF）避开了这些麻烦：它不直接估计状态，而是估计「真实状态与标称状态的误差」，标称状态用IMU积分随时更新，误差状态用卡尔曼滤波修正，最后把误差叠加回标称状态。
ESKF的另一个好处是误差状态的动力学线性化更精确。全状态EKF里的旋转误差要用雅可比矩阵近似，而ESKF里旋转误差本身是小角度，可以近似为三维向量，避开了四元数的非线性约束。我见过不少项目从全状态切换到ESKF后，协方差不再发散，航向估计也稳了，这就是所谓的「同一套数据，换个状态表示就能救回来」。
ESKF的操作流程可以概括为：每次IMU数据到来时，先用标称状态做机械编排（积分），同时推进误差状态的协方差；每当GNSS或视觉观测到来时，用误差状态卡尔曼更新计算最佳误差估计，然后把误差注入标称状态，最后把误差状态清零，进入下一轮。
3.2 核心公式与代码骨架：预测、更新、反馈
下面给出一个最小ESKF预测和更新骨架，用Python写，只包含最关键的部分。这个骨架省略了地球自转和科里奥利力等高阶项，适合车辆和地面机器人，飞行器需要补上这些项。
import numpy as np # 状态向量: [姿态误差(3), 速度误差(3), 位置误差(3), 陀螺零偏误差(3), 加计零偏误差(3)] # 标称状态: p(位置), v(速度), q(四元数), bg(陀螺零偏), ba(加计零偏) def predict(eskf, imu, dt): # imu: [gyro(3), accel(3)] g = np.array([0, 0, -9.81]) R = quat_to_rot(eskf.q) # 旋转矩阵 # 标称状态积分（机械编排） eskf.v += (R @ imu.accel + g) * dt eskf.p += eskf.v * dt + 0.5 * (R @ imu.accel + g) * dt**2 omega = imu.gyro * dt eskf.q = quat_multiply(eskf.q, quat_from_rotation(omega)) eskf.q *= 1 / np.linalg.norm(eskf.q) # 误差状态协方差预测 F = np.eye(15) F[0:3, 3:6] = -skew(R @ imu.accel) * dt # 加速度计对姿态误差的影响 F[0:3, 6:9] = -R * dt # 陀螺零偏误差对姿态的影响 F[3:6, 6:9] = -skew(g) * dt # 姿态误差对速度误差的影响 F[3:6, 9:12] = -R * dt # 加计零偏误差对速度的影响 F[6:9, 3:6] = np.eye(3) * dt # 速度误差对位置的影响 Q = np.zeros((15, 15)) Q[0:3, 0:3] = np.eye(3) * noise_gyro_sigma**2 * dt Q[3:6, 3:6] = np.eye(3) * noise_accel_sigma**2 * dt Q[9:12, 9:12] = np.eye(3) * noise_bg_sigma**2 * dt Q[12:15, 12:15] = np.eye(3) * noise_ba_sigma**2 * dt eskf.P = F @ eskf.P @ F.T + Q return eskf def update_gnss(eskf, gnss_pos, gnss_pos_cov): # 观测矩阵：只观测位置误差 H = np.zeros((3, 15)) H[0:3, 6:9] = np.eye(3) z = gnss_pos - eskf.p # 测量残差 S = H @ eskf.P @ H.T + gnss_pos_cov K = eskf.P @ H.T @ np.linalg.inv(S) dx = K @ z # 误差注入标称状态 eskf.p += dx[6:9] eskf.v += dx[3:6] eskf.q = quat_multiply(quat_from_rotation(dx[0:3]), eskf.q) eskf.q *= 1 / np.linalg.norm(eskf.q) eskf.bg += dx[9:12] eskf.ba += dx[12:15] # 误差状态清零，协方差更新 eskf.P = (np.eye(15) - K @ H) @ eskf.P dx[:] = 0 return eskf这段代码里的逻辑说明：预测部分的关键是误差状态转移矩阵F的构造。F[0:3, 3:6]表示加速度计的实测值对姿态误差的耦合，因为加速度计测量的力包含重力分量，旋转误差会改变比力的方向，进而影响速度积分。观测更新部分只用了 GNSS 位置，H里只有位置误差对应的列是单位阵。残差z由 GNSS 位置减去滤波器标称位置得到，经卡尔曼增益修正后，误差状态被注入标称状态。
参数说明：noise_gyro_sigma、noise_accel_sigma是 IMU 的白噪声密度，通常在 IMU 手册里给出，单位是 rad/s/√Hz 和 m/s²/√Hz，使用时需要按 IMU 采样频率换算成单步噪声标准差。noise_bg_sigma和noise_ba_sigma是零偏随机游走噪声，一般从 Allan 方差曲线读取，如果你懒得测，可以先给一个比较小的值（如1e-5），然后观察滤波器对静止零偏的跟踪效果。GNSS 协方差gnss_pos_cov在 RTK 固定解时可以给[0.1, 0.1, 0.2]，单点定位时给[3, 3, 5]，给大了融合轨迹会偏向 IMU 积分，给小了 GNSS 噪声会被当真实信号吃进去。
3.3 关键参数：过程噪声、量测噪声的初值与调参方向
组合导航调参的本质是在「信任 IMU」和「信任外部观测」之间找平衡。过程噪声越大，滤波器认为 IMU 模型越不可靠，会更依赖观测；量测噪声越大，滤波器认为观测越脏，会更依赖 IMU 积分。很多人一上来就调量测噪声，其实应该先确认 IMU 的噪声模型是否准确。
一个实用的调参顺序是：先把 IMU 白噪声和零偏随机游走设成数据手册里的标称值，然后静止测试，看滤波器输出的速度和位置是否在小范围内振荡。如果振荡幅度超过预期，说明过程噪声设小了，IMU 积分的漂移没有被零偏状态吸收。再跑一段存在 GNSS 但车辆过减速带的路段，观察位置误差的尖峰幅度，如果尖峰过大，说明量测噪声小了，GNSS 的异常跳变被当成了真实位移。
我在调参时还会观察一个新息序列（innovation），也就是残差z的统计特性。如果新息均值不为零且持续偏向一侧，说明状态模型中存在未建模偏差，比如杆臂效应或外参标定错误，这时候调噪声无用，得先查模型。如果新息标准差持续大于理论值sqrt(S)，说明过程噪声偏大或量测噪声偏小，按比例调整即可。这比盲调参数要快得多。
3.4 跑通一个最小仿真：IMU数据生成与GNSS观测注入
没有真机数据时，仿真能帮你验证滤波器的逻辑是否完整。常见做法是设计一条二维正弦轨迹，反推理想比力和角速度，再叠加高斯噪声和零偏漂移生成 IMU 数据，同时每隔一帧加入带噪声的 GNSS 位置。这样跑一遍，理想情况下融合轨迹应该比纯惯导积分结果更贴近真实轨迹。
生成 IMU 观测时要注意一个坑：IMU 测量的是比力（specific force），也就是加速度计感受到的「非引力加速度」，等于载体加速度减去重力。如果你的仿真是从轨迹的加速度直接生成accel = a + g，方向别搞反了。角速度可以直接从姿态角变化率计算，也可以用四元数导数。零偏漂移用一个小的随机游走过程模拟，这样才能测试滤波器对零偏的估计能力。
仿真跑通后，我会故意做几个破坏性测试：把 GNSS 观测整体偏移 3 米，看滤波器是否能逐渐纠偏；把 GNSS 更新频率从 10Hz 降到 1Hz，看位置误差是否还在可接受范围；在仿真中途让 GNSS 完全缺失 10 秒，看纯惯导外推的误差曲线。这些测试能让你在真机踩坑之前，先把系统的能力边界摸清楚。
4. 多传感器组合导航的工程落地：时间同步、标定与坐标系
4.1 时间同步：GNSS的PPS和IMU的采样时刻为什么对不上
组合导航里最容易让所有算法白搭的问题就是时间同步，没有之一。GNSS 接收机通常每秒输出一个 PPS 脉冲，接收机的定位解算时刻与 PPS 对齐；IMU 则有自己的内部采样时钟，可能由独立晶振驱动，漂移率通常在几十到几百 ppm 量级。如果两者不对齐，最直接的后果是 GNSS 观测和 IMU 积分对应的时间点不一致，车辆在急转弯时位置误差能瞬间放大。
工程上最常见的做法是：把 GNSS 的 PPS 作为时间基准，IMU 每次采样时记录本地时间戳，同时利用 PPS 修正本地时钟的漂移。具体做法是在 PPS 上升沿触发一个中断，读取当前计数器的值，与理想 PPS 间隔（1秒）做差，得到本地时钟的偏移和漂移率，然后用一个锁相环或者线性估计去校正。对 IMU 的时间戳校正，我会维护一个简单的线性映射：真实时间 = 本地时间 * scale + bias，其中 scale 每 10 秒用 PPS 差值更新一次。
另一种互补做法是软同步：把 IMU 数据按时间戳插值到 GNSS 观测时刻。比如 GNSS 观测发生在 t_k，IMU 最新样本在 t_k-1 和 t_k 之间，则用两个样本做线性插值得到 t_k 时刻的 IMU 姿态和速度。但插值的前提是时间戳已经精确对齐，否则插值位置就是错的。我建议先把硬同步做好，再考虑插值，不要反过来。
4.2 外参标定：IMU与相机之间旋转和平移求取的坑
多传感器组合的第二个大坑是外参标定。IMU 的 body frame 和相机坐标系之间的旋转和平移，如果标定不准，视觉特征的重投影误差会直接转换成滤波器的虚假观测。常见做法是使用标定板。标定板方法只能得到相机与标定板的位姿，以及 IMU 与标定板的相对运动关系，通过优化求解得到 IMU 与相机的相对位姿，原理是利用一组多视图几何约束加上 IMU 积分轨迹的刚性变换一致性。
实际操作中很常见的一个坑是：标定时 IMU 和相机之间的时间延迟没有被建模。所以我会先做粗时间同步（比如手动调整几个毫秒），再联合标定时间延迟和外参，否则旋转矩阵能解出，但平移向量往往残差很大。另一个坑是标定过程中激励不足，如果只绕一个轴转，平移向量在另外两个方向不可观。正确的做法是画「8」字轨迹，三个轴都有旋转和平移激励。
我处理外参问题时的习惯是：先单独标定 IMU 与 GNSS 天线之间的杆臂，再标定 IMU 与相机的相对位姿。杆臂是 GNSS 天线相位中心相对 IMU body frame 的平移，直接从机械图纸读可能不准（因为天线相位中心不一定在几何中心），最好用一段完整的直线和转弯数据去辨识。用滤波器辨识时可以把杆臂加入状态向量或者用最小二乘法离线估计。
4.3 坐标系链：从IMU body frame到导航系到地心地固系
组合导航的坐标系链条比想象中要长。IMU 在 body frame 上采集角速度和比力，得到的导航结果在导航坐标系（NED或ENU），GNSS 输出位置通常在地心地固系（ECEF）或经纬高（LLH），视觉特征的观测在相机坐标系。这条链条上每多一次转换，就多一次因坐标定义不一致而导致的误差。
我一般遵守的规则是：滤波器内部统一用 ENU 局部坐标系作为导航系，GNSS 输出的 ECEF 坐标先转换到 ENU，再进入滤波器。在 ECEF 与 ENU 之间转换时，要注意参考原点必须固定为里程起点，而不是每次用最新的 GNSS 位置作为原点，否则轨迹会不断被平移。另一个常见错误是把陀螺的角速度用在 ENU 坐标系里，但 ENU 本身随着地球自转在旋转，严格说需要加入地球自转项，这是很多地面机器人实现里直接忽略而不会暴露问题的原因—低速场景下影响极小，但飞行器高速飞行时误差会累积。
IMU 的加速度计测量的是 body frame 的比力，转换到导航系时需要乘以从 body 到导航系的旋转矩阵 R。这个 R 是由姿态更新维护的，而姿态更新用到的是陀螺的角速度。如果陀螺没有做零偏校正，R 会缓慢漂移，进而导致加速度积分方向错误。所以坐标系链的根本问题其实是零偏估计，一旦滤波器的零偏状态收敛了，坐标系链上的误差主要就来自外参和杆臂。
4.4 传感器失效切换：GNSS丢星、视觉特征丢失时的降级策略
多传感器组合不只是融合，更重要的是会降级。GNSS 在隧道、高楼峡谷、地下车库必然失效，视觉在弱纹理、强光、快速旋转时也可能失效。如果你把失效传感器的观测继续送进滤波器，残差会异常放大，滤波器协方差迅速收缩，甚至把本来是正常状态的轨迹拉偏。
我推荐的分层降级策略是这样做的：第一层，每个传感器的观测都带特征质量标记，GNSS 用解状态（固定解、浮点解、单点解），视觉用特征点数量和平均视差。第二层，当 GNSS 降级时，自动切换到视觉里程计或轮速里程计作为位置更新源，同时增大量测噪声；当所有外部位置更新都失效时，关闭位置观测，只保留零偏观测，进入纯惯导模式。第三层是恢复策略：GNSS 重新固定后，不要立刻按原始权重更新，而是先用一个较大的量测噪声让滤波器逐渐收敛，避免跳变。
实现降级策略的关键在代码结构上：不要把所有传感器都塞进同一个 update 函数，而是一个传感器对应一个独立的 update 函数，在接收数据前先检查质量标记。质量标记的阈值是经验值，比如 GNSS 的 PDOP 大于 5 时降权，视觉特征点少于 30 个时降级，这类参数没有绝对标准，需要根据你的车型和场景标定。还有一点容易忽略：降级切换时滤波器的协方差要乘一个膨胀因子（比如 1.5），否则系统会很自信，后续观测修正变得迟钝。
5. 组合导航避坑指南：从数据采集到真机跑飞的5条踩坑记录
5.1 现象：IMU静态零偏标定结果每次开机都不一样
某天你在实验室把 IMU 静止放在桌面上，采集十分钟数据取平均，算出来的陀螺零偏是-0.003 rad/s，第二天同一块 IMU 采出来却是-0.001，以为是传感器坏了。原因拆解：IMU 零偏受温度、上电应力、传感器老化三重影响，每开机一次零偏就换一个值，这是正常物理现象，不是故障。解决办法是不要试图离线标定一次用一辈子，应该把零偏作为状态量放进滤波器，实时在线估计。离线标定只能用来校零偏初始值和 Allan 方差参数，不能当作固定值补偿。
5.2 现象：GNSS信号正常但组合导航位置发散
你的测试路段开阔，GNSS 显示 RTK 固定解，但组合导航的轨迹在直道上越走越歪，甚至是慢慢旋转出去的。原因排查：大概率不是 GNSS 的问题，而是 IMU 的零偏漂流与 GNSS 更新之间出现了时间不同步，或者状态方程里漏了杆臂。另一个高概率原因是 IMU 数据里的角速度方向定义与滤波器预测里使用的旋转方向相反，导致姿态更新一直往一个方向漂移，而 GNSS 位置约束不足以纠正姿态误差。解决：先查看新息序列，如果位置新息持续增大，对比纯 GNSS 轨迹与组合轨迹的差异，确认时间戳是否对齐到同一个时钟源。
5.3 现象：视觉里程计回环修正导致轨迹跳变
视觉里程计在回环检测到闭环时，会修正累计漂移，产生一个突然的位姿跳变。如果你直接把这个跳变后的位姿作为观测送进组合导航滤波器，融合轨迹会出现一个很大的台阶，整个定位结果直接废掉。原因：视觉里程计输出的位姿不是绝对真值，它的回环修正应在独立的图优化后产生，组合导航不应该吸收这种跳变。解决：一种做法是只用视觉里程计的增量（ΔT），而不是绝对位姿；另一种做法是对视觉位姿的跳变做检测，当相邻两帧位姿变化超过阈值时丢弃该次观测，或者给量测噪声乘上一个大系数。
5.4 现象：滤波器协方差快速收敛到零后系统僵死
滤波器跑了几分钟后，所有状态量看起来都稳定了，但一旦 GNSS 短暂失效再恢复，组合轨迹依然不跟 GNSS 走，像是滤波器卡死了。原因：协方差矩阵在持续更新中因为数值计算精度问题变得不正定，或者因为量测噪声设置过小，协方差快速收缩到近似零矩阵，导致卡尔曼增益也趋近零，后续任何观测都无法修正状态。解决：定期给协方差矩阵做对称化和正定化处理，在每次更新后加一个小对角阵（如1e-9 * I）。如果协方差已经强制收敛，可以在状态注入后把误差状态协方差重置为初始值的一部分。这是数值层面的坑，跟算法理论无关，但血泪经验证明它发生概率很高。
5.5 现象：杆臂效应没补偿，转弯时位置误差放大
车辆装了 RTK 天线，天线相位中心在车顶，IMU 在车厢中间，两者间隔约 1 米。直线行驶时误差不明显，一到急转弯，组合轨迹往外侧偏出几十厘米，而且转弯结束后还持续一段时间。原因：GNSS 测到的位置是天线相位中心的位置，不是车辆质心或 IMU 位置。在转弯时，车辆姿态变化，天线相对 IMU 的等效位置绕质心旋转，产生一个随姿态变化的误差。解决：在 GNSS 观测进入滤波器之前，把观测位置从天线的当前位置换算到 IMU 位置：p_imu = p_gnss - R * lever_arm，其中lever_arm是从 IMU 指向天线的平移向量。这个向量需要用前面的标定方法测准，否则转弯误差依然存在但减小。
6. 从原理到落地：验证组合导航精度的三个自测手段
组合导航系统写得再好，也要有验证手段，不然就是黑匣子。我总结的三个自测手段从简单到复杂，能覆盖大部分项目阶段。
第一个是静止测试。把系统放在稳定桌面或者地上，采集 5 分钟数据，观察滤波器输出的姿态、速度和位置。理想情况下，姿态在 0.1 度以内扰动，速度模值小于 0.01 m/s，位置漂移不超过 1 米（纯惯导积分时的位置应远大于此）。这个测试能暴露 IMU 噪声参数是否合理、零偏状态是否能估计出来。如果静止位置误差持续增长，说明滤波器没有把零偏估计好，优先检查过程噪声设置。
第二个是跑圈测试。开着车在开阔场地绕一个 100 米半径的圆圈，起点和终点重合。用组合导航输出的轨迹对比起点与终点的位置偏差，这个闭合误差直接反映系统的整体精度。更好的做法是同时记录 RTK 固定解作为真值，计算组合导航与 RTK 的实时误差曲线。跑圈时应包含加速、减速、匀速转弯和原地掉头几个工况，这样能同时验证速度观测和姿态激励的合理性。我建议跑三圈取平均闭合误差，如果第一圈和第二圈差异很大，说明系统存在未建模的时变误差，比如零偏随机游走设置不当。
第三个是遮挡测试。找一个两侧有高大建筑物的道路，人为制造 GNSS 遮挡和反射环境，让系统在遮挡区间只靠惯导和视觉/轮速外推。测试目标是：遮挡期间的位置误差增长能不能控制在 1% 的里程以内（比如行驶 100 米，漂移小于 1 米）。这个指标是组合导航的黄金标准。如果超了，就去查看遮挡期间的零偏估计是否发散，时间同步是否恶化。
我还养成了一个习惯：每次跑完测试，都会保存一条「新息」时间序列，就是滤波器每个观测时刻的残差。这种方法比只看位置轨迹更能暴露问题。新息均值持续不归零，说明模型有偏；新息振荡过大，说明噪声参数不匹配。把新息和传感器质量标记放在一起回放，能直接定位到是哪一路观测在哪个时间段把系统带跑偏的。测完这三个手段，基本能判断系统是能商用还是要返工。希望这套从原理到落地到避坑的路径，能帮你少走几趟我走过的弯路。
本文还有配套的精品资源，点击获取
