简介：这份资源面向导航算法初学者与从事组合导航开发的工程师，聚焦GPS与IMU融合定位这一经典课题，用仿真方式解决单一传感器定位精度不足的问题。压缩包共3个文件，均为m脚本文件，整体约5KB，涵盖惯性解算、姿态基准与主程序等模块，便于在MATLAB中直接运行与调试。内容围绕卡尔曼滤波的预测与更新两步展开，涉及状态转移矩阵、测量模型与噪声模型等关键环节，可帮助读者理解如何用GPS的远距离高精度信息校正IMU随时间累积的漂移误差，实现优势互补。目前已有652人学习下载，适合自动驾驶、无人机导航与室内定位等方向的学习者参考。通过阅读与运行代码，读者能掌握融合定位的基本流程与参数设置思路，为后续算法改进与工程实践打下基础。

1. GPS-IMU融合定位仿真：从一条飘忽的轨迹说起

如果你把一颗普通的消费级 GPS 模块放在桌上静止不动，打开串口看它输出的经纬度，你会发现坐标并不是钉死在一个点上，而是在几米范围内来回跳动。车在隧道里开，GPS 直接丢星；急转弯时，单靠 GPS 算出来的航向角能偏出几十度。这些现象背后是同一个问题：单一传感器永远有它覆盖不到的盲区。GPS 给的是绝对位置，长期不漂，但短期噪声大、更新率低、遮挡就失效；IMU 给的是加速度和角速度，短期精度高、更新率能到 100Hz 以上，但积分两次之后误差会随时间平方级发散。把两者放进一个卡尔曼滤波器里做融合，就是 GPS-IMU 融合定位要解决的核心问题。这篇笔记面向的是想在自己电脑上把整套仿真跑通、看到轨迹收敛、并且知道每个参数改了会发生什么的工程师，不是讲卡尔曼公式推导的教科书。

2. 仿真环境搭建与传感器数据生成：先让数据动起来

做融合定位仿真，第一步不是写滤波器，而是先把 GPS 和 IMU 的数据造出来。没有数据，滤波器调什么都是玄学。我一般会先写一个轨迹生成器，用一条已知的解析曲线作为真值，然后分别叠加 GPS 噪声和 IMU 噪声，得到两组带误差的观测。这样做的好处是：你手里始终有一条真值轨迹，融合结果好不好，直接和真值比 RMSE 就行，不用猜。

2.1 用解析轨迹生成真值：圆周加直线段

真值轨迹的选择有讲究。纯圆周运动能暴露航向估计的问题，纯直线运动看不出陀螺仪零偏的影响。我习惯用「直线加速—匀速—减速—转弯」的组合，覆盖加减速和角速度变化两种激励。下面这段 Python 代码生成一条二维平面轨迹，同时输出每个时刻的位置、速度、加速度和航向角。

import numpy as np def generate_truth(dt=0.01, T=60.0): """ 生成二维真值轨迹 dt: 采样间隔, IMU 用 100Hz T: 总时长 60 秒 返回: t, pos(N,2), vel(N,2), acc(N,2), yaw(N,) """ t = np.arange(0, T, dt) N = len(t) pos = np.zeros((N, 2)) vel = np.zeros((N, 2)) acc = np.zeros((N, 2)) yaw = np.zeros(N) # 分段: 0-10s 加速, 10-30s 匀速, 30-40s 减速, 40-60s 转弯 v = 0.0 heading = 0.0 x, y = 0.0, 0.0 for i in range(N): if t[i] < 10: a = 1.0 # 1 m/s^2 加速 elif t[i] < 30: a = 0.0 elif t[i] < 40: a = -1.0 else: a = 0.0 heading += 0.02 # 约 1.15 deg/s 转弯 v = max(v + a * dt, 0.0) x += v * np.cos(heading) * dt y += v * np.sin(heading) * dt pos[i] = [x, y] vel[i] = [v * np.cos(heading), v * np.sin(heading)] acc[i] = [a * np.cos(heading), a * np.sin(heading)] yaw[i] = heading return t, pos, vel, acc, yaw

这段代码里dt=0.01对应 IMU 的 100Hz 输出，GPS 通常只有 1Hz 或 10Hz，后面降采样即可。heading在最后 20 秒线性增加，模拟车辆转弯，角速度约 0.02 rad/s，这个量级刚好能让陀螺仪零偏在融合结果里显现出来。真值轨迹生成后，所有传感器数据都从它派生，保证时间对齐。

2.2 给 GPS 加噪声：为什么不能只用高斯白噪声

GPS 误差不是简单的高斯白噪声。实际模块的定位误差包含两部分：一部分是零均值的高斯噪声，另一部分是缓慢变化的偏置，来自电离层、多径和卫星几何构型变化。仿真里如果只加白噪声，卡尔曼滤波器的观测噪声矩阵 R 会设得很小，融合结果好得不像话，一到真实数据就翻车。我的做法是给 GPS 位置叠加「高斯白噪声 + 一阶马尔可夫过程」，后者用下面这个离散递推生成。

def gps_noise(N, dt_gps=1.0, sigma=1.5, tau=30.0): """ 生成 GPS 位置噪声: 白噪声 + 一阶马尔可夫偏置 sigma: 白噪声标准差 (米) tau: 马尔可夫过程相关时间 (秒) """ bias = np.zeros((N, 2)) alpha = np.exp(-dt_gps / tau) for i in range(1, N): bias[i] = alpha * bias[i-1] + np.sqrt(1 - alpha**2) * \ np.random.randn(2) * sigma * 0.5 white = np.random.randn(N, 2) * sigma return white + bias

tau=30表示偏置大约 30 秒变一次，sigma=1.5米对应普通单频 GPS 的水平精度。这两个参数直接决定后面 R 矩阵怎么设。如果你用的仿真数据里 GPS 噪声只有白噪声，R 可以设小；如果有马尔可夫偏置，R 要适当放大，否则滤波器会把偏置当成真实运动跟过去。

2.3 给 IMU 加噪声和零偏：陀螺仪零偏是发散元凶

IMU 误差模型比 GPS 复杂，但仿真里抓住三个关键量就够了：加速度计白噪声、陀螺仪白噪声、陀螺仪零偏。加速度计零偏在水平面上和姿态耦合，仿真初期可以先不加，但陀螺仪零偏必须加，因为航向角误差会直接积分进位置。下面代码在真值角速度上叠加零偏和白噪声。

def imu_measurement(yaw, dt=0.01, gyro_bias=0.005, gyro_noise=0.002): """ 从真值航向生成陀螺仪测量 gyro_bias: 零偏 (rad/s), 0.005 rad/s 约 0.29 deg/s gyro_noise: 白噪声标准差 (rad/s) """ N = len(yaw) omega_true = np.gradient(yaw, dt) omega_meas = omega_true + gyro_bias + np.random.randn(N) * gyro_noise return omega_meas

gyro_bias=0.005 rad/s这个量级意味着：如果你只用陀螺仪积分航向，100 秒后航向误差会累积到 0.5 rad，接近 30 度。这就是为什么纯 IMU 航向推算撑不过一分钟。融合滤波器的任务之一，就是用 GPS 的位置观测把这个零偏估计出来并补偿掉。

3. 卡尔曼滤波器实现：状态量、雅可比与更新顺序

数据有了，接下来是滤波器本身。GPS-IMU 融合最常用的结构是误差状态卡尔曼滤波（ESKF），因为姿态用四元数表示时，误差状态是小量，线性化更准。但如果你只是做二维平面仿真，状态量可以简化成位置、速度、航向和陀螺仪零偏，一共 6 维，实现起来直观，调参也容易看清每一项的作用。

3.1 状态方程离散化：从连续模型到代码

二维平面下，状态向量取x = [px, py, vx, vy, yaw, bias_gyro]。连续时间状态方程里，位置导数是速度，速度导数是旋转后的加速度，航向导数是陀螺仪测量减零偏。离散化用一阶欧拉即可，因为 IMU 采样率 100Hz，步长足够小。

def predict(x, P, acc_meas, gyro_meas, dt, Q): """ 卡尔曼预测步 x: 状态 [px, py, vx, vy, yaw, bias] P: 协方差 6x6 acc_meas: 机体坐标系加速度测量 (2,) gyro_meas: 陀螺仪测量 (标量) """ px, py, vx, vy, yaw, bias = x omega = gyro_meas - bias # 状态转移: 把机体加速度转到世界系 ax_w = acc_meas[0] * np.cos(yaw) - acc_meas[1] * np.sin(yaw) ay_w = acc_meas[0] * np.sin(yaw) + acc_meas[1] * np.cos(yaw) x_pred = np.array([ px + vx * dt, py + vy * dt, vx + ax_w * dt, vy + ay_w * dt, yaw + omega * dt, bias ]) # 雅可比 F = df/dx F = np.eye(6) F[0, 2] = dt F[1, 3] = dt F[2, 4] = -ax_w * dt # 近似: d(ax_w)/d(yaw) 用当前加速度 F[3, 4] = ax_w * dt F[4, 5] = -dt P_pred = F @ P @ F.T + Q return x_pred, P_pred

这里F[2,4]和F[3,4]的符号容易写反，血泪经验是：先把加速度旋转公式写出来，再对 yaw 求导，不要凭记忆填。Q是过程噪声矩阵，通常设成对角阵，位置和速度项小，航向和零偏项稍大，因为这两项模型误差最明显。

3.2 观测更新：GPS 位置怎么进滤波器

GPS 只给位置，所以观测矩阵 H 是 2x6，只取前两个状态。更新步就是标准卡尔曼公式，但要注意新息协方差 S 的条件数，如果 R 设得太小而 P 又很大，S 会接近奇异，求逆时数值不稳定。我一般会在求逆前加一个很小的对角项。

def update(x_pred, P_pred, gps_pos, R): """ 卡尔曼更新步, 观测为 GPS 位置 gps_pos: (2,) 米 R: 2x2 观测噪声协方差 """ H = np.zeros((2, 6)) H[0, 0] = 1.0 H[1, 1] = 1.0 z = gps_pos y = z - H @ x_pred # 新息 S = H @ P_pred @ H.T + R S += np.eye(2) * 1e-9 # 数值保护 K = P_pred @ H.T @ np.linalg.inv(S) x_upd = x_pred + K @ y P_upd = (np.eye(6) - K @ H) @ P_pred return x_upd, P_upd

R的设定直接对应 2.2 节里 GPS 噪声的方差。如果你加了马尔可夫偏置，R 不能只取白噪声方差，要把偏置的稳态方差也加进去，否则滤波器会过度信任 GPS，把偏置也跟进去，轨迹出现低频摆动。

3.3 主循环与时间对齐：GPS 和 IMU 不同频怎么处理

IMU 100Hz，GPS 1Hz，主循环按 IMU 节拍走，每到 GPS 时刻做一次更新。时间对齐用最近邻即可，仿真里时间戳精确，不需要插值。下面主循环把前面几块串起来。

dt = 0.01 T = 60.0 N = int(T / dt) gps_every = 100 # 每 100 个 IMU 步有一次 GPS x = np.array([0, 0, 0, 0, 0, 0.0]) P = np.eye(6) * 1.0 Q = np.diag([0.01, 0.01, 0.1, 0.1, 0.001, 1e-6]) R = np.eye(2) * 2.25 # 对应 sigma=1.5m traj_est = [] for i in range(N): acc_meas = acc[i] + np.random.randn(2) * 0.05 gyro_meas = omega_meas[i] x, P = predict(x, P, acc_meas, gyro_meas, dt, Q) if i % gps_every == 0: gps_pos = pos[i] + gps_noise_series[i // gps_every] x, P = update(x, P, gps_pos, R) traj_est.append(x[:2].copy())

跑完这段，把traj_est和真值pos画在一起，正常情况应该看到融合轨迹紧贴真值，而纯 GPS 点散布在周围。如果融合轨迹反而比 GPS 还差，先检查 F 矩阵的符号和 Q 的量级。

4. 参数调试与避坑：那些让轨迹飞掉的设置

滤波器跑通只是开始，真正花时间的是调参和排查。下面几条是我在仿真里反复踩过的坑，每条都对应一个具体的现象和改法。

4.1 避坑一：Q 矩阵航向项设太小，转弯时轨迹切内角

现象：直线段融合很好，一进转弯段，估计轨迹明显偏向圆心，像抄近道。原因：Q 里航向方差设得太小，滤波器认为航向模型很准，不愿意根据 GPS 位置修正航向，导致转弯时航向跟不上。解决：把 Q 的航向项从 1e-6 提到 1e-3 量级，让滤波器在转弯时更信任 GPS 的位置观测。具体数值取决于你的陀螺仪噪声，一般取陀螺仪白噪声方差乘以 dt 再放大 10 倍左右。

4.2 避坑二：R 设得太小，GPS 多径把轨迹带偏

现象：融合轨迹在 GPS 跳点处出现尖刺，滤波器像被拽了一下。原因：R 设得太小，滤波器过度信任 GPS，一个多径跳点就能把状态拉过去。解决：R 不要低于 GPS 静态测试的实际方差。简单做法是让设备静止采集 5 分钟 GPS，算经纬度换算成米后的方差，直接填进 R。仿真里如果你加了马尔可夫偏置，R 还要再加上偏置的稳态方差。

4.3 避坑三：陀螺仪零偏状态没有可观测性，估计值乱跑

现象：bias 估计值在真值附近来回跳，有时甚至反向。原因：如果轨迹里没有足够的角运动激励，零偏和航向误差耦合在一起，滤波器分不清是零偏错了还是航向错了。解决：仿真轨迹里必须包含转弯段，而且转弯角速度要大于零偏量级。如果实际场景长期直线行驶，零偏估计要单独做静态标定，不要指望滤波器在线估计。

4.4 避坑四：GPS 更新时直接用经纬度，单位不统一

现象：滤波器输出位置数量级完全不对。原因：GPS 给的是经纬度，IMU 积分出来的是米，直接混用会导致观测矩阵和状态量单位不一致。解决：在进入滤波器之前，用局部切平面把经纬度转成东北天坐标，参考点取第一条 GPS 的经纬度。转换公式里纬度每度约 111320 米，经度要乘 cos(纬度)。这一步不做，后面所有调参都是白费。

4.5 避坑五：协方差矩阵失去正定性，滤波器发散

现象：跑一段时间后 P 矩阵出现负特征值，状态估计飞掉。原因：浮点误差累积，尤其是用(I - K H) P这种非对称形式更新时。解决：改用 Joseph 形式更新协方差，或者每次更新后强制对称化P = (P + P.T) / 2。仿真里步数多，这个操作成本很低，但能避免大部分数值发散。

5. 进阶技巧：用新息序列判断滤波器是否健康

滤波器调完之后，怎么知道它工作在正常状态？我的习惯是看新息序列，也就是每次 GPS 更新时的y = z - H x_pred。如果滤波器健康，新息应该是零均值白噪声，协方差接近S = H P H.T + R。如果新息出现低频摆动，说明 Q 或 R 不匹配；如果新息方差远大于 S，说明滤波器过度自信，P 设小了。

具体做法是：跑完仿真后，把每次 GPS 更新的新息存下来，画两张图。第一张是新息随时间的变化，正常应该围绕零线随机跳动，没有趋势。第二张是新息的直方图，叠加理论高斯曲线，看是否吻合。下面代码计算归一化新息平方（NIS），并给出卡方检验的阈值判断。

nis_list = [] for each gps update: y = z - H @ x_pred S = H @ P_pred @ H.T + R nis = y.T @ np.linalg.inv(S) @ y nis_list.append(nis) # 2 维观测, 95% 置信区间对应卡方阈值约 5.99 outliers = [n for n in nis_list if n > 5.99] print(f"NIS 超限比例: {len(outliers)/len(nis_list):.2%}")

如果超限比例远大于 5%，说明 R 设小了或者 Q 设大了，滤波器对新息过于敏感。如果超限比例接近零，说明 R 设大了，滤波器反应迟钝，融合结果会滞后于真值。这个指标比单看轨迹 RMSE 更能反映滤波器内部是否自洽。

另一个进阶方向是把 GPS 的速度观测也加进来。很多 GPS 模块除了位置还输出地速和航向，虽然精度不如位置，但在低速时对速度状态有约束。加的时候把 H 矩阵扩成 4x6，R 扩成 4x4，注意 GPS 航向在静止时不可信，要加一个速度阈值判断，低于 0.5 m/s 时把航向观测的 R 设成无穷大，相当于丢弃。

我自己的习惯是：每次改完 Q 或 R，先跑 NIS 检验，再看轨迹 RMSE，两个都过了才认为这组参数可用。仿真里多花十分钟做这个检查，比后面在真实数据上反复试凑省几个小时。希望帮到你。

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