简介INS_EKF-master压缩包聚焦惯性导航与组合导航中的扩展卡尔曼滤波EKF实现适合导航算法学习者、嵌入式开发者和自动驾驶/无人机方向的工程师用来理解INS/GPS松组合流程。包内共19个文件其中8个cpp源文件承载加速度计、陀螺仪、磁力计、GPS模块与EKF核心算法实现8个h头文件提供对应接口声明另含Makefile构建脚本和README说明整体仅21KB结构简洁便于快速阅读。目前已有468人学习下载。通过该工程可直观对照代码梳理EKF状态预测、量测更新与协方差递推的完整写法掌握惯性器件与卫星观测融合时的接口设计和参数调整思路。在此基础上还能灵活添加新的传感器模型或滤波策略作为组合导航入门与EKF工程实践的紧凑参考。1. 拿到 INS_EKF 组合导航代码后先别急着跑导航工程里最常见的尴尬GNSS 信号一进立交桥就掉IMU 单独积分三分钟就能偏出半个车道。惯组加卫导组合导航的做法是把 IMU 的高频递推和卫导的低频绝对修正放进同一个状态估计环路用扩展卡尔曼滤波EKF把姿态、速度、位置和传感器零偏一起估出来。INS_EKF 这类开源包值得逐行走读因为它把器件误差建模、状态方程离散化、观测更新、协方差反馈串成一条可复现链路。这里不去替某个压缩包里的文件做注释而是顺着这个标题背后的标准套路把状态方程、EKF 循环、最小可运行实现和调参排错关键讲清楚。适合车载定位、无人机飞控、机器人导航和传感器融合的工程师。2. 组合导航状态方程先定义 INS_EKF 要估的 15 个状态2.1 为什么是 15 维而不是 9 维做 INS_EKF 第一步不是写卡尔曼循环而是先回答状态空间里放什么。惯性导航的机械编排递推的是姿态、速度、位置三个 3 维向量一共 9 个量。如果 EKF 只估这 9 个陀螺零偏和加速度计零偏就没有去处它们会反复出现在卡尔曼残差里位置误差以振荡方式越拉越大。标准做法是把 9 个导航误差量加上 6 个器件误差量一起放进状态向量形成 15 维误差状态。器件零偏在日常车载 MEMS 里根本不是常数它随温度和工作时间缓慢变化EKF 的价值之一就是在线跟踪这些零偏让惯导递推在两次 GNSS 修正之间不漂得太快。这里有个容易混淆的点松组合里 GNSS 输出解算好的位置速度观测方程是线性的紧组合里 GNSS 输出伪距、伪距率位置速度被藏在观测函数内部需要把钟差钟漂也加进状态变成 17 维或 19 维。INS_EKF 包大多默认做松组合先把 15 维状态搞清楚再谈升维到紧组合。下表是 15 维状态统一的排布方式程序里几乎所有矩阵的索引都按这个顺序写。索引区间状态符号单位说明0:3位置误差δpmENU 导航系下的位置偏差3:6速度误差δvm/sENU 导航系下的速度偏差6:9姿态失准角φrad从数学平台到真实平台的小角度误差9:12陀螺零偏εrad/s 或 °/h陀螺常值漂移12:15加计零偏∇m/s²加速度计偏置注意单位问题陀螺零偏如果源码里用 °/hEKF 矩阵内全部要换算到 rad/s否则状态更新的量纲错配会让零偏估计变成不收敛的锯齿。很多二次开发卡壳都卡在单位换算而不是算法本身。2.2 状态转移矩阵 F 与离散化连续时间的误差状态方程写成 x_dot F x G w。F 由导航方程对各状态求偏导得到工程上很少有人完整展开 15×15 的符号推导一般写成块状稀疏矩阵位置误差对速度误差的一阶导是单位阵速度误差对失准角的一阶导是比力的反对称阵失准角的自耦合来自地球自转与载体角速度陀螺零偏和加计零偏按随机游走建模对应行为 0。下面代码展示这种块状构造方式。import numpy as np def skew(v): 构造三维向量的反对称矩阵用于叉乘运算 return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) def build_F(Cbn, fn, wien): F np.zeros((15, 15)) I3 np.eye(3) F[0:3, 3:6] I3 # 位置误差对速度误差的导数 F[3:6, 6:9] -skew(fn) # 速度误差对失准角的导数 F[3:6, 12:15] Cbn # 加计零偏映射到速度误差 F[6:9, 6:9] -skew(wien) # 失准角的旋转自耦合 F[6:9, 9:12] -Cbn # 陀螺零偏映射到失准角 return F def discretize(F, dt): 二阶 Taylor 离散dt 为惯导更新周期 I np.eye(15) Fdt F * dt return I Fdt 0.5 * Fdt Fdt逻辑说明skew 把向量变成反对称矩阵对应物理上的角速度叉乘比力或姿态矩阵变换。build_F 里的 Cbn 是机体坐标系到导航坐标系的姿态矩阵fn 是导航系下的比力wien 是导航系下的地球自转角速度折算到当地水平的量。discretize 用二阶 Taylor 展开把连续 F 转成离散 Phi对 100Hz 的 MEMS 数据足够一阶 IF*dt 在快速旋转场景下误差偏大三阶以上对中低精度器件收益不明显。需要提醒的是这个 F 是误差方程的主导项真正的完整方程还包含重力、曲率半径、科里奥利项。定位精度到米级的松组合上面这些块是主要的做 RTK 厘米级或高动态机动场景需要把重力扰动和传输速率项补回来否则高速转弯时估计会滞后。2.3 观测方程位置速度观测如何装进 H 矩阵松组合的观测向量是 GNSS 位置速度与惯导位置速度的差值。惯导递推过程维护的是名义值EKF 只估误差状态因此观测方程在这一步是线性的z H x v。H 是 6×15 或 3×15 的稀疏矩阵。位置观测对应前三行相应列为单位阵速度观测对应后三行相应列为单位阵其余为 0。若只有位置观测H 写为 3×15加速度观测后扩成 6×15。测量噪声协方差 R 与观测维度对应GNSS 的位置误差和速度多普勒误差通常取不同值。某些代码会把高度通道单独加权因为气压测高和椭球高程的误差模型与平面不同H 不变但 R 的高度项可以单独调大这是组合导航里最早会遇到的一处实用调参。紧组合则完全不同观测函数变成伪距和伪距率的非线性函数H 是每次更新前算出的雅可比矩阵。看到源码里状态是 15 维且观测是位置速度差值就按上面的 H 理解这就覆盖了松组合 INS_EKF 的绝大多数实现。3. EKF 循环组合导航预测、更新与噪声矩阵设置3.1 预测步协方差如何传播到下一拍误差状态下的 EKF 分两步。预测步把离散状态转移阵和过程噪声代入P_pred Phi P Phi.T Q。P 是估计误差协方差Q 是过程噪声协方差两者都必须与 15 个状态一一对应任何一行量纲不对滤波结果都会失真。Q 矩阵通常按分块对角构造五块分别对应位置、速度、姿态、陀螺零偏、加计零偏。位置过程噪声来自机动模型速度过程噪声来自比力误差姿态过程噪声来自陀螺白噪声零偏噪声来自 Alla 方差分析中的随机游走系数。如果 Q 取太小滤波过度相信自身模型增益趋近 0GNSS 观测被忽略Q 取太大估计会跟着观测噪声跳轨迹毛刺明显。最常见的错误是直接把 Q 设成单位阵乘一个数单位不对会让位置和姿态互相干扰。3.2 更新步增益、后验与开环/闭环更新步先算新息协方差 S H P_pred H.T R再算卡尔曼增益 K P_pred H.T inv(S)状态修正 x_new x_pred K (z - H x_pred)最后更新协方差 P_new (I - K H) P_pred。更新之后有个必须做的决定误差状态要不要回灌到惯导名义值。开环模式里 EKF 只输出估计值但不修正递推路径适合事后分析和器件评估闭环模式把失准角和零偏反馈到姿态、速度、位置及下一次积分的补偿项惯导递推时刻保持水平对准误差不积累。工程上的 INS_EKF 基本都走闭环否则跑几分钟后误差状态涨到线性化假设失效。闭环实现有个易漏的步骤修正完要把误差状态 x 置零协方差 P 保留继续传。如果忘了置零会看到一个隐蔽 bug——观测残差越来越小看上去在收敛实际是误差被重复叠加轨迹缓慢漂移。import numpy as np def ekf_predict(Phi, x, P, Q): 误差状态预测状态和协方差各自前传 x_pred Phi x P_pred Phi P Phi.T Q return x_pred, P_pred def ekf_correct(x_pred, P_pred, z, H, R): 标准 EKF 更新返回修正状态、协方差和残差 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) innovation z - H x_pred x_new x_pred K innovation P_new (np.eye(x_new.shape[0]) - K H) P_pred return x_new, P_new, innovation, S参数说明Phi 是离散状态转移阵x 是 15 维误差状态P 是先验协方差Q 是过程噪声z 是 GNSS 位置速度减去惯导位置速度的新息H 是量测矩阵R 是量测噪声。np.linalg.inv 在松组合 6 维观测下没有压力如果后面迁移到紧组合观测维数上涨建议换成 np.linalg.solve(S, H P_pred.T).T 求 K数值稳定性更好。P_new 用标准形式即可若要更强数值保证可以改写成 Joseph 形式代价是多几次矩阵乘法。3.3 噪声参数从哪里来Allan 方差与起始值Q 和 R 的取值是 INS_EKF 里对结果影响最大的一步。严格做法是用 Allan 方差分析 IMU 静态数据得到角度随机游走、速度随机游走、零偏不稳定性等系数再按离散时间换算成 Q快速做法是先给一组量级正确的初值再对着实测轨迹调增益。下面是一组车载 MEMS 层面的常见起始值实际器件量级可能差几十倍但量纲方向不会错。矩阵对应状态建议起始值备注Q位置过程噪声1e-6 m²/s机动部分量级很小Q速度过程噪声1e-2 (m/s)²/s由比力误差决定Q姿态过程噪声1e-6 rad²/s由陀螺白噪声决定Q陀螺零偏噪声(0.01 °/h)²/s来自 Allan 随机游走段Q加计零偏噪声(10 μg)²/s来自 Allan 随机游走段RGNSS 位置(1 m)² 对角RTK 可压到 (0.02 m)²RGNSS 速度(0.1 m/s)² 对角多普勒测速精度调参顺序我一般会固定 R 为厂商标称精度先把 Q 的姿态噪声和零偏噪声调大让滤波在直线段不抖、弯道不滞后。如果 GNSS 中断后恢复时轨迹出现跳变说明位置或速度过程噪声偏小静止时位置还在持续漂多半是 Q 的零偏项与 R 的比例失配陀螺零偏没有在有效学习。4. INS_EKF 程序落地IMU 与 GNSS 数据融合的最小实现4.1 时间对齐与杆臂补偿比滤波更早的坑IMU 高频100~200Hz和 GNSS 低频1~10Hz时间戳往往不同步。最稳的处理是维护一条 IMU 队列当队列最后一条时间戳越过 GNSS 观测时刻时取前后两条 IMU 做线性插值得到 GNSS 时刻的惯导名义值。千万不要简单用最新 GNSS 配当前 IMU的写法几毫秒错配在高动态下会形成几十厘米残差而且很难和噪声区分。杆臂补偿是另一个高频出错点。GNSS 天线装在车顶IMU 在车内两者之间存在固定力臂。GNSS 速度是天线处的速度要减去杆臂旋转速度才是 IMU 处速度v_imu v_gnss - ω × lω 是载体角速度l 是杆臂向量。忽略这一项转弯时会出现周期性横向偏差航向误差随转向来回振荡很多人误以为是陀螺零偏没调好。对比项松组合紧组合观测输入GNSS 位置速度伪距、伪距率原始量测状态维度15 维17 维及以上含钟差钟漂观测方程线性 H可预计算非线性需每步算雅可比卫星需求需 4 颗以上输出定位结果少于 4 颗仍可部分约束典型场景开阔道路、低速平台城市峡谷、遮挡环境在城市道路做车载定位建议第一步先做松组合跑通主循环后再评估是否升级到紧组合。4.2 主循环机械编排与 EKF 修正交替最小可运行的组合导航循环由三部分组成惯导机械编排、时间配对、EKF 修正反馈。机械编排负责每帧 IMU 的姿态速度位置递推EKF 在 GNSS 数据到达时做一次修正。下面的循环是一个可迁移到工程代码的骨架。t 0.0 x np.zeros(15) # 误差状态闭环修正后置零 P build_P0() # 初始协方差对角阵量级见 3.3 表 while t t_end: imu next_imu() # 取下一帧 IMU dt imu.t - t # 递推步长 att, vel, pos ins_mechanization(att, vel, pos, imu, dt) if gnss_available(imu.t): # 对 GNSS 观测时刻做 IMU 插值得到同一时刻的名义导航值 p_gnss, v_gnss interpolate_gnss(imu.t) lever_corrected_v v_gnss - rotate(att, lever_arm) z np.concatenate([p_gnss - pos, lever_corrected_v - vel]) H build_H(att) # 松组合 H 与姿态无关也可预计算 x, P ekf_correct(x, P, z, H, R) # 闭环反馈把误差估计写回导航值然后清空误差状态 att, vel, pos apply_correction(att, vel, pos, x) x np.zeros(15) # 误差状态归零协方差 P 不清 t imu.t逻辑说明ins_mechanization 完成姿态四元数更新、比力投影、速度和位置积分对应最耗时的部分gnss_available 判断队列中是否有观测时刻落在当前时间附近。z 的构造顺序必须与 H 的行一一对应这里是先位置后速度apply_correction 把失准角转化为姿态修正把零偏反馈到下一步递推前的原数据补偿。误差状态置零后下一次 GNSS 观测残差反映的是新误差不会累积叠加。P0 的初值也要认真给。姿态失准角初值在主方向上有 1° 量级不确定度就对应约 3e-4 rad 的方差速度 0.1 m/s位置 1 m零偏按 3.3 表近似。P0 过小会让滤波器以为初始对准很准前几百秒修正量偏小P0 过大则开始阶段噪声偏大但会在几十秒内自动收敛。4.3 初始对准姿态零偏从哪里来静止时水平姿态用加速度计反算roll atan2(fy, fz)pitch atan2(-fx, sqrt(fy² fz²))fx、fy、fz 是加速度计在机体坐标系下的读数。航向没有绝对参照可以用 GNSS 速度方位yaw atan2(Ve, Vn)注意单位是弧度且要判断象限。如果车辆起步前没有 GNSS 速度航向只能先给一个粗略初值等第一次直线运动后让 EKF 把它拉正这称为动基座对准。很多工程包把对准状态单独做成一个状态机静止段做加计水平对准运动段做速度航向对准进入 EKF 主循环后才允许闭环反馈。初始对准不做直接进 EKF 会让零偏估计和姿态误差纠缠在一起前后几百秒的零偏曲线是耦合的。4.4 结果验证先看图再调参最直观的验证是画两条轨迹GNSS 原始轨迹和融合轨迹。融合轨迹应比 GNSS 更平滑、在 GNSS 中断段呈惯导外推的形状恢复后无大跳变。第二张图看 EKF 估计的陀螺零偏和加计零偏正常应在几百秒内从初值收敛到稳定区间并在小范围波动如果零偏曲线像斜坡一样单调爬升说明观测更新没能有效约束它多半是 R 太大或 H 缺了对应行。残差白噪声检验可以放在后面做先看轨迹重合度能快速筛掉大部分实现错误。轨迹偏差呈现固定方向偏移且随时间增长检查是否漏了杆臂补偿呈现锯齿状跳变检查时间对齐。5. INS_EKF 调参验证协方差、发散排查与紧组合升级方向5.1 用 NIS 做在线可信度检查新息向量 v z - H x_pred 里面藏着滤波器健康度。它的协方差 S H P_pred H.T R如果模型正确统计量 NIS v.T inv(S) v 应服从自由度等于观测维数的卡方分布。松组合位置速度双观测时自由度为 695% 分位数约 12.59仅位置观测自由度为 3分位数约 7.81。v innovation S H P_pred H.T R nis float(v.T np.linalg.inv(S) v) # 连续出现 NIS 远超阈值说明 Q 偏小或 H/R 不匹配这个指标可以做成滑动窗口均值替代肉眼盯残差。NIS 一直偏大优先放大 Q 中对应维度NIS 偏小但轨迹抖说明滤波器过度信任观测R 应调小。5.2 发散排查从时间戳到量纲EKF 发散时不要立刻调 Q。我一般按这个顺序排查先确认时间戳配对错配会造成周期性残差再查量纲角度用度还是弧度、速度用 m/s 还是 km/h这是最常见的一类发散源头然后核对杆臂向量方向它在导航系还是机体系符号对不对最后查 P0 和 Q 的量级确保与 15 维状态对齐。全部检查完再考虑算法层面的问题比如是否忘了闭环置零、协方差是否被重复初值化。5.3 松组合的边界与紧组合的升级条件松组合在 GNSS 正常输出时性能足够但卫星数降到 4 颗以下或伪距断续时松组合直接失效。紧组合用原始伪距和伪距率做观测即使只有 2~3 颗可见星也能形成对位置的弱约束城市峡谷场景收益明显。升级紧组合要把状态扩到 17 维以上、观测函数改成非线性、雅可比按当前估计位置计算工程改动量约是松组合的 2~3 倍。判断的依据很简单如果测试路线里频繁出现 GNSS 定位丢失或跳变且业务对连续性要求高就值得从松组合迁到紧组合如果开阔场地占比高松组合加良好调参已足够稳定输出。本文还有配套的精品资源点击获取
INS_EKF组合导航实战：从状态方程到调参排错要点
简介INS_EKF-master压缩包聚焦惯性导航与组合导航中的扩展卡尔曼滤波EKF实现适合导航算法学习者、嵌入式开发者和自动驾驶/无人机方向的工程师用来理解INS/GPS松组合流程。包内共19个文件其中8个cpp源文件承载加速度计、陀螺仪、磁力计、GPS模块与EKF核心算法实现8个h头文件提供对应接口声明另含Makefile构建脚本和README说明整体仅21KB结构简洁便于快速阅读。目前已有468人学习下载。通过该工程可直观对照代码梳理EKF状态预测、量测更新与协方差递推的完整写法掌握惯性器件与卫星观测融合时的接口设计和参数调整思路。在此基础上还能灵活添加新的传感器模型或滤波策略作为组合导航入门与EKF工程实践的紧凑参考。1. 拿到 INS_EKF 组合导航代码后先别急着跑导航工程里最常见的尴尬GNSS 信号一进立交桥就掉IMU 单独积分三分钟就能偏出半个车道。惯组加卫导组合导航的做法是把 IMU 的高频递推和卫导的低频绝对修正放进同一个状态估计环路用扩展卡尔曼滤波EKF把姿态、速度、位置和传感器零偏一起估出来。INS_EKF 这类开源包值得逐行走读因为它把器件误差建模、状态方程离散化、观测更新、协方差反馈串成一条可复现链路。这里不去替某个压缩包里的文件做注释而是顺着这个标题背后的标准套路把状态方程、EKF 循环、最小可运行实现和调参排错关键讲清楚。适合车载定位、无人机飞控、机器人导航和传感器融合的工程师。2. 组合导航状态方程先定义 INS_EKF 要估的 15 个状态2.1 为什么是 15 维而不是 9 维做 INS_EKF 第一步不是写卡尔曼循环而是先回答状态空间里放什么。惯性导航的机械编排递推的是姿态、速度、位置三个 3 维向量一共 9 个量。如果 EKF 只估这 9 个陀螺零偏和加速度计零偏就没有去处它们会反复出现在卡尔曼残差里位置误差以振荡方式越拉越大。标准做法是把 9 个导航误差量加上 6 个器件误差量一起放进状态向量形成 15 维误差状态。器件零偏在日常车载 MEMS 里根本不是常数它随温度和工作时间缓慢变化EKF 的价值之一就是在线跟踪这些零偏让惯导递推在两次 GNSS 修正之间不漂得太快。这里有个容易混淆的点松组合里 GNSS 输出解算好的位置速度观测方程是线性的紧组合里 GNSS 输出伪距、伪距率位置速度被藏在观测函数内部需要把钟差钟漂也加进状态变成 17 维或 19 维。INS_EKF 包大多默认做松组合先把 15 维状态搞清楚再谈升维到紧组合。下表是 15 维状态统一的排布方式程序里几乎所有矩阵的索引都按这个顺序写。索引区间状态符号单位说明0:3位置误差δpmENU 导航系下的位置偏差3:6速度误差δvm/sENU 导航系下的速度偏差6:9姿态失准角φrad从数学平台到真实平台的小角度误差9:12陀螺零偏εrad/s 或 °/h陀螺常值漂移12:15加计零偏∇m/s²加速度计偏置注意单位问题陀螺零偏如果源码里用 °/hEKF 矩阵内全部要换算到 rad/s否则状态更新的量纲错配会让零偏估计变成不收敛的锯齿。很多二次开发卡壳都卡在单位换算而不是算法本身。2.2 状态转移矩阵 F 与离散化连续时间的误差状态方程写成 x_dot F x G w。F 由导航方程对各状态求偏导得到工程上很少有人完整展开 15×15 的符号推导一般写成块状稀疏矩阵位置误差对速度误差的一阶导是单位阵速度误差对失准角的一阶导是比力的反对称阵失准角的自耦合来自地球自转与载体角速度陀螺零偏和加计零偏按随机游走建模对应行为 0。下面代码展示这种块状构造方式。import numpy as np def skew(v): 构造三维向量的反对称矩阵用于叉乘运算 return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) def build_F(Cbn, fn, wien): F np.zeros((15, 15)) I3 np.eye(3) F[0:3, 3:6] I3 # 位置误差对速度误差的导数 F[3:6, 6:9] -skew(fn) # 速度误差对失准角的导数 F[3:6, 12:15] Cbn # 加计零偏映射到速度误差 F[6:9, 6:9] -skew(wien) # 失准角的旋转自耦合 F[6:9, 9:12] -Cbn # 陀螺零偏映射到失准角 return F def discretize(F, dt): 二阶 Taylor 离散dt 为惯导更新周期 I np.eye(15) Fdt F * dt return I Fdt 0.5 * Fdt Fdt逻辑说明skew 把向量变成反对称矩阵对应物理上的角速度叉乘比力或姿态矩阵变换。build_F 里的 Cbn 是机体坐标系到导航坐标系的姿态矩阵fn 是导航系下的比力wien 是导航系下的地球自转角速度折算到当地水平的量。discretize 用二阶 Taylor 展开把连续 F 转成离散 Phi对 100Hz 的 MEMS 数据足够一阶 IF*dt 在快速旋转场景下误差偏大三阶以上对中低精度器件收益不明显。需要提醒的是这个 F 是误差方程的主导项真正的完整方程还包含重力、曲率半径、科里奥利项。定位精度到米级的松组合上面这些块是主要的做 RTK 厘米级或高动态机动场景需要把重力扰动和传输速率项补回来否则高速转弯时估计会滞后。2.3 观测方程位置速度观测如何装进 H 矩阵松组合的观测向量是 GNSS 位置速度与惯导位置速度的差值。惯导递推过程维护的是名义值EKF 只估误差状态因此观测方程在这一步是线性的z H x v。H 是 6×15 或 3×15 的稀疏矩阵。位置观测对应前三行相应列为单位阵速度观测对应后三行相应列为单位阵其余为 0。若只有位置观测H 写为 3×15加速度观测后扩成 6×15。测量噪声协方差 R 与观测维度对应GNSS 的位置误差和速度多普勒误差通常取不同值。某些代码会把高度通道单独加权因为气压测高和椭球高程的误差模型与平面不同H 不变但 R 的高度项可以单独调大这是组合导航里最早会遇到的一处实用调参。紧组合则完全不同观测函数变成伪距和伪距率的非线性函数H 是每次更新前算出的雅可比矩阵。看到源码里状态是 15 维且观测是位置速度差值就按上面的 H 理解这就覆盖了松组合 INS_EKF 的绝大多数实现。3. EKF 循环组合导航预测、更新与噪声矩阵设置3.1 预测步协方差如何传播到下一拍误差状态下的 EKF 分两步。预测步把离散状态转移阵和过程噪声代入P_pred Phi P Phi.T Q。P 是估计误差协方差Q 是过程噪声协方差两者都必须与 15 个状态一一对应任何一行量纲不对滤波结果都会失真。Q 矩阵通常按分块对角构造五块分别对应位置、速度、姿态、陀螺零偏、加计零偏。位置过程噪声来自机动模型速度过程噪声来自比力误差姿态过程噪声来自陀螺白噪声零偏噪声来自 Alla 方差分析中的随机游走系数。如果 Q 取太小滤波过度相信自身模型增益趋近 0GNSS 观测被忽略Q 取太大估计会跟着观测噪声跳轨迹毛刺明显。最常见的错误是直接把 Q 设成单位阵乘一个数单位不对会让位置和姿态互相干扰。3.2 更新步增益、后验与开环/闭环更新步先算新息协方差 S H P_pred H.T R再算卡尔曼增益 K P_pred H.T inv(S)状态修正 x_new x_pred K (z - H x_pred)最后更新协方差 P_new (I - K H) P_pred。更新之后有个必须做的决定误差状态要不要回灌到惯导名义值。开环模式里 EKF 只输出估计值但不修正递推路径适合事后分析和器件评估闭环模式把失准角和零偏反馈到姿态、速度、位置及下一次积分的补偿项惯导递推时刻保持水平对准误差不积累。工程上的 INS_EKF 基本都走闭环否则跑几分钟后误差状态涨到线性化假设失效。闭环实现有个易漏的步骤修正完要把误差状态 x 置零协方差 P 保留继续传。如果忘了置零会看到一个隐蔽 bug——观测残差越来越小看上去在收敛实际是误差被重复叠加轨迹缓慢漂移。import numpy as np def ekf_predict(Phi, x, P, Q): 误差状态预测状态和协方差各自前传 x_pred Phi x P_pred Phi P Phi.T Q return x_pred, P_pred def ekf_correct(x_pred, P_pred, z, H, R): 标准 EKF 更新返回修正状态、协方差和残差 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) innovation z - H x_pred x_new x_pred K innovation P_new (np.eye(x_new.shape[0]) - K H) P_pred return x_new, P_new, innovation, S参数说明Phi 是离散状态转移阵x 是 15 维误差状态P 是先验协方差Q 是过程噪声z 是 GNSS 位置速度减去惯导位置速度的新息H 是量测矩阵R 是量测噪声。np.linalg.inv 在松组合 6 维观测下没有压力如果后面迁移到紧组合观测维数上涨建议换成 np.linalg.solve(S, H P_pred.T).T 求 K数值稳定性更好。P_new 用标准形式即可若要更强数值保证可以改写成 Joseph 形式代价是多几次矩阵乘法。3.3 噪声参数从哪里来Allan 方差与起始值Q 和 R 的取值是 INS_EKF 里对结果影响最大的一步。严格做法是用 Allan 方差分析 IMU 静态数据得到角度随机游走、速度随机游走、零偏不稳定性等系数再按离散时间换算成 Q快速做法是先给一组量级正确的初值再对着实测轨迹调增益。下面是一组车载 MEMS 层面的常见起始值实际器件量级可能差几十倍但量纲方向不会错。矩阵对应状态建议起始值备注Q位置过程噪声1e-6 m²/s机动部分量级很小Q速度过程噪声1e-2 (m/s)²/s由比力误差决定Q姿态过程噪声1e-6 rad²/s由陀螺白噪声决定Q陀螺零偏噪声(0.01 °/h)²/s来自 Allan 随机游走段Q加计零偏噪声(10 μg)²/s来自 Allan 随机游走段RGNSS 位置(1 m)² 对角RTK 可压到 (0.02 m)²RGNSS 速度(0.1 m/s)² 对角多普勒测速精度调参顺序我一般会固定 R 为厂商标称精度先把 Q 的姿态噪声和零偏噪声调大让滤波在直线段不抖、弯道不滞后。如果 GNSS 中断后恢复时轨迹出现跳变说明位置或速度过程噪声偏小静止时位置还在持续漂多半是 Q 的零偏项与 R 的比例失配陀螺零偏没有在有效学习。4. INS_EKF 程序落地IMU 与 GNSS 数据融合的最小实现4.1 时间对齐与杆臂补偿比滤波更早的坑IMU 高频100~200Hz和 GNSS 低频1~10Hz时间戳往往不同步。最稳的处理是维护一条 IMU 队列当队列最后一条时间戳越过 GNSS 观测时刻时取前后两条 IMU 做线性插值得到 GNSS 时刻的惯导名义值。千万不要简单用最新 GNSS 配当前 IMU的写法几毫秒错配在高动态下会形成几十厘米残差而且很难和噪声区分。杆臂补偿是另一个高频出错点。GNSS 天线装在车顶IMU 在车内两者之间存在固定力臂。GNSS 速度是天线处的速度要减去杆臂旋转速度才是 IMU 处速度v_imu v_gnss - ω × lω 是载体角速度l 是杆臂向量。忽略这一项转弯时会出现周期性横向偏差航向误差随转向来回振荡很多人误以为是陀螺零偏没调好。对比项松组合紧组合观测输入GNSS 位置速度伪距、伪距率原始量测状态维度15 维17 维及以上含钟差钟漂观测方程线性 H可预计算非线性需每步算雅可比卫星需求需 4 颗以上输出定位结果少于 4 颗仍可部分约束典型场景开阔道路、低速平台城市峡谷、遮挡环境在城市道路做车载定位建议第一步先做松组合跑通主循环后再评估是否升级到紧组合。4.2 主循环机械编排与 EKF 修正交替最小可运行的组合导航循环由三部分组成惯导机械编排、时间配对、EKF 修正反馈。机械编排负责每帧 IMU 的姿态速度位置递推EKF 在 GNSS 数据到达时做一次修正。下面的循环是一个可迁移到工程代码的骨架。t 0.0 x np.zeros(15) # 误差状态闭环修正后置零 P build_P0() # 初始协方差对角阵量级见 3.3 表 while t t_end: imu next_imu() # 取下一帧 IMU dt imu.t - t # 递推步长 att, vel, pos ins_mechanization(att, vel, pos, imu, dt) if gnss_available(imu.t): # 对 GNSS 观测时刻做 IMU 插值得到同一时刻的名义导航值 p_gnss, v_gnss interpolate_gnss(imu.t) lever_corrected_v v_gnss - rotate(att, lever_arm) z np.concatenate([p_gnss - pos, lever_corrected_v - vel]) H build_H(att) # 松组合 H 与姿态无关也可预计算 x, P ekf_correct(x, P, z, H, R) # 闭环反馈把误差估计写回导航值然后清空误差状态 att, vel, pos apply_correction(att, vel, pos, x) x np.zeros(15) # 误差状态归零协方差 P 不清 t imu.t逻辑说明ins_mechanization 完成姿态四元数更新、比力投影、速度和位置积分对应最耗时的部分gnss_available 判断队列中是否有观测时刻落在当前时间附近。z 的构造顺序必须与 H 的行一一对应这里是先位置后速度apply_correction 把失准角转化为姿态修正把零偏反馈到下一步递推前的原数据补偿。误差状态置零后下一次 GNSS 观测残差反映的是新误差不会累积叠加。P0 的初值也要认真给。姿态失准角初值在主方向上有 1° 量级不确定度就对应约 3e-4 rad 的方差速度 0.1 m/s位置 1 m零偏按 3.3 表近似。P0 过小会让滤波器以为初始对准很准前几百秒修正量偏小P0 过大则开始阶段噪声偏大但会在几十秒内自动收敛。4.3 初始对准姿态零偏从哪里来静止时水平姿态用加速度计反算roll atan2(fy, fz)pitch atan2(-fx, sqrt(fy² fz²))fx、fy、fz 是加速度计在机体坐标系下的读数。航向没有绝对参照可以用 GNSS 速度方位yaw atan2(Ve, Vn)注意单位是弧度且要判断象限。如果车辆起步前没有 GNSS 速度航向只能先给一个粗略初值等第一次直线运动后让 EKF 把它拉正这称为动基座对准。很多工程包把对准状态单独做成一个状态机静止段做加计水平对准运动段做速度航向对准进入 EKF 主循环后才允许闭环反馈。初始对准不做直接进 EKF 会让零偏估计和姿态误差纠缠在一起前后几百秒的零偏曲线是耦合的。4.4 结果验证先看图再调参最直观的验证是画两条轨迹GNSS 原始轨迹和融合轨迹。融合轨迹应比 GNSS 更平滑、在 GNSS 中断段呈惯导外推的形状恢复后无大跳变。第二张图看 EKF 估计的陀螺零偏和加计零偏正常应在几百秒内从初值收敛到稳定区间并在小范围波动如果零偏曲线像斜坡一样单调爬升说明观测更新没能有效约束它多半是 R 太大或 H 缺了对应行。残差白噪声检验可以放在后面做先看轨迹重合度能快速筛掉大部分实现错误。轨迹偏差呈现固定方向偏移且随时间增长检查是否漏了杆臂补偿呈现锯齿状跳变检查时间对齐。5. INS_EKF 调参验证协方差、发散排查与紧组合升级方向5.1 用 NIS 做在线可信度检查新息向量 v z - H x_pred 里面藏着滤波器健康度。它的协方差 S H P_pred H.T R如果模型正确统计量 NIS v.T inv(S) v 应服从自由度等于观测维数的卡方分布。松组合位置速度双观测时自由度为 695% 分位数约 12.59仅位置观测自由度为 3分位数约 7.81。v innovation S H P_pred H.T R nis float(v.T np.linalg.inv(S) v) # 连续出现 NIS 远超阈值说明 Q 偏小或 H/R 不匹配这个指标可以做成滑动窗口均值替代肉眼盯残差。NIS 一直偏大优先放大 Q 中对应维度NIS 偏小但轨迹抖说明滤波器过度信任观测R 应调小。5.2 发散排查从时间戳到量纲EKF 发散时不要立刻调 Q。我一般按这个顺序排查先确认时间戳配对错配会造成周期性残差再查量纲角度用度还是弧度、速度用 m/s 还是 km/h这是最常见的一类发散源头然后核对杆臂向量方向它在导航系还是机体系符号对不对最后查 P0 和 Q 的量级确保与 15 维状态对齐。全部检查完再考虑算法层面的问题比如是否忘了闭环置零、协方差是否被重复初值化。5.3 松组合的边界与紧组合的升级条件松组合在 GNSS 正常输出时性能足够但卫星数降到 4 颗以下或伪距断续时松组合直接失效。紧组合用原始伪距和伪距率做观测即使只有 2~3 颗可见星也能形成对位置的弱约束城市峡谷场景收益明显。升级紧组合要把状态扩到 17 维以上、观测函数改成非线性、雅可比按当前估计位置计算工程改动量约是松组合的 2~3 倍。判断的依据很简单如果测试路线里频繁出现 GNSS 定位丢失或跳变且业务对连续性要求高就值得从松组合迁到紧组合如果开阔场地占比高松组合加良好调参已足够稳定输出。本文还有配套的精品资源点击获取
