企业官网建设流程全解析

简介：面向惯性导航与组合导航学习者，这套 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 时刻更新到 k+1 时刻，四元数形式是：

q_{k+1} = q_k ⊗ [cos(|Δθ|/2), (Δθ_x/|Δθ|)·sin(|Δθ|/2), (Δθ_y/|Δθ|)·sin(|Δθ|/2), (Δθ_z/|Δθ|)·sin(|Δθ|/2)]

这里的 ⊗ 是四元数乘法，左右顺序不能写反，工程上写反顺序导致某个轴持续偏转的案例很多。如果你习惯旋转矩阵，一阶近似就是 C_{k+1} ≈ 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_{k+1} = F·δx_k + w_k，F 的每个子块都有物理含义：陀螺零偏积分进姿态误差，姿态误差通过比力耦合进速度误差，速度误差积分进位置误差。这三组耦合是 F 的主体，符号搭错任何一个，滤波器要么不收敛，要么收敛到错误的地方。

误差状态模型依赖小角度假设：姿态误差几度以内、速度误差每秒几米以内。在里程计辅助下这个假设成立，因为每个量测更新周期误差都会被压小。如果出现长时间无更新（比如里程计丢了几千帧），误差长大，小角度近似失效，最常见的后果是滤波发散。所以代码里通常要加保护：当协方差对角线元素超过阈值时，降低对应状态的置信度或者强制重置。

有一个值得单独强调的点：速度误差方程里的比力耦合项绝对不能省，它体现的是"姿态误差导致重力方向判断错误，进而把重力误当成加速度"的机制。省略这一项，滤波器在长时间直线行驶时会持续高估或低估位置误差。这是很多初版组合导航代码"看起来在滤波、其实根本没生效"的原因。判断方法是看速度 innovation 的均值：均值明显不为零且缓慢漂移，多半就是这一项写错了。

3. 用卡尔曼滤波把 IMU 和里程计缝起来：松组合状态量与量测方程搭建

3.1 松组合还是紧组合：exp3 场景下为什么首选松组合

组合导航的"松"和"紧"，区别在量测进滤波器的粒度。松组合是先把里程计解算成速度或位置，再和 IMU 推算出的速度或位置做差，得到一个三维量测向量喂进滤波器；紧组合则是把每个轮子的脉冲数当原始量测，在滤波器内部完成轮速到导航系速度的映射。还有一种变体是把左右轮速差作为航向变化率的量测，这属于紧组合的变形。

对 exp3 这类课设和离线后处理场景，我一般直接选松组合。理由有三条：第一，松组合状态维数小，15 维足够，紧组合通常要到 20 维以上，调参和排查的时间成本高；第二，松组合的量测是速度差，物理含义直观，innovation 序列可以直接画出来看，出问题好定位；第三，离线数据里里程计已经预处理成频率稳定的速度序列，松组合假设量测噪声近似白噪声基本成立。

紧组合的优势在可用性：里程计打滑、短暂丢帧时，松组合会因为一帧异常量测污染整个滤波器，紧组合可以逐轮建模，把异常轮速剔除。但代价是实现复杂度高。如果你做的是实车、轮速有抖动和 ABS 干扰，那是紧组合的主场。exp3 这类离线数据，松组合完全够用，先把松组合跑通再考虑紧组合不迟。下面这张表把差异列清楚：

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，两者做差得到 innovation：

z = 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 的基准。

下面是一组可以直接套用的初始值，单位统一为 SI（m, rad, s）：

注意：这张表只是起点。调参的正确姿势是单变量扫描——固定 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 上。换数据集时先核对这里。下面是一个常见的文件清单和处理要点：

数据加载脚本里，单位换算必须在加载阶段完成：

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, g=9.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/s，f_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, axis=1) 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 频率、里程计更新率、坐标系约定都和数据册不一样，迁移代码时要先改配置块再跑滤波，配置错了会浪费你一整天时间。希望帮到你。

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