简介这份资源面向导航算法初学者与从事组合导航开发的工程师聚焦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(dt0.01, T60.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这段代码里dt0.01对应 IMU 的 100Hz 输出GPS 通常只有 1Hz 或 10Hz后面降采样即可。heading在最后 20 秒线性增加模拟车辆转弯角速度约 0.02 rad/s这个量级刚好能让陀螺仪零偏在融合结果里显现出来。真值轨迹生成后所有传感器数据都从它派生保证时间对齐。2.2 给 GPS 加噪声为什么不能只用高斯白噪声GPS 误差不是简单的高斯白噪声。实际模块的定位误差包含两部分一部分是零均值的高斯噪声另一部分是缓慢变化的偏置来自电离层、多径和卫星几何构型变化。仿真里如果只加白噪声卡尔曼滤波器的观测噪声矩阵 R 会设得很小融合结果好得不像话一到真实数据就翻车。我的做法是给 GPS 位置叠加「高斯白噪声 一阶马尔可夫过程」后者用下面这个离散递推生成。def gps_noise(N, dt_gps1.0, sigma1.5, tau30.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 biastau30表示偏置大约 30 秒变一次sigma1.5米对应普通单频 GPS 的水平精度。这两个参数直接决定后面 R 矩阵怎么设。如果你用的仿真数据里 GPS 噪声只有白噪声R 可以设小如果有马尔可夫偏置R 要适当放大否则滤波器会把偏置当成真实运动跟过去。2.3 给 IMU 加噪声和零偏陀螺仪零偏是发散元凶IMU 误差模型比 GPS 复杂但仿真里抓住三个关键量就够了加速度计白噪声、陀螺仪白噪声、陀螺仪零偏。加速度计零偏在水平面上和姿态耦合仿真初期可以先不加但陀螺仪零偏必须加因为航向角误差会直接积分进位置。下面代码在真值角速度上叠加零偏和白噪声。def imu_measurement(yaw, dt0.01, gyro_bias0.005, gyro_noise0.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_measgyro_bias0.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_updR的设定直接对应 2.2 节里 GPS 噪声的方差。如果你加了马尔可夫偏置R 不能只取白噪声方差要把偏置的稳态方差也加进去否则滤波器会过度信任 GPS把偏置也跟进去轨迹出现低频摆动。3.3 主循环与时间对齐GPS 和 IMU 不同频怎么处理IMU 100HzGPS 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 # 对应 sigma1.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(fNIS 超限比例: {len(outliers)/len(nis_list):.2%})如果超限比例远大于 5%说明 R 设小了或者 Q 设大了滤波器对新息过于敏感。如果超限比例接近零说明 R 设大了滤波器反应迟钝融合结果会滞后于真值。这个指标比单看轨迹 RMSE 更能反映滤波器内部是否自洽。另一个进阶方向是把 GPS 的速度观测也加进来。很多 GPS 模块除了位置还输出地速和航向虽然精度不如位置但在低速时对速度状态有约束。加的时候把 H 矩阵扩成 4x6R 扩成 4x4注意 GPS 航向在静止时不可信要加一个速度阈值判断低于 0.5 m/s 时把航向观测的 R 设成无穷大相当于丢弃。我自己的习惯是每次改完 Q 或 R先跑 NIS 检验再看轨迹 RMSE两个都过了才认为这组参数可用。仿真里多花十分钟做这个检查比后面在真实数据上反复试凑省几个小时。希望帮到你。本文还有配套的精品资源点击获取