简介：一套基于卡尔曼滤波实现GPS与IMU融合的完整工程代码包，主要面向学习组合导航、状态估计或从事相关课程设计的高年级本科生与研究生。工程围绕EKF与ESKF两种滤波方案展开，重点讲解ESKF为何对导航误差而非导航状态本身进行滤波，并给出IMU误差方程及松耦合融合思路。包内共56个文件，以C++头文件与源文件为主，辅以CMake构建脚本、配置文件、测试数据CSV及简要说明文档，目录划分清晰，便于直接编译运行与二次修改，资源压缩包约5.14MB，小巧精炼。目前已有1131人学习下载，适合希望从代码层面理解GPS/IMU融合原理并快速搭建实验环境的读者。通过阅读源码和测试样例，可以掌握EKF/ESKF核心流程、传感器数据读取方式及算法调试方法，具有一定的工程参考价值。
1. 卡尔曼滤波在GPS+IMU融合中的作用：为什么不能只做加权平均
开车进地下车库的瞬间，手机导航上的蓝色箭头会先在原地乱转，随后干脆跳到另一条街上。GPS在遮挡环境下误差能从2米膨胀到20米；而IMU虽然以100Hz输出角速度和加速度，积分出的位置却在几十秒后明显漂移。单独看任何一种传感器，组合导航都无从谈起。把GPS的绝对位置和IMU的相对增量放到同一个滤波框架里，用协方差动态决定信任比例，正是卡尔曼滤波在定位融合里最典型的用法。
下面从EKF（扩展卡尔曼滤波）展开，先讲清楚GPS和IMU各自的误差模型以及坐标变换，再给出一组可运行的松耦合融合代码和Q/R调参基准，最后落到另一个高频选择ESKF（误差状态卡尔曼滤波）和几个实测中最容易翻车的验证技巧。适合需要自己搭组合导航、做定位融合的工程师，也适合把开源代码跑通但对参数一头雾水的初学者。
2. 传感器误差模型与坐标变换：GPS和IMU融合前必须打好的两个底子
很多人上来就写滤波，结果参数调不动，问题大多出在输入数据本身没对齐。融合不是把两串数字扔进公式就行，GPS要先从经纬度转到平面坐标，IMU要先弄清零偏和重力分量，时间戳还要对上。这一章先把“数据能不能直接进滤波器”这件事解决掉。
2.1 GPS误差不是白噪声：位置抖动、慢变漂移与坐标翻转
GPS输出的是WGS84经纬高，融合前必须转成局部ENU或NED坐标。简单做法是取起点为原点，用等距圆柱近似：
import numpy as np EARTH_R = 6378137.0 def lla_to_enu(lat0, lon0, lat, lon): """ 将经纬度转到以 (lat0, lon0) 为原点的ENU平面坐标。 输入输出单位分别为弧度和米。 """ lat_r, lon_r = np.radians(lat), np.radians(lon) lat0_r, lon0_r = np.radians(lat0), np.radians(lon0) x = EARTH_R * (lon_r - lon0_r) * np.cos(lat0_r) y = EARTH_R * (lat_r - lat0_r) return x, y说明：这是局部近似公式，在几百公里范围内精度够用。x方向必须乘cos，纬度越高，同一经度差对应的弧长越短。漏掉这个修正，在高纬地区会出现明显位置偏移，很多人搜“GPS翻转补丁”“坐标转换异常”，其实根因都在原点或投影参数没选对。GPS的单点定位误差也不是理想高斯白噪声，静止时轨迹会慢慢游走，误差具有时间相关性。卡尔曼滤波里通常近似为高斯白噪声，但R要适当放大，给“有色误差”留出余量。
低速或静止时GPS输出的航向角没有意义，会在0到360度之间乱跳，不能直接作为姿态观测使用。
2.2 IMU零偏与积分漂移：位置误差按时间的平方增长
IMU输出的是角速度和比力，位置需要两次积分。零偏是最常见的误差源。静止时加速度计读数包含重力，必须先减掉重力再积分，否则相当于给系统施加了1g的常值加速度，位置瞬间爆炸。零偏导致的位置漂移可以用一段简单代码模拟：
dt = 0.01 # 100Hz采样 bias_a = 0.05 # 加速度计零偏，单位 m/s^2 v, p = 0.0, 0.0 for _ in range(500): # 模拟5秒 v += bias_a * dt p += v * dt print(f"5秒后速度误差 {v:.2f} m/s，位置误差 {p:.2f} m")逻辑说明：每次循环，零偏对速度做一次累加，位置再对速度累加。双重积分下来，位置误差大致是0.5 * bias * t^2，0.05m/s²的零偏在5秒后就能产生0.6米以上偏移。实际零偏还随温度漂移，并有随机游走成分。因此在滤波里除了位置、速度、姿态，通常还要把零偏放进状态向量，否则位置会持续偏移。这也是后面ESKF把零偏作为误差状态分量来估计的原因。
2.3 误差模型参数表：先有量级，再去定Q和R
| 参数 | 含义 | 典型量级 | 对融合的影响 |
|---|---|---|---|
| 陀螺角度随机游走 | 角速度白噪声引起的姿态漂移 | 0.01~0.1°/√h | 姿态小量高频抖动 |
| 陀螺零偏稳定性 | 长时间静止时零偏的慢变 | 1~100°/h | 姿态长时间漂移，尤其是yaw |
| 加速度计零偏 | 比力输出中的常值偏差 | 1~10 mg | 位置按t²漂移 |
| 速度随机游走 | 加速度白噪声积分 | 0.005~0.05 m/s/√h | 速度抖动 |
| GPS位置噪声 | 单点定位的1σ | 1~3 m | R矩阵的基准量级 |
这个表里给的是消费级MEMS和普通单频GPS的量级。换成RTK或者工业级光纤惯导，数值会差很多，但调参思路一致：先用表里的量级做起点，再根据实验结果微调。
2.4 时间对齐：100Hz IMU和10Hz GPS如何做时间戳匹配
松耦合结构里，IMU每步做预测，GPS每100ms到达一次做修正。代码里常见做法是最近邻对齐：
# imu_times: IMU的100Hz时间戳数组, gps_t: 当前GPS观测时刻 k = np.searchsorted(imu_times, gps_t) - 1 # 取出第k时刻的预测状态和协方差做更新说明：searchsorted返回第一个不小于gps_t的位置，减1是取上一个IMU时刻。如果GPS报文有固定的输出延迟，先用gps_t - delay作为真实测量时刻再查索引。不处理时间同步，常见现象是轨迹看起来“差不多”，但转弯时融合位置总是滞后零点几秒，转弯越急误差越大。
3. 用扩展卡尔曼滤波实现GPS+IMU松耦合融合：一组能跑的Python代码
误差模型捋顺之后，现在写EKF。为什么叫扩展？因为状态预测方程里有旋转矩阵和角速度积分，这些都不是线性关系。标准卡尔曼滤波的线性高斯假设在这里撑不住，需要在当前状态附近做一阶泰勒展开，把非线性方程局部线性化，这就是EKF。
3.1 状态向量与IMU预测模型：先定5维还是更高维
我一般先从2D模型起步：状态x = [px, py, vx, vy, yaw]，控制量u = [ax, ay, wz]，其中ax, ay是载体坐标系下的加速度，wz是偏航角速度。位置与速度由体轴系加速度旋转到导航系后积分，yaw直接用角速度积分。加入零偏会变成7维以上，但EKF的结构完全一样；先把核心跑通再加零偏，排查问题会容易很多。
3.2 预测步骤：状态转移与雅可比矩阵
import numpy as np def ekf_predict(x, P, u, dt, Q): px, py, vx, vy, yaw = x ax, ay, wz = u c, s = np.cos(yaw), np.sin(yaw) R_bn = np.array([[c, -s], [s, c]]) # 体坐标系 -> 导航坐标系 a_n = R_bn @ np.array([ax, ay]) # 导航系下的加速度 da_dyaw = np.array([-ax*s - ay*c, ax*c - ay*s]) # a_n 对 yaw 的偏导 x_new = np.array([ px + vx*dt + 0.5*a_n[0]*dt*dt, py + vy*dt + 0.5*a_n[1]*dt*dt, vx + a_n[0]*dt, vy + a_n[1]*dt, yaw + wz*dt ]) F = np.eye(5) F[0, 2] = dt F[1, 3] = dt F[0, 4] = 0.5*da_dyaw[0]*dt*dt # 位置对yaw的偏导 F[1, 4] = 0.5*da_dyaw[1]*dt*dt F[2, 4] = da_dyaw[0]*dt # 速度对yaw的偏导 F[3, 4] = da_dyaw[1]*dt P = F @ P @ F.T + Q return x_new, P逻辑说明：加速度a_n是yaw的函数，所以位置、速度对yaw都有偏导，F里这些交叉项不能省。漏掉任意一项，协方差都会被低估，滤波器后续就会过度信任自己的预测。dt是IMU间隔，100Hz时取0.01。Q取对角线矩阵，维度必须和状态维度一致，否则矩阵运算直接报错。这里给的是解析雅可比；如果状态更复杂，也可以用一阶差分做数值雅可比，IMU频率高时两者差异很小。
3.3 更新步骤：GPS位置观测的注入
def ekf_update(x, P, z, R): H = np.zeros((2, 5)) H[0, 0] = 1.0 H[1, 1] = 1.0 # GPS只观测 px, py y = z - H @ x # 新息 S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) x_new = x + K @ y P_new = (np.eye(5) - K @ H) @ P return x_new, P_new说明：z是GPS在ENU系下的[x, y]，所以H只有两列非零。如果GPS模块还输出速度（比如多普勒测速），可以把H扩成4×5，R相应扩成4×4。但消费级GPS的速度噪声偏大，融合收益有限，我一般不用。重点检查新息y的单位：如果R给的是经纬度方差而z是米，结果完全不对。所有单位换算必须在进滤波器之前完成。
3.4 主循环和Q/R参数的推荐起点
dt = 0.01 Q = np.diag([0.02, 0.02, 0.2, 0.2, 0.005]) # 位置/速度/航向过程噪声 R = np.diag([1.0, 1.0]) # GPS位置观测噪声协方差 x = np.array([0.0, 0.0, 1.0, 0.0, 0.0]) P = np.eye(5) * 0.1 for i in range(10000): u = read_imu() # (ax, ay, wz) x, P = ekf_predict(x, P, u, dt, Q) if gps_ready(i): z = read_gps_xy() # 已转ENU的xy x, P = ekf_update(x, P, z, R)参数说明：Q里的值数量级来自IMU白噪声和运动模式。Q[0:2]=0.02意味着位置过程噪声标准差约0.14m，适合低速车辆场景；速度项给0.2，对应约0.45m/s的随机游走。R=1.0等于假设GPS单点定位1σ误差为1米。用RTK时R可以减到0.01；用手机原始观测时R可能要加到4以上。调参第一原则：一次只动一个参数。
提示：轨迹“长刺”或抖得厉害时，先怀疑R是否偏小。R偏小时滤波器盲目信任GPS噪声，把误差全吸了进来。
| 症状 | 大概率问题 | 调法 |
|---|---|---|
| 轨迹抖动，出现尖刺 | R太小或Q太大 | 把R调大2~5倍再试 |
| 响应慢，转弯跟不上 | Q太小或R太大 | 保持R，把Q的速度项调大 |
| 位置整体漂移不收敛 | 加速度计未扣重力或未估计零偏 | 检查预处理，调参救不回来 |
| 静止时位置缓慢游走 | GPS有色噪声，R偏小 | 适当加大R |
3.5 状态发散时的排查顺序
滤波发散时不要急着调参。先检查时间戳：GPS观测是否和IMU预测状态处于同一时刻；再检查单位：经纬度是否已经转成米，加速度是否已扣除重力；接着看旋转方向：体轴系到导航系的转换写反，yaw会急速旋转；最后才动Q和R。大部分“调不出来”的问题都出在前三步，而不是参数上。
4. 从EKF到ESKF：误差状态卡尔曼滤波如何应对IMU姿态漂移
2D平面里EKF完全够用，但进入三维姿态和零偏联合估计，普通EKF会遇到几个结构性麻烦。我一般会在三维项目里直接上ESKF。ESKF不是新算法，而是把滤波状态搬到了一个更合适的地方。
4.1 直接对姿态做滤波的三个麻烦
- 欧拉角存在万向锁：横滚接近90°后俯仰和航向无法区分，F矩阵出现奇异。
- 四元数维度是4，约束是单位模长。EKF更新
x + K*y会破坏模长，强行归一化后又引入额外误差。 - 姿态误差大时，一阶线性化近似效果差，收敛慢甚至发散。IMU零偏和初始对准误差都会放大这个问题。
这也是在网上搜“基于IMU的位姿解算yaw仍会慢漂”时经常看到的现象：yaw没有绝对观测，零偏和积分误差低估都被EKF的线性化问题进一步放大。
4.2 ESKF的基本思路：名义状态加误差状态
把真实状态拆成两部分。位置、速度用加法：p_true = p_nom + δp，v_true = v_nom + δv。姿态用乘法：q_true = q_nom ⊗ δq，其中δq对应小角度旋转向量δθ。陀螺零偏和加速度计零偏也都放进误差状态。
名义状态直接用IMU积分推进，误差状态用卡尔曼滤波估计。因为误差量始终是小量，线性化点附近的一阶近似非常准。卡尔曼滤波处理的永远是误差的均值与协方差，而不是绝对位置这种大数值状态。ESKF的线性化点几乎不变，精度比直接在绝对状态上做EKF稳定得多。
4.3 ESKF的完整流程
流程文字：
- 预测：用IMU读数更新名义状态
p_nom, v_nom, q_nom；同时按误差方程推进协方差P_err = F_err * P_err * F_err^T + Q_err。 - 观测更新：GPS给位置观测时，新息
y = z - p_nom，观测矩阵H对应误差状态里位置子块是单位阵，其余为零。 - 计算卡尔曼增益，得到误差状态
δx。 - 注入：把
δx修正到名义状态，位置速度直接相加，姿态做四元数乘法，零偏同样累加。 - 对误差状态执行reset，把
δx置零，协方差通过reset_cov保持一致性。
# ESKF 一次迭代的骨架 def eskf_iter(x_nom, P, imu, z_gps, dt): # 1. 名义状态积分 x_nom = integrate_nominal(x_nom, imu, dt) # 2. 误差状态的协方差预测 F = build_error_state_jacobian(x_nom, imu, dt) P = F @ P @ F.T + Q_err # 3. 卡尔曼更新（若GPS可用） if z_gps is not None: H = np.zeros((2, 15)) H[0, 0] = 1.0 H[1, 1] = 1.0 y = z_gps - x_nom.p[0:2] S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) dx = K @ y # 4. 注入 x_nom = inject(x_nom, dx) # 5. reset误差状态 P = reset_cov(P, K, H) return x_nom, P说明：这里状态采用常见15维排列：误差位置3维、误差速度3维、误差姿态3维、陀螺零偏3维、加速度计零偏3维。GPS只观测前两维，H按实际观测维度截取。F_err的构建需要当前IMU的角速度和比力参与，因为在误差状态方程里，这些量出现在交叉项。reset步骤不能省：误差均值清零后，协方差要及时修正，否则下一次预测的协方差会不一致。
4.4 EKF和ESKF怎么选：一张很实用的对比表
| 比较项 | 常规EKF | ESKF |
|---|---|---|
| 状态对象 | 绝对位置、速度、姿态 | 名义状态 + 小量误差状态 |
| 姿态约束 | 欧拉角奇异，四元数需归一化 | δθ天然是三维旋转向量，兼容旋转群 |
| 线性化精度 | 大角度误差时一阶近似偏差大 | 误差小，一阶近似充分 |
| 实现复杂度 | 直接，容易上手 | 多一个注入和reset步骤 |
| 典型场景 | 2D平面、角度变化小的系统 | 3D惯导、视觉SLAM、激光SLAM |
从EKF迁到ESKF时最常见的坑有两个。一个是观测矩阵H写成绝对状态的偏导，维度对不上；另一个是reset后忘了把误差状态清零，下一次更新时新息里混入旧误差。这两个错误在残差诊断里能看出来。
5. 验证融合结果最该先做的三个检查：时间延迟、重力对齐与新息统计
融合代码跑通只是第一步，结果可不可信需要验证。我一般会先做三件事：检查时间延迟、做重力对齐、统计新息。
5.1 检查GPS相对IMU有没有固定时延
固定时延可以用互相关估计：
# x_imu: IMU推算的轨迹, x_gps: GPS位置 corr = np.correlate(x_gps - x_gps.mean(), x_imu - x_imu.mean(), "full") delay = corr.argmax() - (len(x_imu) - 1)说明：互相关峰值对应的索引差就是延迟的帧数。若delay明显不为0，回融合代码里修正时间戳。很多项目里GPS报文的时间戳是接收机内部处理时间，而不是真正的位置采集时刻，这个检查非常值得做。
5.2 初始姿态的重力对齐
静止时取200个加速度样本求平均，在NED坐标定义下计算横滚和俯仰：
roll = atan2(ay_bar, az_bar) pitch = atan2(-ax_bar, sqrt(ay_bar^2 + az_bar^2))yaw在没有磁力计或双天线时无解，可以先置0，等运动激励出GPS轨迹方向后再修正。这就是IMU重力对齐的基本做法。对齐没做好，滤波器会在一个倾斜坐标系里运行，GPS修正也无法把位置拉回来。
5.3 用新息做滤波器一致性检验
新息已经在更新步骤里算过：y = z - H @ x，它的协方差是S。构造NIS统计量：
y = z - H @ x S = H @ P @ H.T + R nis = y @ np.linalg.inv(S) @ y if nis > 5.99: print("NIS超限，检查Q/R或时间同步")说明：NIS在滤波器一致时服从卡方分布，自由度等于观测维数。GPS观测是2维时，95%置信阈值取5.99。长时间运行后若超限比例超过5%，就要回头查时间延迟、单位或者F/H的雅可比是否漏项。这个检查比肉眼看轨迹可靠得多，因为轨迹平滑不等于滤波器一致。
本文还有配套的精品资源，点击获取
