简介面向惯性导航与组合导航学习者这套 MATLAB 程序包基于车载实测数据完整覆盖车辆静止受扰晃动、起步行驶等阶段可完成惯导粗对准、精对准并分别输出惯性导航、惯性/卫星组合导航、惯性/里程计组合导航结果适合课程设计、科研入门或算法对比。压缩包体积仅 12KB共含 16 个 .m 源文件按功能划分为主控流程、数据预处理、粗/精对准、姿态矩阵与四元数/欧拉角/方向余弦变换、卡尔曼滤波修正以及卫星/里程计观测融合等模块代码模块化程度较高便于按需调用和二次开发。已有 729 人浏览学习该资源。读者可参照前2分钟粗对准、后8分钟精对准的实验设置体会载体受干扰晃动时惯导初始姿态的建立过程并通过对比三套导航结果理解不同传感器组合对定位精度的改善。源码以函数形式组织可直接修改运动模型、滤波器参数或输入数据作为组合导航算法实验的实用基线。1. exp3.zip 里的 IMU 惯导里程计组合用轮速把惯性漂移按住的导航方案打开 exp3.zip里面通常是一份 IMU 数据、一份里程计数据和一套待填空的滤波器框架。这个实验的主题一句话就能讲完把 IMU 和轮式里程计的数据喂给卡尔曼滤波得到比两者单独跑都更稳的位置、速度和姿态。IMU 单独积分位置不到一分钟就开始飘里程计单独做航位推算航向全靠陀螺拐几个弯就偏离真实路径。两者组合起来里程计的速度测量持续修正 IMU 的速度误差IMU 的姿态又给里程计一个稳定的航向基准——这就是惯导里程计组合导航的核心价值。它不需要卫星定位、不依赖外部基础设施适合室内机器人、AGV、隧道车辆这类场景也是很多课设和入门项目选择这个题目的原因。这篇文章按模型 → 滤波 → 实现 → 排错 → 落地的顺序把这份实验真正跑通并解释每一步为什么这么做。2. IMU 惯导里程计组合的模型基础姿态更新、刻度因子与误差状态方程2.1 四元数姿态更新从陀螺角增量到旋转矩阵姿态解算是组合导航的地基。IMU 里的陀螺输出载体相对惯性系的角速度中低精度 MEMS 场景下地球自转的影响量级小于陀螺零偏可以忽略直接把角速度乘上采样间隔 dt 得到角增量 Δθ[Δθx, Δθy, Δθz]。姿态从 k 时刻更新到 k1 时刻四元数形式是q_{k1} q_k ⊗ [cos(|Δθ|/2), (Δθ_x/|Δθ|)·sin(|Δθ|/2), (Δθ_y/|Δθ|)·sin(|Δθ|/2), (Δθ_z/|Δθ|)·sin(|Δθ|/2)]这里的 ⊗ 是四元数乘法左右顺序不能写反工程上写反顺序导致某个轴持续偏转的案例很多。如果你习惯旋转矩阵一阶近似就是 C_{k1} ≈ C_k·(I [Δθ]_×)其中 [·]_× 是反对称矩阵。这个近似在 dt 为毫秒级、角速度变化平缓时精度足够exp3 这类离线数据完全够用。下面是四元数更新的最小实现注意归一化那一步不能省import numpy as np def quat_update(q, gyro, dt): # q: [w, x, y, z] 当前姿态四元数已归一化 # gyro: [wx, wy, wz] 陀螺角速度单位 rad/s # dt: 采样间隔单位 s delta_theta np.asarray(gyro) * dt norm np.linalg.norm(delta_theta) if norm 1e-12: # 角增量接近 0 时直接返回 return q half delta_theta / norm * np.sin(norm / 2.0) dq np.array([np.cos(norm / 2.0), half[0], half[1], half[2]]) # 四元数乘法q ⊗ dq注意顺序不能反 q_new np.array([ q[0]*dq[0] - q[1]*dq[1] - q[2]*dq[2] - q[3]*dq[3], q[0]*dq[1] q[1]*dq[0] q[2]*dq[3] - q[3]*dq[2], q[0]*dq[2] - q[1]*dq[3] q[2]*dq[0] q[3]*dq[1], q[0]*dq[3] q[1]*dq[2] - q[2]*dq[1] q[3]*dq[0], ]) q_new / np.linalg.norm(q_new) # 强制归一化防止数值误差累积 return q_new这段代码里gyro 的单位必须是 rad/s。exp3 这类数据里常见的坑是原始角速度给的是 deg/s不换算直接喂进来姿态误差会以 57.3 倍的速度放大最终轨迹根本不可能闭合。另一个参数是 dt不要直接用第一帧和第二帧的时间差先统计整段时间戳的差分序列取均值避免个别时间戳抖动影响积分步长。姿态更新得到的是 C_b^n载体系到导航系的旋转矩阵后续速度、位置推算都要靠它。导航系选 NED 还是 ENU、载体系是 x 前 y 右 z 下还是 x 前 y 左 z 上决定了重力投影的符号和航向角的定义。这个约定要在代码里写死每次换数据集时重新核对。坐标系约定错位是组合导航里最贵的错误之一因为它在滤波层面很难查出来现象却极其严重——轨迹整体颠倒或者左右镜像。2.2 里程计刻度因子与安装偏角标定错了等于白组合里程计输出通常是脉冲数换算速度的公式是 v k_scale · N_pulse / (N_rev · dt)其中 N_rev 是轮子转一圈的脉冲数r 是车轮半径。问题在于 r 不是固定值胎压变化、载重变化都会改变有效滚动半径所以实际工程里引入一个刻度因子 k_scale 去修正。你可以把它理解成名义半径和真实半径的比值。标定做得准直线段定位误差就小标定不准误差随行驶距离线性放大100 米差 1% 就是 1 米。标定刻度因子有两条常见路线。第一条是有外部参考时让车沿一段已知长度的直线跑道匀速行驶记下里程计累计位移 s_odo真实长度为 s_ref修正量就是 s_ref / s_odo。第二条是没参考时找一条能回到起点的闭合路径以闭合误差最小为目标对 k_scale 做一维搜索。这个办法在课设里很常用精度也够。注意做闭合路径标定时要选地面摩擦好、速度平稳的工况跑两遍取平均压低滑移带来的偏差。安装偏角是第二个必须处理的量。里程计坐标系前向和车体坐标系 x 轴通常差一个几度的小角度直线行走时几乎看不出来但转弯时纵向速度会被投影到横向每次转弯都积累一段横向位移。把安装偏角写进量测模型形式是 v_odom_body R(δ)·[v, 0, 0]^Tδ 就是那个偏角。最简单的标定方法是让车转几个 90 度弯观察滤波 innovation 序列在转向段的锯齿幅值幅值就是偏角大小的体现。这里还要提一个很多教程不讲的细节轮式里程计在良好附着条件下横向和垂向速度理论上为零即 v_y ≈ 0、v_z ≈ 0。这个先验本身可以当作伪量测加入滤波器相当于免费给滤波器提供两个速度约束能显著抑制航向漂移。前提是车辆不发生侧滑——如果你跑的是低摩擦地面或者原地转向平台这条约束要关掉否则滤波器会被错误量测带偏。2.3 误差状态方程组合导航是算误差不是算状态直接用 IMU 推算出的位置、速度、姿态当状态量去做卡尔曼滤波理论可行但工程上很少这么干。惯性导航方程的非线性很强状态本身数值很大位置几百米、速度几米每秒而滤波器关心的偏差只有厘米级。直接对状态做线性化线性化误差容易把滤波器带偏。误差状态的做法是把状态拆成标称状态 误差状态标称状态用机械编排直接积分出来误差状态交给滤波器估计再把估计结果反馈回标称状态。典型的中低精度 IMU 加里程计组合误差状态取 15 维x [φ_x, φ_y, φ_z, δv_x, δv_y, δv_z, δp_x, δp_y, δp_z, δb_gx, δb_gy, δb_gz, δb_ax, δb_ay, δb_az]^T前 3 维是姿态误差中间 6 维是速度、位置误差后 6 维是陀螺和加速度计零偏。误差状态转移方程写成离散形式 δx_{k1} F·δx_k w_kF 的每个子块都有物理含义陀螺零偏积分进姿态误差姿态误差通过比力耦合进速度误差速度误差积分进位置误差。这三组耦合是 F 的主体符号搭错任何一个滤波器要么不收敛要么收敛到错误的地方。误差状态模型依赖小角度假设姿态误差几度以内、速度误差每秒几米以内。在里程计辅助下这个假设成立因为每个量测更新周期误差都会被压小。如果出现长时间无更新比如里程计丢了几千帧误差长大小角度近似失效最常见的后果是滤波发散。所以代码里通常要加保护当协方差对角线元素超过阈值时降低对应状态的置信度或者强制重置。有一个值得单独强调的点速度误差方程里的比力耦合项绝对不能省它体现的是姿态误差导致重力方向判断错误进而把重力误当成加速度的机制。省略这一项滤波器在长时间直线行驶时会持续高估或低估位置误差。这是很多初版组合导航代码看起来在滤波、其实根本没生效的原因。判断方法是看速度 innovation 的均值均值明显不为零且缓慢漂移多半就是这一项写错了。3. 用卡尔曼滤波把 IMU 和里程计缝起来松组合状态量与量测方程搭建3.1 松组合还是紧组合exp3 场景下为什么首选松组合组合导航的松和紧区别在量测进滤波器的粒度。松组合是先把里程计解算成速度或位置再和 IMU 推算出的速度或位置做差得到一个三维量测向量喂进滤波器紧组合则是把每个轮子的脉冲数当原始量测在滤波器内部完成轮速到导航系速度的映射。还有一种变体是把左右轮速差作为航向变化率的量测这属于紧组合的变形。对 exp3 这类课设和离线后处理场景我一般直接选松组合。理由有三条第一松组合状态维数小15 维足够紧组合通常要到 20 维以上调参和排查的时间成本高第二松组合的量测是速度差物理含义直观innovation 序列可以直接画出来看出问题好定位第三离线数据里里程计已经预处理成频率稳定的速度序列松组合假设量测噪声近似白噪声基本成立。紧组合的优势在可用性里程计打滑、短暂丢帧时松组合会因为一帧异常量测污染整个滤波器紧组合可以逐轮建模把异常轮速剔除。但代价是实现复杂度高。如果你做的是实车、轮速有抖动和 ABS 干扰那是紧组合的主场。exp3 这类离线数据松组合完全够用先把松组合跑通再考虑紧组合不迟。下面这张表把差异列清楚维度松组合紧组合量测粒度速度/位置差3 维轮速/脉冲原始量测每轮 1 维状态维数15 维左右20 维以上对打滑和丢帧的鲁棒性弱需要额外质检强可逐轮剔除实现和调试成本低innovation 容易分析高量测方程非线性适用场景离线数据、课设、低速机器人实车、复杂轮速工况3.2 15 维误差状态的转移矩阵与速度量测方程误差状态模型搭起来分三步确定状态向量、构造离散转移矩阵 F、构造量测矩阵 H。状态向量在 2.3 已经定了15 维。转移矩阵 F 的连续形式里关键子块如下姿态误差块δφ_dot -[ω_nb]×·δφ - δb_g角速度耦合加陀螺零偏驱动速度误差块δv_dot -[f]×·δφ δb_a比力耦合项符号别写反位置误差块δp_dot δv陀螺零偏块δb_g_dot 0建模为随机游走噪声放在 Q 里加计零偏块δb_a_dot 0同上离散化做一阶近似 F_d ≈ I F_c·dt。这个近似在 dt 超过 0.1 s 时会引入明显误差但 IMU 通常 100 Hz 以上完全够用。Q 的离散化要稍微认真一点Q_d ≈ Φ·Q_c·Φ^T·dt直接用连续 Q 乘 dt 也行精度略差课设里够用。量测方程是松组合的关键。里程计给出车体系前向速度 v_odo按零横向速度约束补零得到 v_odo_b [v, 0, 0]^T再乘姿态矩阵得到导航系速度 v_odo_n C_b^n·v_odo_b。滤波器里 IMU 推算出的导航系速度是 v_ins_n两者做差得到 innovationz v_ins_n - v_odo_n这个 innovation 里包含两类误差速度误差 δv以及姿态误差通过 C_b^n 影响 v_odo_n 投影带来的 [v_odo_n]×·φ。所以量测矩阵 H 在对应位置非零H[0:3, 0:3] skew(v_odo_n) H[0:3, 3:6] -I_3其余块全零。搭 H 时最容易错的是符号速度误差项是减号姿态误差项前面是正号。写反的话滤波器增益方向就反了结果是 innovation 越来越大滤波器越修越偏。量测噪声 R 对应里程计速度噪声方差取静止段速度输出的方差最直接。R 给太小会让滤波器过度相信里程计把姿态估计调得过于激进R 给太大量测不生效滤波器退化成纯惯导积分。工程上 R 对角元取速度噪声方差的 1 到 4 倍是一个稳的起点。3.3 Q/R 噪声参数初始化三个从数据里估出来的初值滤波器的 Q 和 R 最像玄学但初值可以不从拍脑袋开始。我一般按三个来源初始化器件先验陀螺零偏随机游走、加速度计噪声密度中低精度 IMU 数据手册能查到查不到就用同级别典型值静止段估计静态时陀螺输出的标准差可作为姿态激励参考加速度计输出标准差作为加计噪声参考量测估计取静止段里程计速度输出的标准差作为 R 的基准。下面是一组可以直接套用的初始值单位统一为 SIm, rad, s参数物理含义典型初值范围说明Q[0:3]姿态误差过程噪声(0.01~0.1 rad/s)^2 · dt与陀螺噪声水平相关Q[3:6]速度误差过程噪声(0.05~0.5 m/s^2)^2 · dt与加计噪声水平相关Q[9:12]陀螺零偏随机游走(1e-5~1e-4 rad/s^2)^2 · dt太小收敛慢太大零偏抖Q[12:15]加计零偏随机游走(1e-6~1e-4 m/s^3)^2 · dt通常比陀螺零偏还小R里程计速度量测噪声(0.02~0.1 m/s)^2静止段方差 ×1~4 倍注意这张表只是起点。调参的正确姿势是单变量扫描——固定 R把 Q 某一块从 1e-5 到 1e-1 按对数间隔扫 10 档看 innovation 标准差和位置误差的变化找到拐点。一次只动一个参数否则出了问题根本定位不到原因。P 的初值取折中位置不确定性 1 m 量级速度 0.1 m/s姿态角 0.5°零偏按器件精度。P 给太大会让前几十帧剧烈震荡太小会让滤波器反应迟钝。一个判断技巧位置协方差对角线开根号后的估计标准差应该和实际轨迹误差同量级差出 10 倍以上说明 P 或 Q/R 标定有问题别急着往下跑。4. 跑通 IMU里程计组合导航数据预处理、滤波实现与轨迹验证4.1 数据格式、时间同步与坐标系约定拿到 exp3 的数据后第一件事不是写滤波器而是把数据读出来画图。常见的数据文件是两到三个文本IMU 文件每行是时间戳、三轴角速度、三轴加速度里程计文件每行是时间戳和速度或脉冲数有的还带一份参考轨迹用于打分。画图要看的点IMU 静止段是否有明显零偏静止时加速度计 Z 轴应等于 g陀螺输出接近 0里程计速度是否有跳变、丢帧、长时间为 0 的段。预处理发现的问题比滤波本身更容易决定成败。时间同步是松组合最容易翻车的一环。IMU 频率通常 100 Hz 到 200 Hz里程计可能只有 10 Hz 到 50 Hz两个时间戳还未必同一基准。建议做法先统一时间单位有的给毫秒、有的给秒、有的给 GPS 周内秒再检查时间戳单调递增最后以 IMU 时间轴为基准对里程计速度做线性插值。插值不是取最近一帧——里程计在转弯时速度变化很快最近邻会引入半个周期的台阶误差表现为 innovation 锯齿。坐标系约定单独写一个配置块导航系 NED 还是 ENU载体系 x 前 y 右 z 下还是 x 前 y 左 z 上。换数据集时先核对这里。下面是一个常见的文件清单和处理要点文件典型内容预处理要点imu.txt时间戳、角速度(deg/s)、加速度(m/s^2)角速度转 rad/s检查静止段零偏odom.txt时间戳、速度(m/s)或脉冲数换算速度确认正方向和丢帧ref.txt时间戳、参考轨迹坐标核对坐标系插值到同一时间轴数据加载脚本里单位换算必须在加载阶段完成import numpy as np def load_imu(path): # 假设 imu 文件七列t, wx, wy, wz, ax, ay, az按实际列序调整 data np.loadtxt(path) t data[:, 0] gyro np.deg2rad(data[:, 1:4]) # 很多实验数据角速度给的是 deg/s acc data[:, 4:7] return t, gyro, acc def load_odom(path): data np.loadtxt(path) t data[:, 0] speed data[:, 1] # 已换算成 m/s注意正方向 return t, speed def sync_odom_to_imu(t_imu, t_odo, v_odo): # 以 IMU 时间为基准对里程计速度做线性插值 v_odo_interp np.interp(t_imu, t_odo, v_odo) return v_odo_interp这段代码里np.deg2rad 的换算必须在数据加载阶段完成否则后续所有推导都带着 57.3 倍的系统误差。np.interp 默认要求 t_odo 单调递增如果原始时间戳有重复或回跳插值结果会错加载后要先检查时间戳差分序列是否有非正数。里程计速度的正方向也要确认正方向是车前进方向如果给出的是负值或左右轮独立值要先换算成前向速度再进滤波器。预处理阶段还应输出诊断信息静止段陀螺 Z 轴均值和里程计速度均值。静止段陀螺 Z 轴均值就是初始零偏的估计滤波初始化时可以直接扣除里程计静止段速度均值应该是 0如果不是说明数据自带偏移会让 innovation 恒有一个常量偏差。4.2 松组合卡尔曼滤波核心代码滤波器主体是标准的预测-更新循环。预测步由 IMU 机械编排驱动每个 IMU 周期执行更新步由里程计数据驱动只在有量测的时刻执行。下面这个类把滤波器逻辑完整写出来了姿态更新和机械编排用函数壳代替重点放在滤波循环本身import numpy as np class LooseKF: def __init__(self, dt, g9.80665): self.dt dt self.g g self.dim 15 self.x np.zeros(self.dim) # 误差状态初始为 0 self.P np.eye(self.dim) * 1e-4 self.P[9:, 9:] * 100 # 零偏初始不确定度给大 self.I np.eye(self.dim) # 过程噪声按 3.3 的表初始化 self.Q np.eye(self.dim) self.Q[0:3, 0:3] * (0.05)**2 # 姿态激励 self.Q[3:6, 3:6] * (0.2)**2 # 速度激励 self.Q[9:12, 9:12] * (1e-4)**2 # 陀螺零偏随机游走 self.Q[12:15, 12:15] * (1e-4)**2 # 加计零偏随机游走 self.R np.eye(3) * (0.05)**2 # 里程计速度量测噪声 def predict(self, F): # F 为离散状态转移矩阵由机械编排构造 self.x F self.x self.P F self.P F.T self.Q self.P (self.P self.P.T) / 2 # 保持对称防数值漂移 def update(self, z, H): S H self.P H.T self.R K self.P H.T np.linalg.inv(S) self.x self.x K (z - H self.x) self.P (self.I - K H) self.P self.P (self.P self.P.T) / 2 self.innov z - H self.x # 更新后的残差留作诊断 return K主循环里每个 IMU 周期做一次姿态、速度、位置积分并同步构造 F每个里程计周期做量测更新。F 的构造抓住三组耦合写成矩阵块# 主循环伪代码F 的构造在每个 IMU 周期执行 # gyro 已转 rad/sf_n C_bn acc 为导航系比力 Phi np.eye(15) Phi[0:3, 0:3] np.eye(3) - dt * skew(gyro) # 姿态自耦合 Phi[0:3, 9:12] -dt * np.eye(3) # 陀螺零偏 - 姿态误差 Phi[3:6, 0:3] -dt * skew(f_n) # 比力 - 速度误差 Phi[3:6, 12:15] dt * C_bn # 加计零偏 - 速度误差 Phi[6:9, 3:6] dt * np.eye(3) # 速度误差 - 位置误差 # 量测更新有里程计数据时 if has_odom: v_odo_n C_bn np.array([v_odo, 0.0, 0.0]) z v_ins_n - v_odo_n H np.zeros((3, 15)) H[:, 0:3] skew(v_odo_n) # 姿态误差耦合到量测 H[:, 3:6] -np.eye(3) # 速度误差 kf.update(z, H)代码里几个参数的含义skew 是反对称矩阵构造对应三维向量的叉乘矩阵C_bn 是当前姿态矩阵由四元数更新后转换得到v_ins_n 是机械编排出来的导航系速度它是速度误差的标称值。滤波完成后必须做误差反馈位置 p - x[6:9]速度 v - x[3:6]姿态按小角度修正 C_bn - (I skew(x[0:3]))零偏 b_g - x[9:12]、b_a - x[12:15]。提示误差状态反馈完必须把 x 清零这是误差状态滤波的标准步骤。漏掉复位误差会被重复叠加轨迹会出现周期性跳变看起来就像滤波器在抽搐。4.3 轨迹对比与误差统计怎么判断这次组合是成功的滤波跑完后别急着看数值先把三条轨迹叠在一张图上纯里程计航位推算、纯 IMU 积分、组合导航结果。肉眼能看到的情况是纯 IMU 轨迹中段明显飘离纯里程计轨迹在转弯处有锯齿组合轨迹贴着参考轨迹走。如果三条线分不出来说明量测更新没生效回到 3.3 检查 R 是不是太大。误差统计我一般算三个数全程位置 RMSE、最大位置误差、终点位置误差闭合差。RMSE 反映整体水平最大误差反映最差工况闭合差最直观——车回到起点时差多少米。三个数要一起看只报 RMSE 的人经常被最大误差打脸def eval_traj(pos_est, pos_ref): # pos_est, pos_ref: (N, 2) 或 (N, 3) 轨迹 err np.linalg.norm(pos_est - pos_ref, axis1) rmse np.sqrt(np.mean(err**2)) return rmse, np.max(err), err[-1]这个函数建议在滤波器外单独写成回调每次调参后都输出三个指标。注意参考轨迹频率和组合导航输出频率不一致时要先插值到同一时间轴再算误差否则把时间不同步的假误差也算进去了。另外如果参考轨迹坐标系和你的导航系约定不同一个 NED 一个 ENU位置会整体颠倒误差大得离谱——先核对坐标系再谈精度。对精度预期给个参考室内低速平台几百米路径组合导航能把闭合差做到 1% 以下走 100 米误差小于 1 米做得好的能到 0.5%。纯里程计航位推算在同样路径下闭合差通常 2% 到 5%纯 IMU 积分基本是灾难。如果组合结果比纯里程计还差一定是标定或时间同步出了问题不是算法本身的问题。5. IMU 惯导里程计组合导航避坑指南5 个让结果翻车的常见问题5.1 时间戳不对齐轨迹在起点附近画出一个 S 形现象组合后的轨迹在转弯处出现明显锯齿直线段也有分层看起来像两条轨迹错开一段距离贴在一起。原因里程计和 IMU 的时间戳没对齐。常见两个来源一是两个文件时间基准不同IMU 用开机相对时间里程计用系统时间差一个常数偏移二是里程计频率低用最近邻而不是插值转弯时速度变化快半个采样周期的错位就形成台阶。解决先做时间对齐再做插值。对齐的简单方法让平台走一段带明显转弯的路径把里程计速度曲线和 IMU 角速度曲线做互相关峰值对应的延迟就是时间偏移。对齐之后用 np.interp 线性插值见 4.1插值后重画 innovation锯齿消失就说明对齐成功。5.2 刻度因子差 1%直线轨迹越走越偏现象直线段轨迹不直缓慢偏离参考方向走 100 米偏 1 米以上转弯处误差突然跳变滤波器似乎没修正。原因里程计刻度因子 k_scale 和真实值差了一个百分比速度整体偏大或偏小位置误差随行驶距离线性累积。刻度因子误差在直线段很难被 IMU 纠正因为量测模型没有对应的状态量误差只能表现为持续的位置偏差。解决最优先是在预处理阶段标定好见 2.2别指望滤波器吸收一个恒定百分比误差。标定后还有残留可以把刻度因子误差扩进状态向量让滤波器在线估计。但我建议课设阶段老老实实用标定值别急着扩维——扩维会改变可观测性结构很多同学扩完反而滤波发散。等松组合跑稳定了再考虑在线估计。5.3 安装偏角没人管转弯一次横向偏移半米现象直线段误差正常每次转弯后横向误差增加一点多转几次弯轨迹整体往一侧歪回到起点对不齐。原因里程计前向和车体系 x 轴之间存在安装偏角。直线行走时纵向速度投影到横向的分量小转弯时车体姿态快速变化这个横向分量被反复积分每次转弯积累一段横向位移。安装偏角不标定滤波算法本身再对结果也好不了。解决把偏角加进状态向量或者在预处理时静态标定。静态标定最简单让车转几个 90 度弯把 innovation 序列里转向段的三角锯齿取平均锯齿幅度就是偏角的体现。补偿用 v_odo_b [v, δ·v, 0] 的小角度近似即可。注意 δ 的符号以左转为正还是右转为正要和坐标系约定一致符号错一次误差加倍而不是消除。5.4 陀螺零偏收敛太慢航向角持续漂移现象滤波跑了一段时间位置误差不大但航向角慢慢往一个方向偏轨迹整体旋转回不到真航向。原因陀螺零偏的可观测性依赖转角激励。直线行驶时航向误差和陀螺零偏几乎不可区分滤波器对两者的估计都停在初值附近只有转弯时才能通过横向速度响应把它们分开。如果测试路径几乎全是直线零偏估计基本不动航向就跟着零偏慢慢飘。解决先用静止段数据直接估计初始零偏并扣除见 4.1这是成本最低的办法。其次确保测试路径包含足够多的转弯转弯能加速零偏收敛。另外检查状态转移矩阵里零偏到姿态误差的耦合项 F[0:3, 9:12]如果这里误写成 0零偏永远学不出来这是代码层面最常见的遗漏。5.5 滤波器协方差爆炸位置方差变成天文数字现象滤波跑几百步后P 矩阵对角线出现 1e10 以上量级的数值增益 K 变得极小量测更新完全失效误差无约束增长。原因三个常见诱因。一是 Q 给得太大过程噪声每步都在给协方差供血量测更新频率或 R 不足以压回去协方差一路涨。二是 P 初值给太大短的静止段降不下来。三是量测更新频率太低两步更新间预测步数太多协方差已经发散。解决先把 Q 对角元恢复到 3.3 表里的量级这是第一步。其次检查更新频率里程计 10 Hz、IMU 100 Hz 时每个更新周期之间只有 10 步预测协方差涨不了这么快如果预测了几百上千步要么是时间同步坏了要么是主循环更新条件写错。最后可以给 P 加软限幅更新后检查 P 对角元超过阈值按比例压缩。这是保命手段不解决根因只能争取排查时间。协方差爆炸十有八九是 Q/R 量级标定问题跟算法原理没关系别死磕公式。6. 把 exp3 的算法带到真实平台三个验证技巧和调参习惯组合导航在课设数据上跑通只是第一步真正麻烦的是把同一套算法搬到真实平台时如何快速判断还能不能用。这里分享三个我常用的技巧都是没有高精度真值设备时也能做的验证。第一个技巧是往返闭合验证。让平台沿同一条路径走一个往返比较回程终点和出发点的误差。没有参考轨迹也能做闭合差就是最直观的精度指标。往返路径还能暴露时间同步和刻度因子问题如果去程和回程轨迹不重合、形成一个开口多半是刻度因子不对如果开口方向在转弯处突然变化多半是安装偏角或时间同步问题。这个技巧成本最低适合每次改动后第一时间跑一遍。第二个技巧是参数扫描标定。把刻度因子和安装偏角各取十几档组合起来跑一遍离线数据画出闭合差的二维热力图最小值对应的参数组合就是优先采用的标定值。这个办法在数据回放时非常好用几十次运行的耗时也就几分钟换来的是不用靠猜的标定结果。第三个技巧是我最依赖的永远先看 innovation 序列再看轨迹。innovation 的均值、标准差、自相关是最诚实的结果——均值明显非零说明有系统误差时间同步或标定问题标准差大于 R 开根号说明有模型没覆盖的误差序列有趋势性漂移说明状态转移矩阵或 H 矩阵里有符号错误。我处理过不少轨迹看着还行、闭合差就是降不下来的案例最后都定位到 innovation 里的一个缓慢漂移而不是滤波器参数。所以拿到新数据第一件事就是把 innovation 画出来看它的白噪声性质再决定动 Q/R 还是动模型。调参的习惯我也固定下来了每次只改一个参数记录 innovation 标准差和三个误差指标RMSE、最大误差、闭合差改完把结果记在数据旁边。这个习惯帮我避开了大量调参玄学——组合导航的参数不是不能调但每一步都要有可解释的反馈信号。最后提醒一句真实平台上的 IMU 频率、里程计更新率、坐标系约定都和数据册不一样迁移代码时要先改配置块再跑滤波配置错了会浪费你一整天时间。希望帮到你。本文还有配套的精品资源点击获取
