调试一套 9 轴 IMU 的姿态解算最让人抓狂的时刻大概是这样的开机前两分钟姿态还挺稳跑到第十分钟 yaw 开始慢慢歪半小时后飘出去十几度再往后协方差矩阵对角线开始冒出负数滤波器直接自杀。如果你正卡在这个阶段那Error State Extended Kalman FilterES-EKF误差状态扩展卡尔曼滤波基本是绕不开的一步。它不是某个新奇的算法而是过去十几年里从 VIO、组合导航到自动驾驶定位模块里被反复验证过的工程范式把真实状态拆成标称状态 误差状态用一套只有 15 维的小扰动方程去做卡尔曼传播和更新。它解决的问题很具体——姿态的旋转参数化带来的奇异性、协方差随朝向漂移导致的过度自信、以及大状态下线性化点离真值太远导致雅可比失真。不管你现在做的是无人机、机器人底盘的里程计、还是手机端的行人航位推算只要涉及 IMU 和外界观测的融合这套框架都值得你从头到尾吃透一遍。1. 直接 EKF 跑不稳先把误差状态这件事想明白1.1 姿态参数化踩出来的三个死结先说清楚为什么不能在真实状态上直接开 EKF。假设我们把姿态用四元数 q 当状态位置 p、速度 v、陀螺零偏 b_g、加速度零偏 b_a 一起塞进去状态维度就是 3343316 维。这看起来挺正常但一上手就会撞上三堵墙。第一堵墙是约束被忽略。四元数必须满足模长为一这是个非线性约束而卡尔曼滤波的协方差更新是纯线性的矩阵运算它根本不知道这个约束存在。结果就是每次更新之后 q 的模长会漂移一点点你得手动归一化但你手动归一化之后协方差矩阵里对应的 4×4 分块其实已经跟当前状态对不上了误差在悄悄累积。第二堵墙是归一化带来的方向不确定性。四元数有 4 个自由度却只有 3 个真实自由度多出来的那个方向对应的是模长方向协方差矩阵在这个方向上会得到一个毫无物理意义的不确定性数值上常常表现为奇异或者负特征值。第三堵墙是线性化点偏移。EKF 的雅可比是在当前估计值上求的如果姿态估计本身已经歪了 10 度那么在这个歪掉的点上做一阶近似误差会随着姿态误差本身放大——姿态越不准线性化越差线性化越差姿态更不准形成正反馈。我在一个轮式机器人的项目上亲眼见过这个正反馈刚开始只是航向差 2 度跑了一公里之后变成 15 度最后整个地图都对不上了。回头看日志本质就是大状态 EKF 的四元数分块协方差一直在膨胀但增益却被错误的雅可比压得很低滤波器以为自己很准其实早就跑偏了。1.2 误差状态的定义把大状态拆成标称 小扰动ES-EKF 的核心思想非常朴素既然大状态离真值太远、线性化不可靠那就别在大状态上做滤波改成在偏差上做滤波。我们把真实状态 x_t 拆成两部分x_t x ⊞ δx这里的 x 是标称状态nominal state也就是滤波器当前给出的那个我认为的真值它可以用任意非线性的方式传播——用四元数乘法、用旋转矩阵指数映射随便你怎么写不需要线性化。δx 是误差状态error state定义为零均值的高斯小量所有卡尔曼滤波的数学运算都只在这个小量上做。这个拆法的好处立刻显现出来。第一δx 永远在小量附近线性化点极其准确一阶近似几乎不会失真。第二姿态的误差状态可以直接用 3 维的旋转向量 δθ 表示不再需要 4 维四元数模长约束自动消失奇异性被绕开了。第三因为 δx 的均值恒为 0协方差矩阵描述的是当前估计的不确定性而不是状态本身的绝对值数值量级始终很小不容易溢出。第四误差状态的动力学方程比真实状态的动力学方程简单得多——真实状态里那些非线性项比如旋转矩阵乘加速度在误差方程里变成了常数系数矩阵乘以 δx一眼就能看出耦合关系。有个类比我觉得挺贴切这就像 GPS 的 RTK 定位。你不直接去解算一个绝对坐标大数精度受浮点表示限制而是先有一个粗略的绝对位置然后只解算相对这个粗略位置的小修正量小数精度高。ES-EKF 干的是一模一样的事情只不过对象从位置换成了整个状态向量。1.3 全局误差与局部误差选错了后面全是坑误差状态的定义有两种常见约定必须想清楚再动手否则后面所有雅可比的符号都会乱。全局误差global / world-frame error把姿态误差定义在世界系里写成 R_t Exp(δθ_g) · R也就是先转一个小角度、再应用标称旋转。这种定义的物理含义是真值相对于标称值在世界系里多转了多少。局部误差local / body-frame error写成 R_t R · Exp(δθ)物理含义是真值相对于标称值在机体自身坐标系里多转了多少。我强烈建议绝大多数应用直接选局部误差。理由是局部误差下的姿态误差动力学里陀螺零偏项是干净的 -δb_g而全局误差里会多出一个 -[Rω]× δθ_g 的耦合项这一项跟当前朝向 R 有关会导致同一个陀螺、同一个零偏机器人朝东走和朝北走算出来的协方差不一样这种反直觉的现象。局部误差的另一个好处是它跟 IMU 的测量坐标系天然一致标定外参的时候不用来回换来换去。Sola 那篇Quaternion kinematics for the error-state Kalman filter是这套符号体系最完整的参考建议对着它把公式亲手推一遍比看十篇博客都管用。2. ES-EKF 的数学骨架连续方程、离散递推与雅可比2.1 状态分解与扰动约定先把状态量定下来。一个典型的 15 维误差状态向量长这样δx [ δp(3), δv(3), δθ(3), δb_a(3), δb_g(3) ]ᵀ对应的标称状态是p, v, R, b_a, b_g。真实状态和标称状态的关系是p_t p δp v_t v δv R_t R · Exp(δθ) b_a,t b_a δb_a b_g,t b_g δb_g注意这里我刻意把零偏写成加法而不是乘法因为零偏本身就在向量空间里加法误差是最自然的。而姿态必须用乘法或者说用指数映射的复合因为旋转矩阵不构成向量空间直接加法会破坏正交性——这正是最开始的三堵墙之一。IMU 的观测模型写成ω_m ω_t b_g,t n_g a_m R_tᵀ (a_t - g) b_a,t n_a这里 ω_m、a_m 是陀螺和加计的原始读数n_g、n_a 是测量白噪声g 是世界系的重力向量。这个模型里有个容易搞混的地方加速度计的测量本质上是比力也就是真实加速度减去重力在机体系下的投影所以反解真实加速度的时候要写成 a_t R_t(a_m - b_a,t - n_a) g。符号搞反的话滤波器会自信地把重力当成运动加速度跑起来就是一路往下掉。2.2 连续时间误差动力学15 维方程怎么来的把真实状态方程和标称状态方程分别写出来逐项相减、保留一阶项就能得到误差状态的连续时间微分方程。这个过程我在第一次推的时候来回核对了很多遍结论是下面这组局部误差约定δṗ δv δv̇ -R [a_m - b_a]× δθ - R δb_a - R n_a δθ̇ -[ω_m - b_g]× δθ - δb_g - n_g δḃ_a n_ba δḃ_g n_bg逐项解释一下这几个耦合关系因为它们直接决定了滤波器能不能观测到零偏。δv̇里出现-R[a]×δθ说明姿态误差会以加速度叉乘的方式泄漏到速度误差里。如果载体一直在做匀加速运动加速度大姿态误差造成的速度偏差就明显滤波器就有机会通过速度观测反推姿态反过来如果载体长期静止或者匀速这一项接近零速度和姿态的耦合就断了。δv̇里出现-R δb_a说明加速度计零偏和速度误差直接耦合。这也是为什么静止时加速度计零偏和重力方向几乎不可分辨——两者都表现为比力测量有个常值偏差。δθ̇里出现-[ω]× δθ这是一个反对称矩阵物理含义是姿态误差在机体系下随旋转而转动。注意它不是发散因为反对称矩阵的特征值全是纯虚数模长守恒。δθ̇里的-δb_g说明陀螺零偏直接积分成姿态误差所以陀螺零偏的收敛速度基本等于姿态误差的收敛速度。把这组方程整理成矩阵形式δẋ A δx G nA 矩阵就是那个 15×15 的稀疏矩阵里面有三个非平凡分块A[0:3,3:6] I、A[3:6,6:9] -R[a]×、A[3:6,9:12] -R、A[6:9,6:9] -[ω]×、A[6:9,12:15] -I。其余全是零。G 矩阵把噪声映射进来就是前面提到的那些系数。2.3 离散化状态转移矩阵与过程噪声矩阵连续方程到手之后实际跑的时候是按 IMU 的采样周期 Δt 离散递推的。状态转移矩阵最朴素的取法是Φ I A·Δt对 100Hz 以上的 IMU 完全够用。但有两个地方值得多花点心思。第一个是姿态误差分块。δθ̇ -[ω]×δθ这个方程有闭式解就是δθ(tΔt) Exp(-[ω]Δt) δθ(t)。直接用这个闭式解比一阶近似准得多而且完全不会引入额外的数值误差代价只是一次expm调用。我做高动态场景比如手持设备的快速挥动时这个改动能明显降低姿态协方差的振荡。第二个是过程噪声的离散化。连续白噪声的功率谱密度 Q_c 到离散协方差的转换严格写法是Q_d ∫₀^Δt Φ(τ) G Q_c Gᵀ Φ(τ)ᵀ dτ。工程上取一阶近似Q_d ≈ G Q_c Gᵀ Δt就够了因为 IMU 的 Δt 通常只有几毫秒二阶项是 Δt² 量级贡献很小。Q_c 的对角块直接填噪声密度平方陀螺的角度随机游走系数 σ_g单位 rad/s/√Hz的平方、加计的速度随机游走系数 σ_a单位 m/s²/√Hz的平方、以及两个零偏的随机游走系数。这些值别去官网抄典型值一定要用 Allan 方差自己测。一个很常见的错误是把 Q_d 写成G Q_c Gᵀ / Δt理由是Δt 越小噪声越大。这个直觉在离散白噪声序列的建模里是对的方差 密度/采样率但在连续时间系统离散化的语境下是反的。搞混了会导致滤波器在降低 IMU 频率时突然过度自信。2.4 观测更新、误差注入与协方差重置误差状态的观测更新和标准 EKF 形式一样r z - h(x) # 用标称状态算预测观测 H ∂h/∂δx |_{δx0} S H P Hᵀ R_k K P Hᵀ S⁻¹ δx̂ K r P ← (I - KH) P (I - KH)ᵀ K R_k Kᵀ关键在于后面这两步它们是 ES-EKF 区别于普通 EKF 的标志性动作。误差注入error injection把解算出来的 δx̂ 加回到标称状态上。p ← p δp̂ v ← v δv̂ R ← R · Exp(δθ̂) b_a ← b_a δb̂_a b_g ← b_g δb̂_g误差重置error reset注入之后误差状态的定义要求它的均值必须回到零所以直接把 δx 清零、把协方差做一次重置。姿态那块的重置需要一个雅可比因为Exp(δθ) ≈ Exp(δθ̂)Exp(δθ)用 BCH 公式展开到一阶可以得到δθ ≈ (I - 0.5[δθ̂]×) δθ所以P ← G_reset P G_resetᵀ G_reset diag(I, I, I - 0.5[δθ̂]×, I, I)实际代码里 δθ̂ 通常只有毫弧度量级I - 0.5[δθ̂]×跟单位阵的差别在 1e-4 以下很多人直接省略。但如果你的观测噪声设得比较大、单次更新的修正量很猛比如久违地收到一次 GNSS 定位省掉这一项会让姿态协方差轻微失真。我的做法是保留它代价只有一次 3×3 的矩阵乘法。3. 手撸一套 15 维 ES-EKF工程实现细节全记录3.1 状态量、坐标系与参数约定代码动手之前把约定写在注释里贴在文件顶部这一步能省掉后面几个小时的调试。我的习惯是项目约定说明世界系东北天ENU重力 g [0, 0, -9.81]ᵀ机体系前右下FRD与 IMU 外壳丝印一致姿态表示内部用旋转矩阵 R输入输出用四元数转换误差状态排列p, v, θ, b_a, b_g索引固定写死在常量里陀螺单位rad/s不要用 deg/s转换容易漏加计单位m/s²注意 ±2g 量程的量化坐标系的坑我踩过不止一次。如果世界系用 NED 而重力写成 9.81那所有 z 方向的观测都会反号滤波器跑起来的表现是静止时高度慢慢往下掉。这类问题在日志里看残差是看不出来的因为残差的均值和方差都很正常只有静置一段时间看位置漂移才会暴露。参数方面我一般把这些做成配置文件陀螺噪声密度、加计噪声密度、陀螺零偏游走、加计零偏游走、各观测量的噪声标准差、ZUPT 的触发阈值。尤其是观测噪声别用拍脑袋的值先跑一段静态数据用残差统计量标定。3.2 预测步实现先写两个基础工具函数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 exp_so3(phi): theta np.linalg.norm(phi) if theta 1e-10: return np.eye(3) skew(phi) K skew(phi / theta) return np.eye(3) np.sin(theta) * K (1 - np.cos(theta)) * K K预测步的主体def predict(self, w_m, a_m, dt): R, p, v self.R, self.p, self.v ba, bg self.ba, self.bg w w_m - bg a a_m - ba # --- 标称状态传播非线性不做线性化--- a_world R a self.g p_new p v * dt 0.5 * a_world * dt * dt v_new v a_world * dt R_new R exp_so3(w * dt) # --- 误差状态转移矩阵 --- F np.eye(15) F[0:3, 3:6] np.eye(3) * dt F[3:6, 6:9] -R skew(a) * dt F[3:6, 9:12] -R * dt F[6:9, 6:9] exp_so3(-w * dt) # 使用闭式解 F[6:9, 12:15] -np.eye(3) * dt # --- 噪声映射矩阵 --- G np.zeros((15, 12)) G[3:6, 0:3] -R G[6:9, 3:6] -np.eye(3) G[9:12, 6:9] np.eye(3) G[12:15, 9:12] np.eye(3) # --- 协方差传播 --- self.P F self.P F.T G self.Qc G.T * dt # --- 写回标称状态 --- self.p, self.v, self.R p_new, v_new, R_new三个细节值得说。第一位置用二阶积分p v·dt 0.5·a·dt²而不是简单的一阶p v·dt虽然在高频下差别不大但对齐 GNSS 的时候能省一点系统偏差。第二exp_so3(-w*dt)在 w 的模长接近零时会退化所以上面的实现对小于 1e-10 的情况做了泰勒展开兜底否则phi/theta会除零。第三self.Qc是 12×12 的对角阵顺序是[n_a, n_g, n_ba, n_bg]一定要跟 G 的列顺序对上这个错位是新手最常见的 bug——现象是滤波器能跑但零偏收敛极慢。3.3 更新步实现与 Joseph 形式通用的更新函数注意用 Joseph 形式保证数值稳定性def kalman_update(self, r, H, Rk): S H self.P H.T Rk # 用 solve 而不是求逆数值上更稳 K np.linalg.solve(S, H self.P).T dx K r I_KH np.eye(15) - K H self.P I_KH self.P I_KH.T K Rk K.T self.P 0.5 * (self.P self.P.T) # 强制对称 self.inject(dx)inject就是前面说的注入加重置def inject(self, dx): dtheta dx[6:9] self.p dx[0:3] self.v dx[3:6] self.R self.R exp_so3(dtheta) self.ba dx[9:12] self.bg dx[12:15] G_reset np.eye(15) G_reset[6:9, 6:9] np.eye(3) - 0.5 * skew(dtheta) self.P G_reset self.P G_reset.T self.P 0.5 * (self.P self.P.T)具体到某一种观测只需要写它的残差和雅可比。位置观测最简单def update_position(self, p_meas, Rk): r p_meas - self.p H np.zeros((3, 15)) H[0:3, 0:3] np.eye(3) self.kalman_update(r, H, Rk)姿态观测这一块是 ES-EKF 最舒服的地方。如果外界给了你一个姿态测量R_meas残差直接用对数映射算def update_attitude(self, R_meas, Rk): r log_so3(self.R.T R_meas) H np.zeros((3, 15)) H[0:3, 6:9] np.eye(3) self.kalman_update(r, H, Rk)雅可比就是单位阵漂亮得不像话。如果换成直接在大状态上做 EKF这里的雅可比会变成一个包含四元数各项的 4×4 矩阵还得处理模长约束。对比一下就明白为什么大家愿意折腾那套误差状态的符号了。3.4 数值稳定性处理清单P 矩阵跑到负定是新手最容易遇到的崩溃点下面这几条是我在多个项目里固定加上的保险。对称化每次预测和更新之后执行P 0.5*(P P.T)。浮点累加会让 P 轻微不对称长时间跑下来不对称量会放大最后触发Cholesky失败。Joseph 形式更新时用(I-KH)P(I-KH)ᵀ KRKᵀ而不是(I-KH)P。前者的数值稳定性好得多即使 K 计算有微小误差也能保持 P 半正定。避免显式求逆S⁻¹一律用np.linalg.solve或者scipy.linalg.cho_solve后者更快也更稳。特征值钳位调试阶段可以在预测后检查np.linalg.eigvalsh(P)的最小值如果小于 0 就打印警告。正式版本里我一般不钳位因为出现问题应该去查根因而不是掩盖。量纲归一化位置单位用米、姿态误差用弧度、零偏用原始单位。如果姿态误差用度那 H 矩阵和 P 的量级会差出 300 倍S 矩阵的条件数会变得很难看。不要在同一帧内重复注入多个观测到达时可以批量更新把残差和 H 上下拼接后做一次更新也可以顺序更新。顺序更新每次都要注入和重置虽然数学上等价但反复重置会引入额外的舍入误差我的习惯是同一时刻的观测打包成一次更新。4. 观测接入实战IMU 之外的传感器怎么喂进去4.1 位置速度观测GNSS 与轮速里程计GNSS 给位置是最常见的用法。直接拿经纬高换算到局部 ENU 坐标然后走update_position。要注意的是消费级模块的定位噪声高度相关不是白噪声而且有跳点。我的处理方式是在外面套一层卡方检验算NIS rᵀS⁻¹r自由度是 3如果 NIS 超过阈值比如 16对应 99.7% 置信就认为这一帧是野值直接丢弃不更新。这一步能挡掉绝大多数高架桥下、城市峡谷里的跳点。轮速里程计是另一类非常实用的观测。差速底盘可以拿到左右轮速转成机体前进速度v_b观测方程是v_body_meas Rᵀ v v_noise。对应的残差和雅可比def update_odom(self, v_body_meas, Rk): r v_body_meas - self.R.T self.v H np.zeros((3, 15)) H[0:3, 3:6] self.R.T H[0:3, 6:9] skew(self.R.T self.v) # 对 δθ 的雅可比 self.kalman_update(r, H, Rk)注意H[0:3, 6:9]这一块千万不要漏。它的物理含义是姿态误差会让机体系速度的投影方向偏掉。漏掉它的后果是滤波器会在姿态有误差时把方向偏差误判成速度大小的偏差导致速度估计出现周期性抖动。我第一次写的时候就是漏了这项现象是机器人直行时速度估计跟着航向角呈正弦波动查了两天才找到。轮速里程计有个致命弱点打滑。一旦轮子空转观测值会严重偏大。工程上一般做两件事一是用加计的纵向加速度跟轮速微分做一致性检验二是给里程计的观测噪声设一个跟加速度相关的自适应值加速度大时噪声放大。这两招都不完美但比什么都做强。4.2 姿态观测视觉位姿与磁力计视觉惯性里程计VIO里视觉前端给出的位姿可以作为一个绝对观测直接喂进来。姿态部分用 4.2 节那个简洁的写法就行。位置部分要注意视觉的尺度问题——单目 VIO 的尺度是漂的所以要么用双目/深度要么把尺度也放进状态里估计。尺度这东西跟速度、零偏都有耦合放进去之后可观性会变差得配合足够的运动激励才收敛。磁力计单独用是很危险的室内钢铁结构、电机、电流线都会让地磁方向产生几十度的偏差。我的惯例是只有在长时间没有其他航向观测、并且检测到磁力计数据的一致性良好时模长接近当地地磁总场强度、三轴变化平滑才允许用磁力计做一次弱航向观测观测噪声设得很大比如 20 度。用的话观测模型是取磁场水平分量算航向角然后由航向角造一个虚拟姿态观测。别忘了磁力计本身也需要标定硬磁、软磁没标定的磁力计数据基本没法用。我在一个室内机器人上就因为偷懒没做软磁标定结果机器人每次经过某段货架航向就会被拽偏五六度跑完一圈回原点差了将近一米。4.3 ZUPT 零速修正静止检测怎么做才不误触发零速修正Zero Velocity UpdateZUPT是性价比最高的一个技巧尤其适合步行者导航、手持设备和间歇性运动的机器人。原理非常简单当检测到载体静止时把速度当成观测量 0 加进去H [0, I, 0, 0, 0]残差就是0 - v。难度全在静止检测上。只靠加计模长接近 9.81 是不够的因为匀速直线运动时加计模长也接近 9.81。我常用的判据是三个条件同时成立陀螺三轴模长小于阈值比如 0.5 deg/s这是最关键的判据因为匀速运动时陀螺读数不为零加计三轴在滑动窗口0.2 到 0.5 秒内的方差小于阈值说明没有振动加计模长在 9.81 附近的一个宽区间内比如 ±0.3 m/s²排除自由落体。窗口长度需要权衡太短容易误触发比如走路时的脚掌触地瞬间太长会漏掉短暂停顿。我一般用 0.3 秒的滑动窗并且要求连续 N 帧都满足条件才真正进入 ZUPT 状态退出时也要有滞回避免在阈值附近反复横跳。ZUPT 生效期间速度和位置的不确定性会显著下降但航向的不确定性并不会。因为零速只能约束速度约束不了朝向。别指望靠 ZUPT 把 yaw 漂移治好它治不了。4.4 外参与时间偏移的在线估计如果 IMU 和相机或者 IMU 和轮速计之间的外参不准融合结果会出现系统性的偏差。平移外参误差主要表现为位置残差里的常值偏置旋转外参误差主要表现为姿态残差在运动时的规律性偏差。标定的方法无非两种离线用标定板或者专用轨迹做一次或者在线把外参放进状态里一起估计。在线估计外参的代价是状态维度膨胀每增加一个 3 维外参状态加 3 维而且可观性依赖运动激励。我在实际项目里更倾向于离线标定 在线只估计时间偏移。时间偏移IMU 和相机的时间戳差了 td 秒在高动态下影响极大一两毫秒的偏移在快速旋转时就能造成好几度的姿态误差。把 td 作为一个标量状态放进去观测雅可比用姿态残差对 td 的导数近似成角速度乘以残差方向收敛得还算快。外参标定的实操心得标定轨迹一定要包含三个轴的充分旋转尤其是绕重力的 yaw 旋转。只做俯仰和横滚的话IMU 和相机之间的航向安装角是标不出来的因为它和重力方向的可观测性正交。5. 排查实录姿态漂、协方差发散、输出跳变到底怎么回事5.1 协方差发散与滤波器自杀现象是跑一段时间后 P 的对角线指数增长位置估计开始剧烈抖动最后数值溢出。这通常不是单一原因按我的排查顺序是这样的。先看可观性。如果某个状态的观测量长期为零它的协方差就会一直涨。最常见的例子是纯 IMU 位置观测的系统里加速度计零偏在缺少旋转激励时不可观零偏协方差会一路涨到天上去。这时候滤波器其实没崩只是因为零偏不可观它必须用膨胀的协方差来表达我不确定而这会污染到速度估计。解决办法是加约束要么用实际的旋转运动激励它要么给零偏的协方差设一个上限本质上是在告诉滤波器零偏不可能超过这个范围这是合理的先验。再看过程噪声。Q 设大了协方差膨胀快滤波器会过度依赖观测表现为跟踪噪声大、输出抖动Q 设小了协方差增长慢滤波器会过度自信表现为慢慢偏离真值、观测被忽略。判断哪个方向的诀窍是看 NIS 统计量的均值NIS 均值远大于自由度说明协方差偏小过度自信远小于自由度说明协方差偏大过度保守。这个诊断花五分钟就能做能省掉大量瞎猜。最后看数值。跑几万步之后如果 P 出现了负特征值基本可以确定是累加误差造成的。检查是不是漏了对称化、是不是用了(I-KH)P而不是 Joseph 形式。5.2 yaw 漂移与可观测性yaw 漂移是 ES-EKF 最经典的痛点也是最多人问的问题。根源在于只有 IMU 加位置观测的系统里航向角、北向速度、东向加速度计零偏这三者构成一个不可观测子空间。直观解释是这样如果我把整条轨迹旋转一个小角度同时把北向速度做相应的调整再微调一下加速度计零偏最终产生的位置观测序列可以完全一样。也就是说光看位置轨迹你无法区分我朝向偏了和我速度偏了。所以滤波器只能靠积分陀螺来维持航向陀螺零偏一旦漂yaw 就跟着漂而且没有任何观测能纠正它。打破这个困局只有三条路一是引入绝对航向观测双天线 GNSS 的基线航向、磁力计、视觉给出绝对朝向二是引入从运动学约束推出的航向观测比如车辆的非完整约束横向速度理论上为零三是保证轨迹有足够的旋转励磁让陀螺零偏可观测。第二点在实际中很好用四轮车的横向速度约束是一个很强的伪观测实现只需要一行 H 矩阵。别指望通过调参解决 yaw 漂移。调参只能改变漂移的速率和形状改变不了漂移的必然性。这是结构性的可观测性问题只能通过增加信息源解决。5.3 常见问题速查表下面这张表是我和同事们的病历本大部分现场问题都能在里面找到影子。现象可能原因排查手段处理方式静置时高度缓慢下漂世界系/重力方向约定不一致打印静止时 a_world 的 z 分量检查 g 的符号与坐标系静置时速度缓慢增长加计零偏未被观测重力泄漏看零偏估计是否持续漂用 ZUPT 或提高加计噪声密度yaw 缓慢漂移陀螺零偏不可观看陀螺零偏协方差是否发散加磁力计/双天线/横向速度约束姿态输出周期性抖动速度雅可比漏了对姿态的偏导检查 H 矩阵非零块补上 skew(Rᵀv) 项收到观测后输出跳变观测噪声设太小或残差未做野值剔除统计 NIS 分布提高 R加卡方检验协方差出现负特征值未做对称化或用错更新形式检查 eigvalsh(P)对称化 Joseph 形式更新后滤波器反而更差误差注入与重置顺序写反打印注入前后标称状态先注入再重置重置用 G_reset收敛极慢G 矩阵列顺序与 Qc 顺序不匹配打印 G 和 Qc 的对角对齐噪声顺序与单位快速旋转时姿态偏离姿态传播时未用指数映射检查大角速度下的姿态用 exp_so3 做标称传播长时间运行后发散浮点累积 缺少周期性重正交检查 R 的正交性误差每帧对 R 做一次正交化6. 我个人踩坑后的几条建议6.1 初始化与对齐初始化的质量决定了后面能跑多远。我一般分三步做。第一步是静置对齐让设备静止 2 到 5 秒用这段时间的陀螺均值作为零偏初值用加计均值反解初始俯仰和横滚。加计反解姿态的时候要注意静止状态下加计测的完全部是重力所以R R_from_gravity(a_m / |a_m|)。这一步的精度受加计噪声影响通常能到 0.5 度以内。第二步是航向初始化如果没有任何航向观测就把初始 yaw 设成 0但协方差要设大比如 30 度对应的方差告诉滤波器我不知道朝向。如果把初始 yaw 协方差设得很小那么在遇到第一个航向观测之前滤波器会一直坚信这个错误的朝向等观测来了会产生一次大跳变。第三步是零偏协方差初始化陀螺零偏的初始标准差建议设成 1 到 2 deg/s加计零偏设成 0.1 到 0.2 m/s²都比典型真值大不少给滤波器足够的调整空间。关于姿态初始化有个细节从加计反解姿态时yaw 是完全不可知的所以要显式地把 yaw 自由度留出来。我见过有人用R_from_gravity之后顺手归一化了一个随意的朝向结果 yaw 协方差被错误地压小了。6.2 验证与评估评价一个 ES-EKF 好不好别只看最终的轨迹误差。我一般看四个指标。第一是残差的统计特性正常的残差应该接近零均值白噪声如果出现明显的偏置或者周期性说明模型有问题。第二是 NIS 的一致性这是最有力的工具直接反映协方差是否合理。第三是协方差收敛曲线所有可观状态的协方差都应该收敛到稳态如果某一路一直不降说明它不可观需要确认这是不是设计意图。第四是零偏估计的稳定性陀螺和加计零偏的估计值应该在真值附近小幅波动如果持续单调漂移说明观测信息不足或者 Q 设得不对。我还会做一次回放测试把同一段数据用不同参数配置跑几遍比较轨迹的差异。如果参数微调导致结果差异很大说明系统鲁棒性不够很可能有状态处于临界可观测的状态。这个测试比单次跑通更能说明问题。最后分享一个小技巧专门针对调试阶段把每次观测更新后的δx̂的模长打印出来。正常情况下它应该是毫弧度、厘米量级的。如果某次更新之后δx̂的模长突然变成几米或者几十度那这一帧观测几乎肯定有问题——要么是野值要么是你的某个雅可比符号写反了。这个指标比看最终轨迹敏感得多能在问题酿成大错之前就提醒你。
ES-EKF 误差状态扩展卡尔曼滤波：IMU 姿态解算与传感器融合实战
调试一套 9 轴 IMU 的姿态解算最让人抓狂的时刻大概是这样的开机前两分钟姿态还挺稳跑到第十分钟 yaw 开始慢慢歪半小时后飘出去十几度再往后协方差矩阵对角线开始冒出负数滤波器直接自杀。如果你正卡在这个阶段那Error State Extended Kalman FilterES-EKF误差状态扩展卡尔曼滤波基本是绕不开的一步。它不是某个新奇的算法而是过去十几年里从 VIO、组合导航到自动驾驶定位模块里被反复验证过的工程范式把真实状态拆成标称状态 误差状态用一套只有 15 维的小扰动方程去做卡尔曼传播和更新。它解决的问题很具体——姿态的旋转参数化带来的奇异性、协方差随朝向漂移导致的过度自信、以及大状态下线性化点离真值太远导致雅可比失真。不管你现在做的是无人机、机器人底盘的里程计、还是手机端的行人航位推算只要涉及 IMU 和外界观测的融合这套框架都值得你从头到尾吃透一遍。1. 直接 EKF 跑不稳先把误差状态这件事想明白1.1 姿态参数化踩出来的三个死结先说清楚为什么不能在真实状态上直接开 EKF。假设我们把姿态用四元数 q 当状态位置 p、速度 v、陀螺零偏 b_g、加速度零偏 b_a 一起塞进去状态维度就是 3343316 维。这看起来挺正常但一上手就会撞上三堵墙。第一堵墙是约束被忽略。四元数必须满足模长为一这是个非线性约束而卡尔曼滤波的协方差更新是纯线性的矩阵运算它根本不知道这个约束存在。结果就是每次更新之后 q 的模长会漂移一点点你得手动归一化但你手动归一化之后协方差矩阵里对应的 4×4 分块其实已经跟当前状态对不上了误差在悄悄累积。第二堵墙是归一化带来的方向不确定性。四元数有 4 个自由度却只有 3 个真实自由度多出来的那个方向对应的是模长方向协方差矩阵在这个方向上会得到一个毫无物理意义的不确定性数值上常常表现为奇异或者负特征值。第三堵墙是线性化点偏移。EKF 的雅可比是在当前估计值上求的如果姿态估计本身已经歪了 10 度那么在这个歪掉的点上做一阶近似误差会随着姿态误差本身放大——姿态越不准线性化越差线性化越差姿态更不准形成正反馈。我在一个轮式机器人的项目上亲眼见过这个正反馈刚开始只是航向差 2 度跑了一公里之后变成 15 度最后整个地图都对不上了。回头看日志本质就是大状态 EKF 的四元数分块协方差一直在膨胀但增益却被错误的雅可比压得很低滤波器以为自己很准其实早就跑偏了。1.2 误差状态的定义把大状态拆成标称 小扰动ES-EKF 的核心思想非常朴素既然大状态离真值太远、线性化不可靠那就别在大状态上做滤波改成在偏差上做滤波。我们把真实状态 x_t 拆成两部分x_t x ⊞ δx这里的 x 是标称状态nominal state也就是滤波器当前给出的那个我认为的真值它可以用任意非线性的方式传播——用四元数乘法、用旋转矩阵指数映射随便你怎么写不需要线性化。δx 是误差状态error state定义为零均值的高斯小量所有卡尔曼滤波的数学运算都只在这个小量上做。这个拆法的好处立刻显现出来。第一δx 永远在小量附近线性化点极其准确一阶近似几乎不会失真。第二姿态的误差状态可以直接用 3 维的旋转向量 δθ 表示不再需要 4 维四元数模长约束自动消失奇异性被绕开了。第三因为 δx 的均值恒为 0协方差矩阵描述的是当前估计的不确定性而不是状态本身的绝对值数值量级始终很小不容易溢出。第四误差状态的动力学方程比真实状态的动力学方程简单得多——真实状态里那些非线性项比如旋转矩阵乘加速度在误差方程里变成了常数系数矩阵乘以 δx一眼就能看出耦合关系。有个类比我觉得挺贴切这就像 GPS 的 RTK 定位。你不直接去解算一个绝对坐标大数精度受浮点表示限制而是先有一个粗略的绝对位置然后只解算相对这个粗略位置的小修正量小数精度高。ES-EKF 干的是一模一样的事情只不过对象从位置换成了整个状态向量。1.3 全局误差与局部误差选错了后面全是坑误差状态的定义有两种常见约定必须想清楚再动手否则后面所有雅可比的符号都会乱。全局误差global / world-frame error把姿态误差定义在世界系里写成 R_t Exp(δθ_g) · R也就是先转一个小角度、再应用标称旋转。这种定义的物理含义是真值相对于标称值在世界系里多转了多少。局部误差local / body-frame error写成 R_t R · Exp(δθ)物理含义是真值相对于标称值在机体自身坐标系里多转了多少。我强烈建议绝大多数应用直接选局部误差。理由是局部误差下的姿态误差动力学里陀螺零偏项是干净的 -δb_g而全局误差里会多出一个 -[Rω]× δθ_g 的耦合项这一项跟当前朝向 R 有关会导致同一个陀螺、同一个零偏机器人朝东走和朝北走算出来的协方差不一样这种反直觉的现象。局部误差的另一个好处是它跟 IMU 的测量坐标系天然一致标定外参的时候不用来回换来换去。Sola 那篇Quaternion kinematics for the error-state Kalman filter是这套符号体系最完整的参考建议对着它把公式亲手推一遍比看十篇博客都管用。2. ES-EKF 的数学骨架连续方程、离散递推与雅可比2.1 状态分解与扰动约定先把状态量定下来。一个典型的 15 维误差状态向量长这样δx [ δp(3), δv(3), δθ(3), δb_a(3), δb_g(3) ]ᵀ对应的标称状态是p, v, R, b_a, b_g。真实状态和标称状态的关系是p_t p δp v_t v δv R_t R · Exp(δθ) b_a,t b_a δb_a b_g,t b_g δb_g注意这里我刻意把零偏写成加法而不是乘法因为零偏本身就在向量空间里加法误差是最自然的。而姿态必须用乘法或者说用指数映射的复合因为旋转矩阵不构成向量空间直接加法会破坏正交性——这正是最开始的三堵墙之一。IMU 的观测模型写成ω_m ω_t b_g,t n_g a_m R_tᵀ (a_t - g) b_a,t n_a这里 ω_m、a_m 是陀螺和加计的原始读数n_g、n_a 是测量白噪声g 是世界系的重力向量。这个模型里有个容易搞混的地方加速度计的测量本质上是比力也就是真实加速度减去重力在机体系下的投影所以反解真实加速度的时候要写成 a_t R_t(a_m - b_a,t - n_a) g。符号搞反的话滤波器会自信地把重力当成运动加速度跑起来就是一路往下掉。2.2 连续时间误差动力学15 维方程怎么来的把真实状态方程和标称状态方程分别写出来逐项相减、保留一阶项就能得到误差状态的连续时间微分方程。这个过程我在第一次推的时候来回核对了很多遍结论是下面这组局部误差约定δṗ δv δv̇ -R [a_m - b_a]× δθ - R δb_a - R n_a δθ̇ -[ω_m - b_g]× δθ - δb_g - n_g δḃ_a n_ba δḃ_g n_bg逐项解释一下这几个耦合关系因为它们直接决定了滤波器能不能观测到零偏。δv̇里出现-R[a]×δθ说明姿态误差会以加速度叉乘的方式泄漏到速度误差里。如果载体一直在做匀加速运动加速度大姿态误差造成的速度偏差就明显滤波器就有机会通过速度观测反推姿态反过来如果载体长期静止或者匀速这一项接近零速度和姿态的耦合就断了。δv̇里出现-R δb_a说明加速度计零偏和速度误差直接耦合。这也是为什么静止时加速度计零偏和重力方向几乎不可分辨——两者都表现为比力测量有个常值偏差。δθ̇里出现-[ω]× δθ这是一个反对称矩阵物理含义是姿态误差在机体系下随旋转而转动。注意它不是发散因为反对称矩阵的特征值全是纯虚数模长守恒。δθ̇里的-δb_g说明陀螺零偏直接积分成姿态误差所以陀螺零偏的收敛速度基本等于姿态误差的收敛速度。把这组方程整理成矩阵形式δẋ A δx G nA 矩阵就是那个 15×15 的稀疏矩阵里面有三个非平凡分块A[0:3,3:6] I、A[3:6,6:9] -R[a]×、A[3:6,9:12] -R、A[6:9,6:9] -[ω]×、A[6:9,12:15] -I。其余全是零。G 矩阵把噪声映射进来就是前面提到的那些系数。2.3 离散化状态转移矩阵与过程噪声矩阵连续方程到手之后实际跑的时候是按 IMU 的采样周期 Δt 离散递推的。状态转移矩阵最朴素的取法是Φ I A·Δt对 100Hz 以上的 IMU 完全够用。但有两个地方值得多花点心思。第一个是姿态误差分块。δθ̇ -[ω]×δθ这个方程有闭式解就是δθ(tΔt) Exp(-[ω]Δt) δθ(t)。直接用这个闭式解比一阶近似准得多而且完全不会引入额外的数值误差代价只是一次expm调用。我做高动态场景比如手持设备的快速挥动时这个改动能明显降低姿态协方差的振荡。第二个是过程噪声的离散化。连续白噪声的功率谱密度 Q_c 到离散协方差的转换严格写法是Q_d ∫₀^Δt Φ(τ) G Q_c Gᵀ Φ(τ)ᵀ dτ。工程上取一阶近似Q_d ≈ G Q_c Gᵀ Δt就够了因为 IMU 的 Δt 通常只有几毫秒二阶项是 Δt² 量级贡献很小。Q_c 的对角块直接填噪声密度平方陀螺的角度随机游走系数 σ_g单位 rad/s/√Hz的平方、加计的速度随机游走系数 σ_a单位 m/s²/√Hz的平方、以及两个零偏的随机游走系数。这些值别去官网抄典型值一定要用 Allan 方差自己测。一个很常见的错误是把 Q_d 写成G Q_c Gᵀ / Δt理由是Δt 越小噪声越大。这个直觉在离散白噪声序列的建模里是对的方差 密度/采样率但在连续时间系统离散化的语境下是反的。搞混了会导致滤波器在降低 IMU 频率时突然过度自信。2.4 观测更新、误差注入与协方差重置误差状态的观测更新和标准 EKF 形式一样r z - h(x) # 用标称状态算预测观测 H ∂h/∂δx |_{δx0} S H P Hᵀ R_k K P Hᵀ S⁻¹ δx̂ K r P ← (I - KH) P (I - KH)ᵀ K R_k Kᵀ关键在于后面这两步它们是 ES-EKF 区别于普通 EKF 的标志性动作。误差注入error injection把解算出来的 δx̂ 加回到标称状态上。p ← p δp̂ v ← v δv̂ R ← R · Exp(δθ̂) b_a ← b_a δb̂_a b_g ← b_g δb̂_g误差重置error reset注入之后误差状态的定义要求它的均值必须回到零所以直接把 δx 清零、把协方差做一次重置。姿态那块的重置需要一个雅可比因为Exp(δθ) ≈ Exp(δθ̂)Exp(δθ)用 BCH 公式展开到一阶可以得到δθ ≈ (I - 0.5[δθ̂]×) δθ所以P ← G_reset P G_resetᵀ G_reset diag(I, I, I - 0.5[δθ̂]×, I, I)实际代码里 δθ̂ 通常只有毫弧度量级I - 0.5[δθ̂]×跟单位阵的差别在 1e-4 以下很多人直接省略。但如果你的观测噪声设得比较大、单次更新的修正量很猛比如久违地收到一次 GNSS 定位省掉这一项会让姿态协方差轻微失真。我的做法是保留它代价只有一次 3×3 的矩阵乘法。3. 手撸一套 15 维 ES-EKF工程实现细节全记录3.1 状态量、坐标系与参数约定代码动手之前把约定写在注释里贴在文件顶部这一步能省掉后面几个小时的调试。我的习惯是项目约定说明世界系东北天ENU重力 g [0, 0, -9.81]ᵀ机体系前右下FRD与 IMU 外壳丝印一致姿态表示内部用旋转矩阵 R输入输出用四元数转换误差状态排列p, v, θ, b_a, b_g索引固定写死在常量里陀螺单位rad/s不要用 deg/s转换容易漏加计单位m/s²注意 ±2g 量程的量化坐标系的坑我踩过不止一次。如果世界系用 NED 而重力写成 9.81那所有 z 方向的观测都会反号滤波器跑起来的表现是静止时高度慢慢往下掉。这类问题在日志里看残差是看不出来的因为残差的均值和方差都很正常只有静置一段时间看位置漂移才会暴露。参数方面我一般把这些做成配置文件陀螺噪声密度、加计噪声密度、陀螺零偏游走、加计零偏游走、各观测量的噪声标准差、ZUPT 的触发阈值。尤其是观测噪声别用拍脑袋的值先跑一段静态数据用残差统计量标定。3.2 预测步实现先写两个基础工具函数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 exp_so3(phi): theta np.linalg.norm(phi) if theta 1e-10: return np.eye(3) skew(phi) K skew(phi / theta) return np.eye(3) np.sin(theta) * K (1 - np.cos(theta)) * K K预测步的主体def predict(self, w_m, a_m, dt): R, p, v self.R, self.p, self.v ba, bg self.ba, self.bg w w_m - bg a a_m - ba # --- 标称状态传播非线性不做线性化--- a_world R a self.g p_new p v * dt 0.5 * a_world * dt * dt v_new v a_world * dt R_new R exp_so3(w * dt) # --- 误差状态转移矩阵 --- F np.eye(15) F[0:3, 3:6] np.eye(3) * dt F[3:6, 6:9] -R skew(a) * dt F[3:6, 9:12] -R * dt F[6:9, 6:9] exp_so3(-w * dt) # 使用闭式解 F[6:9, 12:15] -np.eye(3) * dt # --- 噪声映射矩阵 --- G np.zeros((15, 12)) G[3:6, 0:3] -R G[6:9, 3:6] -np.eye(3) G[9:12, 6:9] np.eye(3) G[12:15, 9:12] np.eye(3) # --- 协方差传播 --- self.P F self.P F.T G self.Qc G.T * dt # --- 写回标称状态 --- self.p, self.v, self.R p_new, v_new, R_new三个细节值得说。第一位置用二阶积分p v·dt 0.5·a·dt²而不是简单的一阶p v·dt虽然在高频下差别不大但对齐 GNSS 的时候能省一点系统偏差。第二exp_so3(-w*dt)在 w 的模长接近零时会退化所以上面的实现对小于 1e-10 的情况做了泰勒展开兜底否则phi/theta会除零。第三self.Qc是 12×12 的对角阵顺序是[n_a, n_g, n_ba, n_bg]一定要跟 G 的列顺序对上这个错位是新手最常见的 bug——现象是滤波器能跑但零偏收敛极慢。3.3 更新步实现与 Joseph 形式通用的更新函数注意用 Joseph 形式保证数值稳定性def kalman_update(self, r, H, Rk): S H self.P H.T Rk # 用 solve 而不是求逆数值上更稳 K np.linalg.solve(S, H self.P).T dx K r I_KH np.eye(15) - K H self.P I_KH self.P I_KH.T K Rk K.T self.P 0.5 * (self.P self.P.T) # 强制对称 self.inject(dx)inject就是前面说的注入加重置def inject(self, dx): dtheta dx[6:9] self.p dx[0:3] self.v dx[3:6] self.R self.R exp_so3(dtheta) self.ba dx[9:12] self.bg dx[12:15] G_reset np.eye(15) G_reset[6:9, 6:9] np.eye(3) - 0.5 * skew(dtheta) self.P G_reset self.P G_reset.T self.P 0.5 * (self.P self.P.T)具体到某一种观测只需要写它的残差和雅可比。位置观测最简单def update_position(self, p_meas, Rk): r p_meas - self.p H np.zeros((3, 15)) H[0:3, 0:3] np.eye(3) self.kalman_update(r, H, Rk)姿态观测这一块是 ES-EKF 最舒服的地方。如果外界给了你一个姿态测量R_meas残差直接用对数映射算def update_attitude(self, R_meas, Rk): r log_so3(self.R.T R_meas) H np.zeros((3, 15)) H[0:3, 6:9] np.eye(3) self.kalman_update(r, H, Rk)雅可比就是单位阵漂亮得不像话。如果换成直接在大状态上做 EKF这里的雅可比会变成一个包含四元数各项的 4×4 矩阵还得处理模长约束。对比一下就明白为什么大家愿意折腾那套误差状态的符号了。3.4 数值稳定性处理清单P 矩阵跑到负定是新手最容易遇到的崩溃点下面这几条是我在多个项目里固定加上的保险。对称化每次预测和更新之后执行P 0.5*(P P.T)。浮点累加会让 P 轻微不对称长时间跑下来不对称量会放大最后触发Cholesky失败。Joseph 形式更新时用(I-KH)P(I-KH)ᵀ KRKᵀ而不是(I-KH)P。前者的数值稳定性好得多即使 K 计算有微小误差也能保持 P 半正定。避免显式求逆S⁻¹一律用np.linalg.solve或者scipy.linalg.cho_solve后者更快也更稳。特征值钳位调试阶段可以在预测后检查np.linalg.eigvalsh(P)的最小值如果小于 0 就打印警告。正式版本里我一般不钳位因为出现问题应该去查根因而不是掩盖。量纲归一化位置单位用米、姿态误差用弧度、零偏用原始单位。如果姿态误差用度那 H 矩阵和 P 的量级会差出 300 倍S 矩阵的条件数会变得很难看。不要在同一帧内重复注入多个观测到达时可以批量更新把残差和 H 上下拼接后做一次更新也可以顺序更新。顺序更新每次都要注入和重置虽然数学上等价但反复重置会引入额外的舍入误差我的习惯是同一时刻的观测打包成一次更新。4. 观测接入实战IMU 之外的传感器怎么喂进去4.1 位置速度观测GNSS 与轮速里程计GNSS 给位置是最常见的用法。直接拿经纬高换算到局部 ENU 坐标然后走update_position。要注意的是消费级模块的定位噪声高度相关不是白噪声而且有跳点。我的处理方式是在外面套一层卡方检验算NIS rᵀS⁻¹r自由度是 3如果 NIS 超过阈值比如 16对应 99.7% 置信就认为这一帧是野值直接丢弃不更新。这一步能挡掉绝大多数高架桥下、城市峡谷里的跳点。轮速里程计是另一类非常实用的观测。差速底盘可以拿到左右轮速转成机体前进速度v_b观测方程是v_body_meas Rᵀ v v_noise。对应的残差和雅可比def update_odom(self, v_body_meas, Rk): r v_body_meas - self.R.T self.v H np.zeros((3, 15)) H[0:3, 3:6] self.R.T H[0:3, 6:9] skew(self.R.T self.v) # 对 δθ 的雅可比 self.kalman_update(r, H, Rk)注意H[0:3, 6:9]这一块千万不要漏。它的物理含义是姿态误差会让机体系速度的投影方向偏掉。漏掉它的后果是滤波器会在姿态有误差时把方向偏差误判成速度大小的偏差导致速度估计出现周期性抖动。我第一次写的时候就是漏了这项现象是机器人直行时速度估计跟着航向角呈正弦波动查了两天才找到。轮速里程计有个致命弱点打滑。一旦轮子空转观测值会严重偏大。工程上一般做两件事一是用加计的纵向加速度跟轮速微分做一致性检验二是给里程计的观测噪声设一个跟加速度相关的自适应值加速度大时噪声放大。这两招都不完美但比什么都做强。4.2 姿态观测视觉位姿与磁力计视觉惯性里程计VIO里视觉前端给出的位姿可以作为一个绝对观测直接喂进来。姿态部分用 4.2 节那个简洁的写法就行。位置部分要注意视觉的尺度问题——单目 VIO 的尺度是漂的所以要么用双目/深度要么把尺度也放进状态里估计。尺度这东西跟速度、零偏都有耦合放进去之后可观性会变差得配合足够的运动激励才收敛。磁力计单独用是很危险的室内钢铁结构、电机、电流线都会让地磁方向产生几十度的偏差。我的惯例是只有在长时间没有其他航向观测、并且检测到磁力计数据的一致性良好时模长接近当地地磁总场强度、三轴变化平滑才允许用磁力计做一次弱航向观测观测噪声设得很大比如 20 度。用的话观测模型是取磁场水平分量算航向角然后由航向角造一个虚拟姿态观测。别忘了磁力计本身也需要标定硬磁、软磁没标定的磁力计数据基本没法用。我在一个室内机器人上就因为偷懒没做软磁标定结果机器人每次经过某段货架航向就会被拽偏五六度跑完一圈回原点差了将近一米。4.3 ZUPT 零速修正静止检测怎么做才不误触发零速修正Zero Velocity UpdateZUPT是性价比最高的一个技巧尤其适合步行者导航、手持设备和间歇性运动的机器人。原理非常简单当检测到载体静止时把速度当成观测量 0 加进去H [0, I, 0, 0, 0]残差就是0 - v。难度全在静止检测上。只靠加计模长接近 9.81 是不够的因为匀速直线运动时加计模长也接近 9.81。我常用的判据是三个条件同时成立陀螺三轴模长小于阈值比如 0.5 deg/s这是最关键的判据因为匀速运动时陀螺读数不为零加计三轴在滑动窗口0.2 到 0.5 秒内的方差小于阈值说明没有振动加计模长在 9.81 附近的一个宽区间内比如 ±0.3 m/s²排除自由落体。窗口长度需要权衡太短容易误触发比如走路时的脚掌触地瞬间太长会漏掉短暂停顿。我一般用 0.3 秒的滑动窗并且要求连续 N 帧都满足条件才真正进入 ZUPT 状态退出时也要有滞回避免在阈值附近反复横跳。ZUPT 生效期间速度和位置的不确定性会显著下降但航向的不确定性并不会。因为零速只能约束速度约束不了朝向。别指望靠 ZUPT 把 yaw 漂移治好它治不了。4.4 外参与时间偏移的在线估计如果 IMU 和相机或者 IMU 和轮速计之间的外参不准融合结果会出现系统性的偏差。平移外参误差主要表现为位置残差里的常值偏置旋转外参误差主要表现为姿态残差在运动时的规律性偏差。标定的方法无非两种离线用标定板或者专用轨迹做一次或者在线把外参放进状态里一起估计。在线估计外参的代价是状态维度膨胀每增加一个 3 维外参状态加 3 维而且可观性依赖运动激励。我在实际项目里更倾向于离线标定 在线只估计时间偏移。时间偏移IMU 和相机的时间戳差了 td 秒在高动态下影响极大一两毫秒的偏移在快速旋转时就能造成好几度的姿态误差。把 td 作为一个标量状态放进去观测雅可比用姿态残差对 td 的导数近似成角速度乘以残差方向收敛得还算快。外参标定的实操心得标定轨迹一定要包含三个轴的充分旋转尤其是绕重力的 yaw 旋转。只做俯仰和横滚的话IMU 和相机之间的航向安装角是标不出来的因为它和重力方向的可观测性正交。5. 排查实录姿态漂、协方差发散、输出跳变到底怎么回事5.1 协方差发散与滤波器自杀现象是跑一段时间后 P 的对角线指数增长位置估计开始剧烈抖动最后数值溢出。这通常不是单一原因按我的排查顺序是这样的。先看可观性。如果某个状态的观测量长期为零它的协方差就会一直涨。最常见的例子是纯 IMU 位置观测的系统里加速度计零偏在缺少旋转激励时不可观零偏协方差会一路涨到天上去。这时候滤波器其实没崩只是因为零偏不可观它必须用膨胀的协方差来表达我不确定而这会污染到速度估计。解决办法是加约束要么用实际的旋转运动激励它要么给零偏的协方差设一个上限本质上是在告诉滤波器零偏不可能超过这个范围这是合理的先验。再看过程噪声。Q 设大了协方差膨胀快滤波器会过度依赖观测表现为跟踪噪声大、输出抖动Q 设小了协方差增长慢滤波器会过度自信表现为慢慢偏离真值、观测被忽略。判断哪个方向的诀窍是看 NIS 统计量的均值NIS 均值远大于自由度说明协方差偏小过度自信远小于自由度说明协方差偏大过度保守。这个诊断花五分钟就能做能省掉大量瞎猜。最后看数值。跑几万步之后如果 P 出现了负特征值基本可以确定是累加误差造成的。检查是不是漏了对称化、是不是用了(I-KH)P而不是 Joseph 形式。5.2 yaw 漂移与可观测性yaw 漂移是 ES-EKF 最经典的痛点也是最多人问的问题。根源在于只有 IMU 加位置观测的系统里航向角、北向速度、东向加速度计零偏这三者构成一个不可观测子空间。直观解释是这样如果我把整条轨迹旋转一个小角度同时把北向速度做相应的调整再微调一下加速度计零偏最终产生的位置观测序列可以完全一样。也就是说光看位置轨迹你无法区分我朝向偏了和我速度偏了。所以滤波器只能靠积分陀螺来维持航向陀螺零偏一旦漂yaw 就跟着漂而且没有任何观测能纠正它。打破这个困局只有三条路一是引入绝对航向观测双天线 GNSS 的基线航向、磁力计、视觉给出绝对朝向二是引入从运动学约束推出的航向观测比如车辆的非完整约束横向速度理论上为零三是保证轨迹有足够的旋转励磁让陀螺零偏可观测。第二点在实际中很好用四轮车的横向速度约束是一个很强的伪观测实现只需要一行 H 矩阵。别指望通过调参解决 yaw 漂移。调参只能改变漂移的速率和形状改变不了漂移的必然性。这是结构性的可观测性问题只能通过增加信息源解决。5.3 常见问题速查表下面这张表是我和同事们的病历本大部分现场问题都能在里面找到影子。现象可能原因排查手段处理方式静置时高度缓慢下漂世界系/重力方向约定不一致打印静止时 a_world 的 z 分量检查 g 的符号与坐标系静置时速度缓慢增长加计零偏未被观测重力泄漏看零偏估计是否持续漂用 ZUPT 或提高加计噪声密度yaw 缓慢漂移陀螺零偏不可观看陀螺零偏协方差是否发散加磁力计/双天线/横向速度约束姿态输出周期性抖动速度雅可比漏了对姿态的偏导检查 H 矩阵非零块补上 skew(Rᵀv) 项收到观测后输出跳变观测噪声设太小或残差未做野值剔除统计 NIS 分布提高 R加卡方检验协方差出现负特征值未做对称化或用错更新形式检查 eigvalsh(P)对称化 Joseph 形式更新后滤波器反而更差误差注入与重置顺序写反打印注入前后标称状态先注入再重置重置用 G_reset收敛极慢G 矩阵列顺序与 Qc 顺序不匹配打印 G 和 Qc 的对角对齐噪声顺序与单位快速旋转时姿态偏离姿态传播时未用指数映射检查大角速度下的姿态用 exp_so3 做标称传播长时间运行后发散浮点累积 缺少周期性重正交检查 R 的正交性误差每帧对 R 做一次正交化6. 我个人踩坑后的几条建议6.1 初始化与对齐初始化的质量决定了后面能跑多远。我一般分三步做。第一步是静置对齐让设备静止 2 到 5 秒用这段时间的陀螺均值作为零偏初值用加计均值反解初始俯仰和横滚。加计反解姿态的时候要注意静止状态下加计测的完全部是重力所以R R_from_gravity(a_m / |a_m|)。这一步的精度受加计噪声影响通常能到 0.5 度以内。第二步是航向初始化如果没有任何航向观测就把初始 yaw 设成 0但协方差要设大比如 30 度对应的方差告诉滤波器我不知道朝向。如果把初始 yaw 协方差设得很小那么在遇到第一个航向观测之前滤波器会一直坚信这个错误的朝向等观测来了会产生一次大跳变。第三步是零偏协方差初始化陀螺零偏的初始标准差建议设成 1 到 2 deg/s加计零偏设成 0.1 到 0.2 m/s²都比典型真值大不少给滤波器足够的调整空间。关于姿态初始化有个细节从加计反解姿态时yaw 是完全不可知的所以要显式地把 yaw 自由度留出来。我见过有人用R_from_gravity之后顺手归一化了一个随意的朝向结果 yaw 协方差被错误地压小了。6.2 验证与评估评价一个 ES-EKF 好不好别只看最终的轨迹误差。我一般看四个指标。第一是残差的统计特性正常的残差应该接近零均值白噪声如果出现明显的偏置或者周期性说明模型有问题。第二是 NIS 的一致性这是最有力的工具直接反映协方差是否合理。第三是协方差收敛曲线所有可观状态的协方差都应该收敛到稳态如果某一路一直不降说明它不可观需要确认这是不是设计意图。第四是零偏估计的稳定性陀螺和加计零偏的估计值应该在真值附近小幅波动如果持续单调漂移说明观测信息不足或者 Q 设得不对。我还会做一次回放测试把同一段数据用不同参数配置跑几遍比较轨迹的差异。如果参数微调导致结果差异很大说明系统鲁棒性不够很可能有状态处于临界可观测的状态。这个测试比单次跑通更能说明问题。最后分享一个小技巧专门针对调试阶段把每次观测更新后的δx̂的模长打印出来。正常情况下它应该是毫弧度、厘米量级的。如果某次更新之后δx̂的模长突然变成几米或者几十度那这一帧观测几乎肯定有问题——要么是野值要么是你的某个雅可比符号写反了。这个指标比看最终轨迹敏感得多能在问题酿成大错之前就提醒你。
