简介：在自动驾驶、机器人导航与手持测绘等场景中，单一传感器往往无法提供稳定可靠的定位结果。GPS在遮挡环境下易丢星跳变，IMU积分则随时间快速漂移，二者互补性极强。如何将两类异构数据融合成平滑、高精度的运动状态，是组合导航系统的核心问题。卡尔曼滤波作为经典状态估计框架，通过预测与更新不断修正系统状态，其中扩展卡尔曼滤波（EKF）和误差状态卡尔曼滤波（ESKF）是处理非线性系统最常用的两种实现路线。ESKF将状态拆分为名义状态与误差状态，显著提升了数值稳定性与零偏估计精度，尤其适合高频IMU与长时间运行的场景。本文从松耦合融合的最小实现出发，给出GPS+IMU融合的姿态、速度、位置与零偏估计代码骨架，并围绕协方差初值设定、时间戳同步、静止初始化等工程细节，系统梳理融合调参的常见陷阱与避坑实操，帮助工程人员快速搭建可运行的组合导航原型，并理解每个参数的物理含义。

1. GPS+IMU融合到底在解决什么问题：先把定位这件事说透

做车载定位、机器人导航或者手持测绘设备的人，大概率都遇到过这种场景：车开进高架桥下，GPS信号被遮挡，轨迹瞬间跳出去十几米；或者车停在原地，定位点却像喝醉了一样缓慢漂移。另一方面，纯IMU积分出来的轨迹，起步三秒还挺准，五秒之后就开始上天入地，因为陀螺零偏和加速度计噪声被双重积分放大成误差。GPS+IMU融合要做的，就是用卡尔曼滤波这套状态估计框架，把这两种“各有硬伤”的传感器约束在一起，让GPS的长期稳定性去修正IMU的短期漂移，让IMU的高频平滑性去填补GPS的丢星和跳变。这也是标题里EKF和ESKF两个关键字的由来——两种扩展卡尔曼滤波的工程实现形态。这篇文章面向的是准备动手做融合、但还在纠结怎么建模、怎么调参、怎么排查问题的从业者；读完你至少能搭出一套可运行的松耦合融合骨架，并且知道每个参数的物理含义。

2. 卡尔曼滤波系为什么能用于融合：EKF与ESKF的模型差异和选型依据

2.1 线性卡尔曼前提与EKF的必要性：GPS观测和IMU状态转移都不是线性的

经典卡尔曼滤波要求系统满足线性高斯假设：状态转移方程和观测方程都必须是线性的，且噪声服从高斯分布。但IMU的转动积分涉及姿态矩阵，GPS观测的经纬度转换到直角坐标也是非线性映射，这两条都不满足线性条件。EKF的做法是把非线性函数在当前估计点做一阶泰勒展开，用雅可比矩阵替代线性卡尔曼里的状态转移矩阵和观测矩阵。工程上这意味着两步：预报阶段用原始非线性方程积分状态，更新阶段用线性化后的矩阵计算增益和协方差。

EKF有个经典痛点：当姿态用四元数表示时，四元数必须保持单位模长约束。EKF里把四元数的增量当作普通向量加到状态里更新，更新完之后四元数的模长就不再是1了，需要额外归一化；更麻烦的是，四元数协方差更新没有考虑它的流形结构，会让一部分方差被分配到无意义的方向上。实践中如果IMU数据频率高、积分时间长，这个误差会被逐步放大。

2.2 ESKF的思路：将真实状态拆成名义状态和误差状态

ESKF（Error-State Kalman Filter，误差状态卡尔曼滤波）把真实状态拆成两部分：名义状态（nominal state）加上误差状态（error state）。名义状态用IMU原始测量直接积分，不考虑噪声项；误差状态则是一个小量，代表名义状态偏离真实状态的修正量，卡尔曼滤波作用在误差状态上。因为误差状态数值上非常小，线性化误差远小于直接对完整状态做EKF，而且四元数误差可以用三维向量表示，天然避开了四元数归一化问题。

ESKF的典型流程是：首先，IMU数据到来时，更新名义状态的姿态、速度、位置，按顺序完成积分；其次，构建误差状态的线性递推关系，包括误差状态转移矩阵F和噪声驱动矩阵V，它们都是可解析推导的；再次，GPS观测到来时，把观测残差映射到误差状态上，用卡尔曼增益修正误差状态；最后，将修正后的误差状态注入名义状态，然后把误差状态置零。整个过程里，卡尔曼滤波始终在处理“小误差”，数值稳定性显著优于对完整状态做EKF。

2.3 选型依据：松耦合场景下EKF与ESKF如何取舍

融合结构上还分松耦合和紧耦合。松耦合指GPS接收机先输出位置/速度解，融合算法再把它们当作观测；紧耦合则直接吃GPS的伪距、载波相位等原始观测。松耦合实现简单、调试直观，绝大多数入门到中级的项目够用，也是本文代码骨架的默认结构。在这个前提下，EKF和ESKF的取舍主要看IMU积分在状态里的占比。

如果IMU只是每几十毫秒给一次增量，且GPS更新率也有5Hz以上，直接做EKF一般不会太离谱。但一旦系统里IMU频率高（100Hz以上）、任务时间长（几分钟连续运动），或者需要频繁处理卫星信号中断后的重收敛，ESKF的优势就很明显。我做这类融合项目时默认选ESKF，因为它对零偏估计和姿态误差的处理更干净，调试中翻车概率明显低，代价只是代码量稍大。标题里同时出现EKF和ESKF，本质上反映的是同一个融合问题下的两种实现路线，理解差异比二选一更重要。

3. 把ESKF跑起来：松耦合GPS+IMU融合的最小实现

3.1 系统状态定义与坐标系约定

先约定坐标系，这是整个融合最容易被忽略、又最容易翻车的地方。IMU输出的加速度和角速度在IMU自身载体坐标系（b系）下描述；GPS输出的经纬度高程需要投影到当地导航坐标系（n系，通常取ENU或NED）。融合计算统一在n系下进行，状态向量包含位置、速度、姿态以及两个零偏。

实际工程里IMU安装会偏离载体坐标系原点，还会存在安装角误差，但对于入门级的松耦合验证，可以假设IMU坐标系与载体坐标系重合，把杆臂（lever arm）误差和安装角误差留到后面标定阶段处理。状态向量的具体定义如下：

# 定义ESKF的状态结构 # 名义状态向量: # p: 位置 (3,)，n系下，单位米 (ENU) # v: 速度 (3,)，n系下，单位 m/s # q: 姿态四元数 (4,)，n系相对于b系的旋转，即 R_nb # bg: 陀螺零偏 (3,)，单位 rad/s # ba: 加速度计零偏 (3,)，单位 m/s^2 # 误差状态向量: # dp: 位置误差 (3,) # dv: 速度误差 (3,) # dtheta: 姿态角误差 (3,)，小角度假设下的旋转向量 # dbg: 陀螺零偏误差 (3,) # dba: 加速度计零偏误差 (3,)

这段定义看起来平淡，实际决定了后面所有矩阵的维度。误差状态是15维，包含3维位置误差、3维速度误差、3维姿态误差、3维陀螺零偏误差和3维加速度计零偏误差。姿态误差用三维小角度表示，是ESKF避开四元数归一化问题的关键。如果要在位置里加入GPS天线杆臂补偿，需要额外在位置观测之前先把杆臂投影到n系并减掉。

3.2 IMU预测：名义状态积分和误差状态递推

IMU预测阶段，每来一帧IMU数据，就做一次名义状态积分和误差状态递推。名义状态积分用的是IMU原始测量减去零偏后的值；误差状态递推则用线性化模型。这里最需要注意的是积分顺序：一般先更新姿态，用更新后的姿态去旋转加速度，再更新速度，最后更新位置。

陀螺和加速度计的测量模型分别受零偏加白噪声驱动，零偏自身又被建模为随机游走，这是ESKF噪声矩阵V的来源。

import numpy as np from scipy.linalg import block_diag # 名义状态积分核心代码（姿态、速度、位置顺序更新） def nominal_state_integration(imu_gyro, imu_acc, state, dt): # imu_gyro: 陀螺测量 (3,), rad/s # imu_acc: 加速度计测量 (3,), m/s^2 # state: 包含 q, v, p, bg, ba # 先做姿态积分: 当前角速度 = 测量 - 陀螺零偏 omega = imu_gyro - state.bg delta_angle = omega * dt # 小角度增量四元数 delta_q = np.array([1, delta_angle[0]/2, delta_angle[1]/2, delta_angle[2]/2]) delta_q = delta_q / np.linalg.norm(delta_q) state.q = quaternion_multiply(state.q, delta_q) state.q = state.q / np.linalg.norm(state.q) # 用更新后的姿态旋转加速度: a_n = R(q)^T * (acc - ba) + g R_nb = quaternion_to_rotation_matrix(state.q) a_b = imu_acc - state.ba a_n = R_nb.T @ a_b + np.array([0, 0, g]) # 速度积分 state.v += a_n * dt # 位置积分 state.p += state.v * dt + 0.5 * a_n * dt * dt

这里的积分顺序不能颠倒，先用陀螺更新姿态再旋转加速度，否则转弯机动时加速度投影方向是错的，速度误差会在转弯时被明显放大。四元数积分用的是零阶保持角增量法，适用于100Hz以上的IMU数据；如果IMU频率降到50Hz以下，就需要考虑更高阶积分，否则车辆高动态转弯时姿态误差会先于GPS修正积累起来。

误差状态递推矩阵F的解析形式是ESKF实现里最绕的一部分。完整的F矩阵是15×15的分块矩阵，这里给出构造逻辑而不是逐元素推导：

def build_error_state_transition(state, imu_acc, dt): # 构建15x15误差状态转移矩阵F R_nb = quaternion_to_rotation_matrix(state.q) a_hat = imu_acc - state.ba a_skew = skew_symmetric(a_hat) # 三维向量转反对称矩阵 F = np.eye(15) # 位置误差对速度误差的耦合 F[0:3, 3:6] = np.eye(3) * dt # 速度误差对姿态误差的耦合: -R^T * skew(a_hat) * dt F[3:6, 6:9] = -R_nb.T @ a_skew * dt # 速度误差对加速度计零偏误差的耦合 F[3:6, 12:15] = -R_nb.T * dt # 姿态误差对陀螺零偏误差的耦合 F[6:9, 9:12] = -np.eye(3) * dt return F

速度误差对姿态误差的耦合项是ESKF里最容易写错的地方。它的物理含义是：当名义姿态存在误差dtheta时，用名义姿态去旋转加速度得到的n系加速度就有一个偏差，这个偏差反过来又影响速度误差的积分。加速度计零偏对速度误差的耦合方式与之类似，只是不需要旋转。这里如果漏了负号或放错了位置，融合结果会表现出特征性的“转完弯速度回不来”的现象，且GPS修正很难把它压回去。

3.3 GPS观测更新：位置残差与误差状态注入

GPS观测的更新频率通常比IMU低一个量级，典型值是5Hz到10Hz。收到一帧GPS位置后，先把它从经纬高投影到ENU坐标系，再和名义状态里的位置做差得到残差。观测矩阵H只需要选择位置对应的三个维度，因为松耦合不做速度观测的话GPS速度一般不参与，直接忽略即可。

def gps_update(state, P, gps_pos, H, R_gps): # state: 名义状态 # P: 15x15误差状态协方差 # gps_pos: GPS位置观测 (3,), ENU系 # H: 观测矩阵, 3x15, 形如 [I3, 0, 0, 0, 0] # R_gps: GPS位置噪声协方差 (3,3) z = gps_pos - state.p # 残差 (3,) # 卡尔曼增益 S = H @ P @ H.T + R_gps K = P @ H.T @ np.linalg.inv(S) # 误差状态更新 dx = K @ z # 协方差更新: 用Joseph形式保证对称性 I_KH = np.eye(15) - K @ H P = I_KH @ P @ I_KH.T + K @ R_gps @ K.T # 误差状态注入名义状态 state.p += dx[0:3] state.v += dx[3:6] state.q = quaternion_multiply(state.q, small_angle_to_quat(dx[6:9])) state.q = state.q / np.linalg.norm(state.q) state.bg += dx[9:12] state.ba += dx[12:15] # 误差状态重置为0, 同时P保持修正 dx = np.zeros(15) return state, P

协方差更新用Joseph形式而非标准形式KHP，能显著减少数值误差导致的协方差非对称问题。注意误差状态注入后四元数需要重新归一化，这是为数不多需要显式归一化的操作，因为注入的姿态误差小角度旋转可能让模长偏离1e-8量级。GPS观测协方差R_gps的设置不能只看接收机标称精度，还需要考虑多路径效应，在开阔场景和城市峡谷场景下取不同的值，这一点留到参数章说明。

3.4 完整跑通的最小骨架：代码组织方式

把预测和更新串起来的调度逻辑决定融合系统能不能实时运行。常见做法是维护一个待处理数据队列，IMU数据进队就做预测，GPS数据进队就做更新。由于IMU频率远高于GPS，每次GPS更新前可能有十几帧IMU已经在队列里，需要先对这些IMU逐帧预测，再执行当前GPS的观测更新。

def run_fusion(imu_queue, gps_queue, init_state, P0, dt_imu): state = init_state P = P0 for gps_msg in gps_queue: # 处理GPS时间戳之前的所有IMU帧 while imu_queue.front().timestamp < gps_msg.timestamp: imu = imu_queue.pop() state, P = esfk_prediction(state, P, imu.gyro, imu.acc, dt_imu) # 执行GPS观测更新 state, P = gps_update(state, P, project_to_enu(gps_msg.lat, gps_msg.lon, gps_msg.alt), H, R) # 记录融合结果 log_fused_state(state)

这里最核心的调度原则是严格按时间戳对齐，先预测到GPS时刻再更新，绝不能按IMU帧数凑合对齐。否则GPS观测和状态不在同一时间点，残差里会混入预测误差，表现为融合轨迹在GPS更新瞬间出现“台阶”。如果实时系统里存在消息延迟，合理做法是做消息等待而不是丢弃，因为在100Hz IMU下丢一帧IMU的数据影响远小于时间错配。

4. 参数与协方差调优：融合效果的分水岭

4.1 三个必调的协方差初值：陀螺噪声、加速度计噪声、GPS位置噪声

ESKF参数分为两类：IMU噪声功率谱密度和GPS观测噪声方差。它们分别决定预测可信度与观测可信度，比例失衡的后果是融合结果偏向某一类传感器。工程上最有效的调试方式是从IMU的Allan方差曲线出发标定功率谱密度，没有条件做Allan方差时，可以按器件手册的噪声密度值起调，再结合实际残差微调。

以下是一组车载场景的典型初值，来源是某低速机器人平台的调试结果，可以直接作为起步值：

这些值不是拍脑袋。sigma_g和sigma_a决定IMU预测阶段协方差增长的快慢，如果设得偏小，系统会过度相信IMU积分，GPS修正作用变弱，轨迹在GPS有效时依然有漂移；如果设得偏大，预测协方差快速膨胀，GPS一来就大幅拉偏轨迹，IMU的平滑性被破坏。GPS的R_v比R_h大是常识，因为高程方向卫星几何分布天然不如水平方向，加上多路径影响，高程噪声通常比平面大2到4倍。

4.2 时间戳同步与消息对齐策略

融合系统的整体精度上限由时间同步精度决定，推理再多也弥补不了毫秒级错配。GPS接收机输出的PPS（秒脉冲）和UTC时间戳通常精度很高，但IMU的时间戳如果走独立时钟，长时间运行后会出现秒级偏差。最常见的坑是IMU数据用本地微控制器时间打戳，GPS用接收机时间打戳，两者不统一，系统的融合结果在GPS更新瞬间反复跳动。

我一般会做两层校验。第一层是系统级同步，用一个统一的主时钟源给IMU消息和GPS消息同时打戳，主流做法是让GPS的PPS信号触发IMU采集并记录时间戳，把IMU时间对齐到GPS时间基准；第二层是数据级校验，每次GPS更新前检查当前处理到的IMU时间戳与GPS时间戳的差，超过一个IMU周期比如10ms就报警并标记该GPS观测不可用。这一条在很多开源项目里被省略，实际调试中它直接决定融合输出是平滑轨迹还是锯齿轨迹。

4.3 初值对齐：启动瞬间的偏置估计不能靠猜

ESKF开始工作前，需要给姿态、位置、速度和零偏一个初值。位置可以从GPS的第一帧读出，速度也可以由后续GPS差分得到，比较麻烦的是姿态和两个零偏。车载场景下起点通常是静止或缓慢运动的，这时有一个非常可靠的初始化技巧：静止状态下加速度计测量的模值应等于重力加速度，利用这个关系可以估计初始俯仰和横滚角；陀螺在静止时输出均值近似为零偏。

def initialize_stationary(imu_samples, gps_first_pos, dt): # imu_samples: 静止状态下采集的IMU数据列表, 建议采集1~2秒 acc_mean = np.mean([s.acc for s in imu_samples], axis=0) gyro_mean = np.mean([s.gyro for s in imu_samples], axis=0) # 由加速度均值估计俯仰和横滚角 roll = np.arctan2(acc_mean[1], acc_mean[2]) pitch = np.arctan2(-acc_mean[0], np.sqrt(acc_mean[1]**2 + acc_mean[2]**2)) # 航向角初始化为GPS首次运动方向或0 yaw = 0.0 init_q = euler_to_quat(roll, pitch, yaw) init_state = State(p=gps_first_pos, v=np.zeros(3), q=init_q, bg=gyro_mean, ba=acc_mean - np.array([0, 0, g])) return init_state

静止初始化最大的坑是采集时间太短，1秒内的噪声均值不够稳定，零偏初值偏差会持续影响整个融合过程。建议至少采集2秒静止数据，并且计算这段时间内加速度计的方差，如果方差过大说明车辆未真正静止，初始化要延后。航向角用0初始化通常可以接受，因为GPS位置观测对航向的可观性本身就弱，航向需要依赖运动学特征逐步收敛。

5. 避坑：GPS+IMU融合最常见的五个翻车现场

5.1 融合结果比纯GPS还差：协方差比例失衡

现象：融合轨迹与纯GPS输出的轨迹基本重合，甚至更跳，IMU的平滑优势完全没有体现出来。原因：IMU的Q矩阵设置偏大，或者GPS的R矩阵设置偏小，导致卡尔曼增益K的数值偏向于GPS，滤波器实际上成了GPS的低通版本。解决：检查稳态增益的数值，做法是把连续运行时的K矩阵打印出来，观察位置对应的前三行增益，发现它们接近1就说明R相对Q过小，需要增大R或减小Q，调整方向是把sigma_a降低一个数量级同时把R_gps提高一倍，再跑同一段路对比。

5.2 转弯时姿态漂移明显：航向可观性弱

现象：直线行驶融合轨迹正常，转弯过程中车头方向和融合输出姿态偏差大，轨迹出现向弯道内侧或外侧的偏移。原因：GPS位置观测对姿态的约束是间接的，需要通过速度、加速度的耦合才能反向估计角度。转弯时加速度变大，加速度计零偏和姿态之间的耦合效应被放大，二者难以区分。解决：在转弯工况下引入非完整约束，例如轮式车辆的侧向速度近似为零，将其作为虚拟观测加入滤波器；或者在有条件的场景增加磁力计航向观测，但要处理磁偏角和干扰。

5.3 协方差发散或变成负定矩阵：数值稳定性问题

现象：程序运行一段时间后，P矩阵对角线出现负值，或者滤波器增益突变，融合结果剧烈震荡。原因：协方差更新用标准形式时，由于浮点舍入误差累积，P矩阵逐渐失去对称正定性。解决：协方差更新统一改用Joseph形式，如第3章代码所示；同时每个更新周期结束后强制做一次对称化处理，取P=(P+P^T)/2。另一个容易忽视的问题是单精度浮点，IMU数据量大时单精度累加误差不可接受，至少用双精度，这在嵌入式平台上需要提前评估算力。

5.4 静止时位置缓慢漂移：零偏激励不足

现象：车辆原地停车，GPS位置基本不动，但融合输出的位置以每分钟几十厘米的速度缓慢漂移。原因：静止状态下加速度计零偏和姿态之间的耦合无法被GPS位置观测有效区分，滤波器处于弱可观测状态，零偏估计缓慢蠕变。解决：加一个静止检测逻辑，当一段窗口内的加速度方差和角速度方差都低于阈值时，认为车辆静止，此时冻结零偏误差状态的更新，或者把速度误差强置为零并缩小位置噪声，让位置预测暂停。静止检测窗口一般取0.5秒到1秒，方差阈值需要按IMU器件噪声水平实测确定。

5.5 GPS跳变导致融合轨迹出现台阶：观测粗差未剔除

现象：融合轨迹整体平滑，但偶尔在某个位置出现一个突发性的台阶，随后又恢复平滑，台阶高度可达几米。原因：GPS接收机在城市峡谷或树木遮挡环境下会出现瞬时多路径误差，输出的位置单点跳变，这不是高斯噪声而是粗差。解决：在观测更新前增加残差检验，计算残差的马氏距离，超过卡方检验阈值直接丢弃该观测；同时设置一个最大跳跃阈值，比如单步位置变化超过5米就标记为可疑。注意卡方检验的自由度取3（三维位置观测），置信度取0.95时阈值约为7.81，取0.99时约为11.34。

6. 验证融合效果：一条可复用的闭环评测技巧

判断融合算法有没有调对，不能只看跑出来的轨迹好看。我常用的做法是做一组对照实验：同一段路、同一个设备、跑两圈，第一圈只记录GPS原始轨迹，第二圈记录融合输出，然后对比两者与已知真值或参考轨迹的偏差统计量。更简单的做法是开环固定路径测试，在操场上走一个矩形或8字形，回到起点后测量融合输出的终点闭合误差，纯GPS的闭环误差通常在1到3米，ESKF融合调到1米以内才算及格。

另一个有效的量化手段是画残差分布直方图。融合滤波器的残差序列z的均值应接近零向量，如果残差均值显著偏离零，说明状态估计有偏，常见原因是IMU零偏初值不准；残差标准差如果远大于R的平方根，说明GPS观测噪声协方差被低估，数据里还有未建模的粗差，此时结合第5.5节的卡方检验进一步过滤。这个直方图画出来，比反复目测轨迹更能定位问题出在预测还是观测侧。

最后一招是回放调试。项目里我会把IMU和GPS原始数据录成文件，融合算法改成离线回放模式，每次调参后用同一包数据跑一遍结果，而不是重新去操场采集。好处是所有对比都建立在同一时间序列上，不会因为两次采集的轨迹不一致而误导判断。这个习惯帮我挡掉了至少一半的调参返工。希望帮到你。

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