GPS+IMU融合中EKF与ESKF的选型与工程实践

简介：基于卡尔曼滤波实现GPS与IMU融合的课程设计工程，同时涵盖EKF与ESKF两种方案，核心是演示ESKF误差状态滤波方法：不对导航信息直接滤波，而是估计IMU的误差量，利用误差为小量、线性化更精确的特点完成融合，适合学习组合导航、状态估计或卡尔曼滤波原理的学生与开发者。资源包共56个文件，包含13个hpp头文件、12个cpp源文件、9个csv数据文件、4个cmake构建脚本，另有yaml配置、Python辅助脚本、说明文档等，整体约5.14MB，目录划分清晰，核心算法、数据集、第三方库与测试程序一应俱全。代码中重点展示了IMU误差方程的运用方式，并配有测试程序帮助对照验证，可通过工程快速搭建EKF/ESKF融合框架，理解从数据读取、状态预测到误差更新的完整流程，直接用于课程设计或相关课题实验。已有1131人学习下载。

1. 融合不是把传感器拼起来：先说清标题里的 EKF、ESKF 与 100012392

我在某车载平台上调试组合导航时遇到过这样的尴尬：单用 GPS，城市峡谷里位置瞬间跳出去十几米；单用 IMU，过一个路口位置就已经不知道偏到哪里去了。把两者用卡尔曼滤波融合之后，轨迹看着像样了，但一上颠簸路段就露馅。后来把 EKF 换成 ESKF，才真正体会到「误差状态」这个设计有多关键。标题里的【100012392】我理解是一个工程代号，代表这套 GPS+IMU 组合导航项目的落地版本；但真正决定精度的不是代号，而是你对 EKF 和 ESKF 的选择、协方差的设置、以及每个观测进来时你心里有没有底。本文面向的是准备在机器人、车载或无人机上做 GPS+IMU 融合的开发者，从原理、选型到可复现代码和踩坑记录，一次讲透。

2. 为什么选卡尔曼滤波做 GPS+IMU 融合：从状态估计问题到选型依据

2.1 GPS 与 IMU 各自的原理与互补性

GPS 是一个绝对测量传感器，输出经纬度和高程，频率通常在 5~20Hz，误差不随时间累积，但在遮挡环境下会跳变甚至完全失效。IMU 是一个相对测量传感器，通过陀螺仪测角速度、加速度计测比力，频率可以做到 100~1000Hz，短时间内的相对变化非常准，但积分一次得速度、积分两次得位置，误差随时间二次增长。单独使用任何一方都无法满足长时间、高可靠的定位需求，这是融合的第一个动机。

这种互补性在工程上非常好用：GPS 负责把长期漂移拽回来，IMU 负责在 GPS 丢星或更新间隔里维持高频位姿。更进一步的互补体现在模型上——GPS 提供的绝对位置可以约束 IMU 积分产生的平移漂移，而 IMU 的姿态又可以把 GPS 的位置观测投影到车体坐标系下。换句话说，融合不是简单取平均值，而是根据两者各自的噪声特性做最优加权，这正是卡尔曼滤波要做的事情。

从估计问题的角度看，我们真正想估计的是系统的状态——位置、速度、姿态，以及 IMU 的零偏误差。所有传感器数据都是对这个状态的间接观测：GPS 给出的位置是直接观测，IMU 给出的比力和角速度则是驱动状态变化的输入。这样把问题建模成一个带噪声的非线性动态系统估计，卡尔曼滤波及其变体就成了最自然的选择。理解了这个建模过程，后续所有参数调整才有了判断依据。

2.2 从标准卡尔曼到 EKF：非线性系统怎么办

标准卡尔曼滤波要求系统是线性的、噪声是高斯的，即状态转移和观测方程都可以写成矩阵乘法的形式。GPS+IMU 融合系统显然不满足这个条件：姿态用旋转矩阵或四元数表示，状态转移里有三角运算，GPS 的经纬度还要转换成平面坐标。强行套标准 KF 会得到严重失真的结果，这一点通常在做完第一个仿真后就会体会到。

EKF 的思路是在每个时刻把非线性模型在当前状态估计附近做一阶泰勒展开，用雅可比矩阵替代原模型的线性变换，然后套用标准 KF 的递推公式。这是一个「用局部线性近似全局非线性」的策略，在扰动足够小的时候精度够用。实际工程里，车辆缓慢行驶或者无人机平飞时，EKF 表现出色，这也是很多原型系统选择它的原因。

但 EKF 有两个绕不开的痛点。第一，线性化误差会随系统非线性程度加剧而累积，当车辆大幅度转弯或剧烈颠簸时，雅可比矩阵与实际偏差变大，滤波器容易出现不一致。第二，姿态大多数时候用四元数表示，四元数本身有归一化约束，而 EKF 的协方差更新并不天然尊重这个约束，需要额外做归一化和特殊扰动定义，处理不当整个滤波会崩。我在调车时遇到过协方差矩阵变得不正定、然后滤波器输出直接跳到天边的情况，源头就是四元数更新处的雅可比简化过头。

因此在做选型评估时，我一般这样判断：如果系统长期工作在小角度扰动环境，比如室内机器人慢速移动，EKF 足够胜任；如果面对激烈运动或需要高精度姿态基准，比如车载高速或者无人机穿行，建议直接上 ESKF。与其在 EKF 上调几个月参数去补线性化的洞，不如一开始就选对结构。

2.3 EKF 与 ESKF 的本质区别和选型逻辑

ESKF（Error State Kalman Filter）的核心思想是：把状态拆成标称状态和误差状态两部分。标称状态由 IMU 的测量直接积分得到，不带协方差；误差状态才是滤波器真正估计和更新协方差的对象。这样做的好处非常明显。

第一，误差状态通常是小量，比如姿态误差对应一个小角度旋转矢量，比起标称姿态的四元数，线性化误差小得多。第二，小角度旋转可以用向量表示，避免了四元数更新时复杂的归一化问题。第三，误差状态的运动学方程比原状态方程更简洁，雅可比计算也更省事。这些特性让 ESKF 在处理强非线性运动和长时间运行时，明显优于直接使用四元数的 EKF。

下面这份选型参考表是我的个人经验，不是标准答案，但大致能覆盖多数项目决策场景：

从我个人的项目经验看，做原型验证先用 EKF 把数据链路跑通，把握系统噪声量级，再切换到 ESKF 做精度优化，是一条稳妥路线。有了这一层基础认识，下一章直接给出 EKF 方案的最小可复现工程，让新手也能先把系统转起来。

3. 用 EKF 实现 GPS+IMU 融合的最小可复现方案

3.1 状态向量、系统方程与观测方程的设计

EKF 方案里，状态向量我一般取 16 维：三维位置、三维速度、四元数姿态、陀螺零偏三维、加速度计零偏三维。四元数排在第 7~10 维，更新时用一阶近似并做归一化。系统方程以 IMU 的运动学为基础：位置由速度积分得到，速度由加速度计测量减去零偏和重力得到，姿态由陀螺角速度积分得到。这里要特别注意，IMU 的原始数据是状态转移的输入，不是观测，这个角色区分决定了滤波器的结构。

观测方程则把 GPS 的位置和速度映射到状态空间。GPS 的位置通常以经纬高表示，需要先投影到当地平面坐标系再进滤波器。二维或三维的速度观测可以直接对应状态里的速度分量。整个滤波器的核心流程是标准的预测-更新两段式：预测步骤用 IMU 数据做状态和协方差递推，更新步骤用 GPS 数据修正。这里不涉及任何高深技巧，关键是每个矩阵的维度要对齐，排错时最常出现的就是矩阵形状不匹配。

还有一个关键参数直接影响融合表现：IMU 与 GPS 的时间对齐。IMU 频率典型在 100Hz 以上，GPS 只有 10Hz。我一般把滤波器设计成 IMU 每次采样都做预测，只有 GPS 新数据到达时才做更新，这样既保持了高频输出，又充分利用了绝对观测。时间戳同步在 ROS 或嵌入式系统里通常用消息头时间戳对齐，偏差超过一个 IMU 周期就会出现可观测的震荡，这一节在避坑章还会展开。

3.2 核心代码骨架：预测与更新两个函数的实现

我用 Python 写过一个最小可复现的 EKF 融合骨架，喂入预先录好的数据包就能跑。下面给出核心结构，重点看状态预测和 GPS 更新两个函数。

import numpy as np from scipy.linalg import block_diag class EKFCombo: def __init__(self, dt_imu=0.01): # 状态维度：位置3 + 速度3 + 四元数4 + 陀螺零偏3 + 加计零偏3 self.dim = 16 self.x = np.zeros(self.dim) self.x[6] = 1.0 # 四元数初值 w=1 self.P = np.eye(self.dim) * 0.1 # 初始协方差 self.dt = dt_imu # 过程噪声：IMU 加速度噪声和角速度噪声 self.sigma_a = 0.5 # m/s^2 self.sigma_g = 0.01 # rad/s self.R_gps_pos = np.eye(3) * 2.0 # GPS 位置噪声 m^2 self.R_gps_vel = np.eye(3) * 0.5 # GPS 速度噪声 (m/s)^2 def predict(self, acc, gyro): # acc: 加速度计测量(减去零偏后)，gyro: 陀螺测量(减去零偏后) q = self.x[6:10] R = self._quat_to_rot(q) # 四元数转旋转矩阵 # 加速度世界系 = R * (acc - ba) + g a_world = R @ (acc - self.x[13:16]) + np.array([0, 0, -9.81]) # 位置/速度更新 self.x[0:3] += self.x[3:6] * self.dt + 0.5 * a_world * self.dt**2 self.x[3:6] += a_world * self.dt # 角速度增量更新四元数 omega = gyro - self.x[10:13] dq = self._delta_quat(omega, self.dt) q_new = self._quat_multiply(q, dq) q_new /= np.linalg.norm(q_new) self.x[6:10] = q_new # 协方差预测：用线性化雅可比 F 和过程噪声 Q F = self._compute_F(a_world, omega, self.dt) F = np.eye(self.dim) + F * self.dt Q = block_diag( self.sigma_a**2 * np.eye(3) * self.dt**2, self.sigma_a**2 * np.eye(3) * self.dt, self.sigma_g**2 * np.eye(3) * self.dt, np.eye(3) * 1e-6, # 陀螺零偏随机游走 np.eye(3) * 1e-6, # 加计零偏随机游走 ) self.P = F @ self.P @ F.T + Q def update_gps(self, gps_pos, gps_vel=None): z = gps_pos # 位置观测 H = np.zeros((3, self.dim)) H[:, 0:3] = np.eye(3) # 位置直接对应状态前三维 R = self.R_gps_pos # 标准卡尔曼更新 S = H @ self.P @ H.T + R K = self.P @ H.T @ np.linalg.inv(S) innovation = z - self.x[0:3] self.x += K @ innovation self.P = (np.eye(self.dim) - K @ H) @ self.P return innovation

这段代码的核心是预测里的四元数更新和更新里的标准卡尔曼增益。注意predict中的协方差预测用了一个近似雅可比 F，实际工程中需要从四元数扰动推导完整雅可比，否则协方差会不完全一致；update_gps里 H 矩阵把 GPS 位置映射到状态，这是松耦合里最常见的观测设计。

参数说明里最该强调的是sigma_a和sigma_g。很多新手把这两个参数理解为传感器的噪声指标直接抄手册，但数据手册给的是噪声密度，除以采样时间才是离散噪声协方差；而且滤波器里的过程噪声应当包含模型误差，我一般会把sigma_a比手册值调大 2~5 倍，让滤波器对 IMU 积分更「不信任」，从而在 GPS 更新时快速修正位置。GPS 的 R 矩阵同样不能只按 RTK 标称精度写，城市峡谷里非视距误差可能几十米，我习惯用卫星数和残差设计一个自适应系数，这个改动对跳变抑制非常明显。

3.3 初始化与首次跑通的三步建议

第一次跑通这套 EKF，建议按三个步骤来，避免在玄学调参里浪费一整天。第一步，采集一段静态数据，把设备静止放置 1~2 分钟，用这段时间估计 IMU 零偏和初始姿态。初始姿态偏差是最容易被忽视的坑：如果初始航向差 10 度，融合轨迹会持续带偏差且很难收敛。

第二步，只做预测不做更新，把 IMU 积分的结果和 GPS 轨迹叠加显示。这一步能看到纯惯导的漂移形态，同时确认数据时间戳对齐是否正常。如果 IMU 积分轨迹在 30 秒内就飞出去了，先不要急着调滤波器，回头查零偏和姿态初值。第三步，接上 GPS 更新，先只更新位置、暂不更新速度，观察位置是否被拉回真值。三步都顺利后再加入速度更新和完整的协方差整定。这套顺序的核心是让问题在链路中尽早暴露，而不是等融合输出一团糟时从中间排查。

提示：EKF 只是组合导航这座大厦的骨架，真正的精度来源是零偏标定、时延补偿和协方差整定。后面章节的避坑记录会逐一展开这些细节。

4. 用 ESKF 替代 EKF：误差状态系统设计与完整落地

4.1 误差状态传播：为什么标称状态不用修协方差

ESKF 的设计里有一个看似反直觉的点：IMU 积分出来的标称状态（位置、速度、姿态）完全不用协方差修正，滤波器所有的估计能力都集中在误差状态上。背后的逻辑是，标称状态是一条确定性积分路径，它存在的唯一意义是作为误差状态的线性化参考点；而误差状态是真实状态与标称状态之间的小偏差，它才是需要被统计学描述的量。

误差状态的维数设计比直接法 EKF 更精细。姿态误差用三维小角度矢量表示，位置误差、速度误差各三维，陀螺零偏误差和加计零偏误差各三维，总共 15 维。传播方程把 IMU 测量噪声和零偏随机游走注入误差协方差，而标称状态的积分路径只依赖原始测量。二者组合最终得出完整的估计输出。

我在实现中把整个过程分成两部分。预测阶段：先对标称状态做积分，再对误差状态协方差做递推；更新阶段：用 GPS 观测构造关于误差状态的观测方程，计算卡尔曼增益并修正误差状态，最后把修正量合并回标称状态。感知上比 EKF 多了一步，但因为误差状态是小量，所有线性化都更干净，实际运行反而更稳。

误差状态设计还有一个直接好处：IMU 零偏估计不再需要额外技巧。零偏本身在误差状态里就是待估计量，随着 GPS 更新不断被矫正；标称状态里减去的则是当前零偏估计值。这比在 EKF 里直接把零偏放进状态向量、同时面对四元数协方差尴尬要干净得多。

4.2 完整的 ESKF 预测与更新代码骨架

import numpy as np class ESKFCombo: def __init__(self): # 标称状态：位置3 + 速度3 + 四元数4 self.nom = {'p': np.zeros(3), 'v': np.zeros(3), 'q': np.array([1., 0, 0, 0])} # 误差状态：位置3 + 速度3 + 姿态3 + 陀螺零偏3 + 加计零偏3 self.dx = np.zeros(15) self.P = np.eye(15) * 0.1 self.ba = np.zeros(3) # 加计零偏估计 self.bg = np.zeros(3) # 陀螺零偏估计 self.dt = 0.01 def predict(self, acc, gyro): # 1. 标称状态积分（确定性路径） R = self._quat_to_rot(self.nom['q']) a = acc - self.ba w = gyro - self.bg g = np.array([0, 0, -9.81]) self.nom['p'] += self.nom['v'] * self.dt + 0.5 * (R @ a + g) * self.dt**2 self.nom['v'] += (R @ a + g) * self.dt dq = self._delta_quat(w, self.dt) self.nom['q'] = self._quat_multiply(self.nom['q'], dq) self.nom['q'] /= np.linalg.norm(self.nom['q']) # 2. 误差状态协方差预测 # 误差位置：dp += dv * dt # 误差速度：dv += (-R*[a]^T * dtheta + R*da_ba) * dt # 误差姿态：dtheta += R * dbg * dt Fx = self._compute_Fx(R, a, self.dt) # 15x15 雅可比 Qi = self._compute_Qi(self.dt) # 过程噪声注入 self.P = Fx @ self.P @ Fx.T + Qi # 误差状态均值保持为零，不递推 def update_gps(self, gps_pos): # 观测残差作用在误差状态上 z = gps_pos - self.nom['p'] H = np.zeros((3, 15)) H[:, 0:3] = np.eye(3) R = np.eye(3) * 1.5 S = H @ self.P @ H.T + R K = self.P @ H.T @ np.linalg.inv(S) dx_corr = K @ z # 补偿：把误差状态修正量合并回标称状态 self.nom['p'] += dx_corr[0:3] self.nom['v'] += dx_corr[3:6] dtheta = dx_corr[6:9] self.nom['q'] = self._quat_multiply(self.nom['q'], self._so3_exp(dtheta)) self.nom['q'] /= np.linalg.norm(self.nom['q']) self.ba += dx_corr[9:12] self.bg += dx_corr[12:15] # 修正后误差状态归零，协方差做标准更新 self.P = (np.eye(15) - K @ H) @ self.P self.dx = np.zeros(15)

重点解释两个设计决策。第一，误差状态预测完要不要把 dx 做递推？在我的实现里，误差状态的均值始终保持为零，因为真实误差围绕零上下分布，滤波器更新的是它偏离零的幅度；如果预测阶段强行递推误差均值，反而会把标称状态的积分误差重复计入。这在实际代码里省掉了一个 15 维状态转移，也让协方差一致性更容易保持。

第二，更新阶段把 GPS 位置与标称位置的差值当作观测残差，经卡尔曼增益换算得到误差状态修正量，再补偿回标称状态。补偿完成后误差状态清零、协方差保留修正后的不确定性，这是 ESKF 与直接法 EKF 最明显的分水岭。第一次从 EKF 迁移过来最容易在这里卡住，总以为误差状态应该带着残差继续跑，实际上清零才是正确做法。

协方差更新用(I - K H) P是标准形式，但在实际工程中我更推荐 Joseph 形式(I-KH)P(I-KH)^T + K R K^T，它能在数值上保证 P 保持对称正定，防止长期运行后协方差悄悄变成奇异矩阵，让滤波器错误地认为自己已经完全准确。

4.3 为什么 ESKF 在高速高动态场景中更稳

在低速、平稳路况下，EKF 与 ESKF 的输出差异可能只有几个百分点；但上了高速匝道、颠簸路面或无人机做大机动时，差异就非常显著。EKF 的雅可比矩阵在大角度姿态变化时误差偏大，并且四元数协方差更新后经常违反归一化约束；ESKF 因为永远处理小量误差，这两个问题基本不出现。

我在车载实验里对比过同一段数据，ESKF 的航向误差在连续急转弯后比 EKF 小大约一个数量级，GPS 短暂中断 5 秒后位置发散速度也明显更慢。原因在于误差状态协方差更准确，滤波器在 GPS 重新可用时能够合理分配对惯导结果的信任度。如果项目最终要在复杂路况上长期运行，我不太建议在 EKF 上调太久，直接切 ESKF 更划算。

不过 ESKF 并不是终点。在高精度地图、车道级定位这类场景里，基于因子图的多源融合正在成为趋势，滤波器主要保留实时性优势。下一章先不急着做架构迁移，我要把这几个方案里最常踩的坑一次说清，因为很多项目翻车都不是原理没看懂，而是参数和时序细节没处理好。

5. 避坑与排查：GPS+IMU 融合中最常见的 5 个问题

这章把我在实际项目里碰到的高频问题按现象到解决完整记录一次。这些问题不会同时出现，但任何一个冒出来都会让融合输出的可信度归零。排查顺序也很有讲究，我一般按时间同步、坐标系、参数量级、观测异常的顺序来，不要一上来就怀疑滤波算法本身。

5.1 协方差发散：滤波器输出直接跳出天际

现象：位置估计突然跳到几百米外，之后长时间无法拉回来，P 矩阵对角线数值飙到极大。

原因：绝大多数时候是 GPS 观测混入了粗差，多路径或非视距信号导致定位出现单个跳点，而固定 R 矩阵没有能力识别这种异常。卡尔曼增益被这个错误观测拉偏，协方差在错误方向上急剧收缩，滤波器此后认为自己很确定地走在错误道路上，再也没法纠正。

解决：先接一个卡方粗差检测，计算每个 GPS 观测残差的马氏距离，超过阈值就丢弃这帧数据。我习惯用自由度 2 的卡方分布 95% 分位值约 5.991 作为门限。更稳的做法是给 R 矩阵加自适应系数，让残差大的观测自动降权。这个改动通常能挽回一半以上的「滤波器神奇跳变」问题，属于投入产出比极高的一项改造。

5.2 静止时速度不归零，位置持续漂移

现象：设备放在桌面上不动，融合后的速度输出有 0.1~0.5m/s 的残留，位置随时间线性偏移。

原因：IMU 零偏估计不收敛，或者初始姿态对重力方向的估计不准。静止时加速度计测量里混入零偏，经姿态投影到世界系后变成持续的小加速度，积分后速度自然不归零。很多人以为零偏只影响角度，实际上加计零偏对位置的影响更隐蔽。

解决：初始化阶段要静止足够久，我一般取 60~120 秒。先用重力投影初始化姿态的俯仰和横滚，再用静止段均值做陀螺和加计零偏初始值。另一个容易被忽略的细节是振动：设备如果安装在发动机运转的平台上，加速度计方差会偏大，需要用零速检测器决定何时启用静止零偏更新，而不是一直让静止假设生效。

5.3 GPS 与 IMU 时间不同步导致融合震荡

现象：车辆急加速或急刹车时，位置输出出现周期约一秒的锯齿形振荡，振荡频率恰好与 GPS 更新频率吻合。

原因：GPS 接收机内部处理会引入几十到几百毫秒的延迟，如果直接把到达时刻的数据当作当前时刻对准 IMU，相当于用一个过去的位置修正当前状态，动态场景下会产生系统性超前或滞后。这个问题在静态测试中几乎看不出来，所以特别容易被漏掉。

解决：通过接收机的 PPS 脉冲或数据包时间戳估算延迟，在融合前把 IMU 状态回退或外推到 GPS 观测时刻再执行更新。量化地说，100ms 的未补偿延迟在 36km/h 下就对应 1 米的位置误差，而车载场景中最难调的几米误差往往就藏在这里。时间同步是组合导航里性价比最高的优化项，优先级高于任何滤波参数调整。

5.4 IMU 标定参数不准：融合效果不如单 GPS

现象：融合轨迹看起来比单 GPS 还差，尤其是姿态有明显倾斜，转弯时位置过冲，直行时航向慢慢偏移。

原因：IMU 的 scale factor 和安装角度偏差没有标定，陀螺在转弯时给出的角速度偏大或偏小，姿态长期带偏差，位置解算自然被带偏。零偏只是标定的一部分，不是全部；许多消费级 IMU 的 scale factor 误差在 0.5%~1% 之间，高速时足以产生明显轨迹误差。

解决：做六位置静态标定估计加计零偏和 scale factor，再用转台或足够长的多姿态旋转估计陀螺参数；没有转台时，也可以用采集到的多组静止数据配合非线性优化求解。标定结果直接决定融合精度的上限，这一步省不得。另一个庞大的坑是安装角度：IMU 坐标系与车体坐标系之间哪怕偏差 1 度，在位置解算中的影响也会随速度线性放大。

5.5 仿真和实车效果差异大：高性能滤波器为什么在实车上翻车

现象：同样的参数在仿真数据集上效果优秀，换到实车数据后位置抖动明显，GPS 更新时有台阶状跳变。

原因：仿真数据噪声模型太干净，GPS 和 IMU 时间戳理想对齐；实车上存在杆臂效应、车体振动、温度漂移和时延。杆臂效应指 IMU 与 GPS 天线在车体上的安装位置不重合，车辆转弯时两者感受的线速度不同，未补偿时会产生几厘米到几十厘米的额外误差。

解决：把杆臂向量显式建模到观测方程里，至少要在初始化时测量并在投影 GPS 位置时扣除。真实数据的协方差要放大到仿真值的 3~10 倍再开始整定。我的习惯是先用一段最烂的实车数据来调试滤波器鲁棒性，而不是挑效果最好的路段去追求「漂亮轨迹」；能扛住烂数据的滤波器，换到好路段往往才有惊喜。

6. 验证闭环与进阶方向：当精度不够用时该往哪走

6.1 用一段可控仿真数据验证滤波器是否正确

在没有实车或飞行器之前，我建议先做闭环仿真。构造一条带真实噪声的轨迹，生成 IMU 采样和 GPS 观测，然后跑融合。这个阶段重点看两个指标：位置误差的均值和标准差是否收敛、姿态误差是否在零附近波动。如果协方差显示收敛但实际误差很大，问题几乎都出在雅可比推导或状态定义上；如果输出误差大且协方差也大，则是观测噪声设置不合理。

最有效的验证不是看最终轨迹贴不贴真值，而是做残差分析。把每个 GPS 更新时刻的 innovation 序列打印出来，看它是否满足零均值白噪声特性。如果 innovation 有系统性偏置，说明模型里漏了某个误差项；如果方差比 S 矩阵预测值大，说明噪声参数设置得过低。这套自检方法能帮你从「玄学调参」走向「有依据的整定」，也是我在每次换平台后必做的一步。

6.2 从松耦合走向紧耦合：什么时候值得升级

松耦合把 GPS 的位置和速度当作观测量，紧耦合则直接使用伪距、载波相位或多普勒观测。紧耦合的优势在于：GPS 卫星数少于 4 颗时松耦合直接无解，紧耦合仍能用部分观测量维持更新；城市峡谷里多路径严重的单颗星也可以被检测并剔除。代价是状态维度增加、代码复杂度翻倍、整定难度陡增，对团队的调试能力要求明显更高。

我的建议是：如果系统已经在用 RTK 或 PPP 这类高精度定位源，且工作环境存在信号遮挡，紧耦合的回报很高；如果只消费 NMEA 输出或普通定位结果，松耦合已经能发挥绝大部分潜力，先把时间同步和粗差检测做好更实际。不要为了技术上的「先进」盲目上紧耦合，项目交付节奏往往不允许你在滤波架构上反复折腾。

6.3 航向可观测性与零速修正：两个性价比极高的补充

组合导航里有一个容易被忽略的事实：GPS 位置观测对航向的直接可观测性很弱。车辆长时间直线行驶时，位置残差几乎无法约束航向误差，只有转弯时航向误差才变得可观。解决这个问题的常见做法是引入磁力计做航向参考，或者利用车辆非完整性约束——假设车辆不会侧向滑动，把约束做成伪观测。这个约束对车载场景的航向估计有立竿见影的效果，值得优先做。

对行人导航或低速设备，零速修正（ZUPT）是比任何滤波算法都更管用的技巧。每当检测到设备静止，就把速度观测设为零执行一次更新，位置漂移立刻被压住。静止检测一般基于加速度计方差和角速度幅值联合判断，误检测会导致错误修正，所以阈值要保守一些。这两项补充都属于改动量不大但收益明显的优化方向。

最后我想说一个切身教训：早期做这套融合时，我花了两周调 EKF 的 R 和 Q，试图把位置误差从 2 米压到 1 米，始终不见好转。后来检查数据才发现，GPS 天线与 IMU 的杆臂有 30 厘米没补偿，而这个量级恰好就在 1 米以内的误差来源里。从那以后我养成了一个习惯：任何融合精度的争议，先检查时间同步、杆臂和坐标系，再回到滤波算法本身。技术选型再正确，也架不住最底层输入的错误；这套排查顺序希望对你有帮助。

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