第一次把误差状态扩展卡尔曼滤波Error State Extended Kalman FilterES-EKF跑通的那天我盯着曲线看了很久同一块消费级 IMU、同一段数据、同一套噪声参数只是把状态从四元数直接进滤波换成名义状态加误差状态姿态角的慢漂移就肉眼可见地小了一截横滚和俯仰在静止段的抖动也从零点几度压到了零点零几度。ES-EKF 不是什么新算法它在惯性导航、组合导航、视觉惯性里程计里已经被用了很多年但真正自己动手写一遍的人大概率会在三个地方卡住姿态误差到底该左乘还是右乘、残差算完以后怎么注入回名义状态、协方差在重置时要不要跟着变换。这篇文章就围绕这三个卡点把我从零实现一套 15 维 ES-EKF 的完整思路、公式、代码骨架和踩坑记录摊开讲一遍。内容适合两类人看一类是做机器人、无人机、自动驾驶定位手上已经有 IMU 数据、想把姿态和速度估计做扎实的工程师另一类是学生或者刚转方向的朋友知道扩展卡尔曼滤波是什么但没搞明白误差状态这几个字到底省了什么事。全篇不依赖任何特定硬件公式和代码都是通用的你拿一块几百块的 MEMS IMU 加一个能跑 Python 的板子就能复现。1. 先搞清 ES-EKF 到底解决什么问题1.1 直接法 EKF 在姿态估计上的三个隐性代价最直观的做法是把姿态当成状态直接塞进扩展卡尔曼滤波。比如用四元数表示姿态状态向量写成[p, v, q, b_g, b_a]其中 q 是四元数。这套方案在论文里能跑但落到代码里有三个绕不开的问题。第一个问题是维度虚高。四元数有四个分量但姿态只有三个自由度因为存在单位模长约束||q|| 1。这意味着 4×4 的姿态协方差矩阵一定是奇异的理论上不可逆。你在代码里求逆的时候数值上可能侥幸不出错但矩阵条件数会很差长时间跑下来协方差会出现负特征值滤波器直接崩给你看。有人用伪逆或者加个小正则项糊过去本质上是把误差藏起来了不是消除。第二个问题是归一化破坏高斯假设。四元数积分几十步以后模长会偏离 1你必须每一步都归一化。归一化是个非线性操作它会改变误差的统计分布而卡尔曼滤波的全部推导都建立在误差服从高斯分布、且经过线性变换后仍然服从高斯这个前提上。每次归一化都相当于往系统里注入一点非高斯的扰动短时间看不出来长时间就是缓慢积累的偏差。第三个问题最阴险q 和 -q 表示同一个姿态。这个双覆盖性质在残差计算时会直接翻脸。假设真实姿态是q_true [0.707, 0, 0.707, 0]滤波器估计出来的名义姿态是q_nom [-0.707, 0, -0.707, 0]两者物理上完全一样但你算个简单的四元数差值得到的残差是原来的两倍更新量直接朝着错误方向推。工程上解决办法是每次算残差前判断点积符号if dot(q_true, q_nom) 0: q_true -q_true这叫符号翻转处理。能解决但是补丁而且必须在每个观测更新处都记得写。注意如果你现在手上的滤波器姿态总是慢慢歪而不是突然炸先别急着调噪声参数先检查是不是四元数双覆盖没处理。这个坑的典型症状是姿态在 180 度附近跳变或者静止时姿态缓慢单向漂移。1.2 误差状态的核心思想大信号走非线性小信号走线性ES-EKF 的破局点在于把状态拆成两层。真实状态不再直接进滤波而是写成x_true x_nom ⊕ δx这里的x_nom叫名义状态δx叫误差状态⊕是广义加法——位置、速度、零偏这些向量直接用普通加法姿态用一个乘的操作。名义状态用完整的非线性运动学方程大步推进该积分积分、该旋转旋转不做任何线性化近似误差状态则始终保持在一个非常小的量级附近通常角度误差在毫弧度级别所有线性化都只在这个小量上做。这带来三个立刻能感受到的好处。第一线性化点永远在原点。普通 EKF 需要在当前估计值附近做泰勒展开估计值离真值越远丢掉的高阶项越大ES-EKF 里误差状态的展开点永远是 0因为误差状态本身就定义成离名义状态多远它天然接近 0二阶以上项小到可以忽略。第二姿态误差可以用最小参数化。误差姿态不需要用四元数表示直接用三维旋转向量δθ就够了因为它是个小角度量不存在模长约束问题也不需要归一化。第三协方差始终是满秩的。15 维状态对应 15 个独立自由度不用再面对奇异矩阵。打个生活化的比方你要记录一个城市到另一个城市的路程。直接法相当于把全部 1200 公里记在一个数字里稍微有点测量误差相对精度就很差ES-EKF 相当于记住主干路线走了 1200 公里然后单独记录最后一段偏了 3 米这 3 米永远是小数用小尺子量就够了精度还高。1.3 什么场景该上 ES-EKF什么场景别折腾不是所有姿态估计都值得上 ES-EKF。适合的场景有几条明确特征IMU 采样率很高通常 100 Hz 到 1 kHz需要在一个滤波周期内做几十次状态传播姿态必须进状态因为要估计陀螺零偏零偏和姿态是强耦合的有外部观测源比如 GNSS 位置、轮速计、磁力计、视觉特征点、或者最简单的零速检测算力受限跑不了因子图优化或者粒子滤波。反过来如果你的问题是一维温度估计或者只有一个低频传感器偶尔给个观测那用普通卡尔曼滤波就够了硬套 ES-EKF 只会把代码搞复杂。另外如果你有大量强非线性的观测模型比如鱼眼相机的重投影误差而且算力充足那用滑动窗口优化或者因子图往往比滤波更准因为滤波只用了当前时刻的信息历史信息被压缩进协方差里丢掉了不少。我自己的经验判断线只要你的系统里有 IMU而且 IMU 数据要用来做姿态和速度递推优先考虑 ES-EKF如果 IMU 只是辅助主力是多视图几何那可以考虑松耦合甚至只在优化里加 IMU 预积分约束。2. 状态量定义与符号约定把地基打正2.1 名义状态、真值状态、误差状态的三层关系先把状态清点一遍。一个标准的 15 维 ES-EKF 状态包含五组量每组三维加上可选的重力就是 18 维。我把它们的物理含义和常用坐标系列在下表里坐标系这块必须先定死中途换坐标系是新手最容易犯的错。状态块符号物理含义常用坐标系单位位置p机体在世界系下的位置世界系ENU 或 NEDm速度v机体在世界系下的速度世界系m/s姿态q机体系到世界系的旋转四元数无量纲陀螺零偏b_g角速度计的输出偏置机体系rad/s加速度计零偏b_a加速度计的输出偏置机体系m/s²三层关系写清楚真值 名义 ⊕ 误差。具体展开是p_true p_nom δp v_true v_nom δv q_true q_nom ⊗ Exp(δθ) b_g_true b_g_nom δb_g b_a_true b_a_nom δb_a这里的⊗是四元数乘法Exp(δθ)是把三维旋转向量映射成四元数。位置、速度、零偏都是向量误差直接相加就行姿态不行因为两个旋转的差不是相减而是一个旋转作用在另一个上。这个区别就是 ES-EKF 与普通 EKF 在写法上的分水岭。名义状态用 IMU 数据做实打实的非线性积分误差状态用卡尔曼滤波的预测和更新来处理最后把误差状态估出来的修正量注入回名义状态同时把误差状态清零。注意是清零不是保留——这一步后面会专门讲忘了清零等于把同一份修正用了两遍姿态会以两倍速度冲向真值然后开始震荡。2.2 姿态误差的四种参数化以及左右扰动之争姿态误差的定义方式不止一种这是让很多人卡住的第一个大坑。核心问题是Exp(δθ)是乘在名义姿态的右边还是左边右乘局部扰动 / 机体系扰动q_true q_nom ⊗ Exp(δθ)。物理含义是在机体系下真实姿态相对于名义姿态偏转了多少。因为 IMU 的角速度天然是在机体系下测量的这个约定的雅可比推导最顺手绝大多数开源实现视觉惯性里程计、多状态约束滤波这一脉都用这个。左乘全局扰动 / 世界系扰动q_true Exp(δθ) ⊗ q_nom。物理含义是在世界系下偏转了多少。某些纯 GNSS/INS 组合导航的教材用它因为位置速度误差都在世界系写起来符号统一。对比项右乘局部/机体系左乘全局/世界系关系式q_true q_nom ⊗ Exp(δθ)q_true Exp(δθ) ⊗ q_nom姿态误差动力学δθ -[ω]× δθ - δb_gδθ -R δb_g与陀螺零偏耦合直接耦合-I 块通过 R 耦合-R 块速度误差中的姿态项-R [a]× δθ-[R a]× δθ注入方式q ← q ⊗ Exp(δθ)q ← Exp(δθ) ⊗ q常见使用场景VIO、MSCKF、多数机器人栈部分 GNSS/INS 教材实现两种约定在小角度下一阶等价区别只体现在二阶项和实际代码里。关键不是选哪个而是选定之后从头到尾别换。我见过的最典型的 bug 是动力学矩阵按右乘推的注入的时候却按左乘写的结果是姿态在静止时缓慢旋转看起来像陀螺零偏估计不准其实是符号约定打架。判断方法很简单——把 IMU 完全静止放十分钟如果姿态慢慢地单向匀速旋转八成就是这个原因。还有第三种更绕的写法用Exp(-δθ)定义误差残差符号会整体翻过来。这种约定在协方差传播上没影响因为噪声输入矩阵同时变号G Q G^T不变但你调试的时候会一头雾水明明残差看着是负的修正确实往正的方向走。建议新手直接锁死右乘加Exp(δθ)把符号问题从变量里彻底消掉。2.3 惯性器件的噪声模型和单位换算噪声参数是整个滤波器里最玄学、也最容易被随手填的部分。有人直接把数据手册上零偏不稳定性 10 °/h这个数字填进Q矩阵然后抱怨滤波器要么发散要么不收敛。这里必须搞清楚三类噪声的区别。角度/速度随机游走白噪声对应 Allan 方差曲线斜率为 -1/2 的那一段是每个采样周期都在变的随机量。它用谱密度描述单位是rad/s/√Hz陀螺和m/s²/√Hz加速度计。数据手册上常给的是 ARW单位°/√h换算关系是σ_g [rad/s/√Hz] ARW [°/√h] × π / (180 × 60)代入一下ARW 0.2 °/√h 的消费级陀螺谱密度大约是 5.8e-5 rad/s/√Hz。加速度计如果手册给的是μg/√Hz直接乘 9.81e-6 就换成m/s²/√Hz。零偏不稳定性Allan 方差曲线谷底那个值单位也是°/h它描述的是零偏在长时间尺度上的慢变。它对应的是随机游走噪声不是白噪声应该进Q矩阵里零偏那一块的随机游走项。零偏常值这是标定能去掉的部分理论上不需要进Q但实际中因为温度漂移总会剩下一点。离散化以后一个采样周期 Δt 内各项的噪声方差大概是Q_v σ_a² × Δt Q_θ σ_g² × Δt Q_bg σ_bg² × Δt Q_ba σ_ba² × Δt提示这里的σ_bg和σ_ba是零偏随机游走的谱密度不是零偏本身的大小。零偏本身的量级比如 10 °/h应该用来设初始协方差P0不要混进Q。这两个地方填反了会导致滤波器一开始对零偏过度自信几十秒内速度估计就被零偏带跑。给一组参考初值消费级 MEMS IMU 常见范围是陀螺 ARW 0.1 到 0.5 °/√h加速度计噪声密度 100 到 500 μg/√Hz陀螺零偏 5 到 50 °/h加速度计零偏 1 到 10 mg。工业级或战术级器件数值会小一到两个数量级。不要迷信这些数字最终还是要用你自己器件的 Allan 方差曲线标定静态采两三个小时数据用开源工具跑一条 Allan 曲线比抄任何参考值都靠谱。3. 公式推导的关键几步推给真动手写代码的人3.1 名义状态递推该非线性就非线性名义状态的推进完全不做线性化IMU 给什么就用什么。假设陀螺输出角速度ω_m、加速度计输出比力a_m采样间隔 Δt那么a_world R(q_nom) · (a_m - b_a_nom) g_world p_nom ← p_nom v_nom · Δt 0.5 · a_world · Δt² v_nom ← v_nom a_world · Δt ω_corrected ω_m - b_g_nom q_nom ← q_nom ⊗ Exp(ω_corrected · Δt) b_g_nom, b_a_nom 保持不变几个容易写错的地方得强调。R(q_nom)是机体系到世界系的旋转矩阵加速度计测的是机体系下的比力必须先转到世界系再加地球重力。重力向量g_world的符号取决于你的世界系约定ENU东北天下重力指向 -z所以g_world [0, 0, -9.80665]NED北东地下重力指向 zg_world [0, 0, 9.80665]。这两个约定混用滤波器会立刻给你颜色看——静止时速度会以 9.8 m/s² 的加速度往下冲然后被零速修正硬按回来整个滤波器变成锯齿状震荡。四元数更新那一步Exp(ω Δt)的实部是cos(|ω|Δt/2)虚部是sin(|ω|Δt/2) · ω/|ω|。当角速度很小时直接除模长会出问题工程上用小角度近似|ω|Δt/2 1e-8时用[1, ωΔt/2]代替。这个细节不加设备静止时偶尔会蹦出 NaN。3.2 误差状态连续时间动力学每一块都要讲得清物理意义误差状态的连续时间方程是 ES-EKF 的核心右乘约定下写成这样δp δv δv -R [a_b]× δθ - R δb_a n_v δθ -[ω_b]× δθ - δb_g n_θ δb_g n_bg δb_a n_ba其中[·]×是反对称矩阵算子a_b a_m - b_a_nom是机体系下的比力估计值ω_b ω_m - b_g_nom是机体系下的角速度估计值。逐项解释一下为什么长这样。速度误差受姿态误差影响如果姿态估计偏了δθ那么把机体系比力转到世界系时方向就偏了投影到世界系的速度增量也跟着偏。这一项的系数是-R [a_b]×注意是比力在机体系下的反对称矩阵再左乘旋转矩阵不是先转世界系再取反对称这个顺序写反了结果完全不一样。姿态误差受陀螺零偏影响零偏估小了姿态积分就会持续往一个方向超调这一项直接是-I负单位阵耦合非常强这也是为什么陀螺零偏必须和姿态一起进状态。速度误差受加速度计零偏影响机体系零偏经旋转矩阵转到世界系系数是-R。姿态误差方程里的-[ω_b]× δθ这一项经常被漏掉。它的物理含义是姿态误差是在机体系下定义的而机体系自己在旋转所以误差向量本身也在被带着转。漏掉这一项在大角速度转动时比如无人机快速转向协方差传播会明显偏小滤波器会变得过度自信观测一进来就产生大跳变。3.3 离散化与状态转移矩阵的成型把上面的连续方程写成矩阵形式δx F δx G n按[δp, δv, δθ, δb_g, δb_a]的顺序排列F 是一个 15×15 的分块矩阵δpδvδθδb_gδb_aδp0I000δv00-R[a_b]×0-Rδθ00-[ω_b]×-I0δb_g00000δb_a00000噪声输入矩阵 G 只在 δv、δθ、δb_g、δb_a 四行有非零块分别是-R、-I、I、I对应加速度计白噪声、陀螺白噪声、陀螺零偏随机游走、加速度计零偏随机游走。离散化用一阶泰勒展开就够Φ I F Δt。如果要更稳可以加二阶项Φ I F Δt 0.5 F² Δt²。什么时候需要二阶我的经验是当采样周期内姿态变化超过 1 度或者比力变化剧烈比如落地的冲击、车辆急刹二阶项能明显改善协方差传播的准确性。1 kHz 采样的 IMU 用一阶完全够100 Hz 采样的建议加二阶算力代价只是多两次矩阵乘法。注意F里的R、[a_b]×、[ω_b]×在整个 Δt 内被当成常数处理这是标准做法。如果采样率足够高≥100 Hz这个近似引入的误差可以忽略。但如果你的 IMU 只有 20 Hz强烈建议在预测内部做子步细分比如每个滤波周期跑 5 次 4 ms 的小步而不是一次跑 20 ms。3.4 观测模型残差在名义状态上算雅可比在误差状态上求这是 ES-EKF 与普通 EKF 最不一样的地方也是最容易写反的地方。观测更新的流程是先用名义状态算出预测观测值h(x_nom)然后残差r z - h(x_nom)接着求观测雅可比注意是对误差状态求导H ∂h/∂δx而不是对名义状态求导。因为我们要估算的是误差状态观测对误差状态的敏感度才是卡尔曼增益需要的。以几个最常见的观测为例GNSS 或 UWB 给的位置观测h p_nom雅可比H [I₃ 0 0 0 0]非常简单因为位置误差直接相加。观测噪声R取 GNSS 的水平精度平方一般 1 到 5 米RTK 可以到厘米级。零速修正ZUPTz 0h v_nomH [0 I₃ 0 0 0]R取一个很小的值比如 1e-4。这一项是纯惯导系统能长时间工作的救命稻草只要检测到静止就触发。磁力计航向观测磁力计测得的是机体系下的磁场方向预测值是h R(q_nom)^T · m_world其中m_world是当地磁场在世界系下的参考方向。对右乘扰动求导q → q ⊗ Exp(δθ)意味着R → R Exp(δθ)所以R^T → Exp(-δθ) R^T ≈ (I - [δθ]×) R^T代入得到H_θ [h]×也就是预测磁场向量自己的反对称矩阵。这个结论很漂亮不用记推一遍就明白了。气压计高度观测h -p_nom.zENU 系下 z 轴向上高度越高气压越低符号自己按坐标系确定H [0 0 -1 0 0]配到对应的行上。提示残差一定要用测量值减预测值这个顺序并且和后面卡尔曼增益的符号保持一致。顺序反了滤波器不会发散但会以极慢的速度往错误方向收敛看起来像观测权重太低实际上你调 R 矩阵调到天亮也调不好。判断方法给一个明显偏离的观测看状态是往观测方向走还是往反方向走。3.5 注入与误差重置九成的人都在这翻过车卡尔曼更新算出误差状态的估计值δx之后要把它注入回名义状态p_nom ← p_nom δp v_nom ← v_nom δv q_nom ← q_nom ⊗ Exp(δθ) b_g_nom ← b_g_nom δb_g b_a_nom ← b_a_nom δb_a δx ← 0这里有两个细节。第一注入完必须清零误差状态。误差状态的定义是真值相对名义状态的偏差注入之后名义状态就等于新的估计值了误差状态当然应该归零。如果忘了清零下一轮预测会从这个非零的误差继续外推相当于把修正量用了两遍姿态会以两倍速度冲向估计值然后过冲震荡。第二协方差还要跟着变换一次。因为姿态误差被注入以后误差状态的参考点从旧的名义姿态变成了新的名义姿态误差量的定义变了协方差也得跟着做一次相似变换δθ_reset -δθ G I₁₅但姿态那一块替换成 (I₃ 0.5 [δθ]×) P ← G P G^T这个 Jacobian 是因为四元数乘法的二阶项带来的当δθ很小毫弧度级时G接近单位阵P的变化可以忽略。很多实现直接跳过这一步短期没问题但在高噪声、高频次更新的场景下会积累出偏差。我的建议是加上代价只是一次矩阵乘法还能顺手把P重新对称化P 0.5 * (P P.T)对称化这一步非常值得加。浮点运算累积的舍入误差会让P逐渐失去对称性而所有卡尔曼公式的推导都假设P对称一旦不对称特征值可能变负滤波器就会输出无意义的增益。每次预测和更新后都对称化一次是成本最低的稳定性保障。4. 代码落地一套可跑通的 ES-EKF 骨架4.1 数据结构与状态打包顺序代码组织的第一件事是把状态打包顺序定死并且写成常量不要在代码里到处硬编码索引。下面这套结构我在几个项目里都用过够用。import numpy as np IDX_P slice(0, 3) IDX_V slice(3, 6) IDX_TH slice(6, 9) IDX_BG slice(9, 12) IDX_BA slice(12, 15) DIM 15 class NominalState: def __init__(self): self.p np.zeros(3) self.v np.zeros(3) self.q np.array([1.0, 0.0, 0.0, 0.0]) # w, x, y, z self.bg np.zeros(3) self.ba np.zeros(3)四元数我习惯用[w, x, y, z]的顺序和 Eigen 的Quaterniond一致。如果你的代码里用了别的库比如 ROS 的tf用[x, y, z, w]一定要在接口处做一次显式转换并注释清楚这是找不到原因的鬼畜漂移的常见来源。4.2 预测步80% 的计算量在这里预测步分三件事推进名义状态、算 F 矩阵、传播协方差。写成函数大概是这个结构。def skew(v): return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) def quat_to_R(q): w, x, y, z q return np.array([ [1-2*(y*yz*z), 2*(x*y-w*z), 2*(x*zw*y)], [2*(x*yw*z), 1-2*(x*xz*z), 2*(y*z-w*x)], [2*(x*z-w*y), 2*(y*zw*x), 1-2*(x*xy*y)]]) def quat_mul(a, b): aw, ax, ay, az a bw, bx, by, bz b return np.array([ aw*bw - ax*bx - ay*by - az*bz, aw*bx ax*bw ay*bz - az*by, aw*by - ax*bz ay*bw az*bx, aw*bz ax*by - ay*bx az*bw]) def exp_map(theta): ang np.linalg.norm(theta) if ang 1e-8: return np.array([1.0, theta[0]/2, theta[1]/2, theta[2]/2]) axis theta / ang return np.concatenate(([np.cos(ang/2)], np.sin(ang/2) * axis))预测主循环里R quat_to_R(q)只算一次因为F里的三个块都用到它。def predict(nom, P, gyro, accel, dt, Q, g_world): R quat_to_R(nom.q) a_b accel - nom.ba w_b gyro - nom.bg # 名义状态推进 a_w R a_b g_world nom.p nom.p nom.v * dt 0.5 * a_w * dt * dt nom.v nom.v a_w * dt nom.q quat_mul(nom.q, exp_map(w_b * dt)) nom.q nom.q / np.linalg.norm(nom.q) # 误差状态转移矩阵 F np.zeros((DIM, DIM)) F[IDX_P, IDX_V] np.eye(3) F[IDX_V, IDX_TH] -R skew(a_b) F[IDX_V, IDX_BA] -R F[IDX_TH, IDX_TH] -skew(w_b) F[IDX_TH, IDX_BG] -np.eye(3) Phi np.eye(DIM) F * dt 0.5 * (F F) * dt * dt P Phi P Phi.T Q P 0.5 * (P P.T) return nom, P有一个细节值得说四元数归一化我放在了乘完之后。有人担心归一化引入偏差但对Exp(ω Δt)这种单位四元数q ⊗ Exp(·)的模长偏差在一阶上可以忽略因为两个单位四元数相乘的结果模长仍然是 1数学上是精确的偏差全部来自浮点截断。每步归一化是成本极低的保险。4.3 更新步以 ZUPT 为例走完整流程更新步的骨架对所有观测都一样差别只在h和H。def update(nom, P, z, h, H, R, gate_thresholdNone): r z - h S H P H.T R # 卡方检验剔除外点 if gate_threshold is not None: d float(r.T np.linalg.solve(S, r)) if d gate_threshold: return nom, P, False # 该观测被拒绝 K np.linalg.solve(S.T, (P H.T).T).T dx K r I_KH np.eye(DIM) - K H P I_KH P I_KH.T K R K.T # Joseph 形式 P 0.5 * (P P.T) # 注入 nom.p nom.p dx[IDX_P] nom.v nom.v dx[IDX_V] nom.q quat_mul(nom.q, exp_map(dx[IDX_TH])) nom.q nom.q / np.linalg.norm(nom.q) nom.bg nom.bg dx[IDX_BG] nom.ba nom.ba dx[IDX_BA] # 姿态误差重置的协方差修正 G np.eye(DIM) G[IDX_TH, IDX_TH] 0.5 * skew(dx[IDX_TH]) P G P G.T P 0.5 * (P P.T) return nom, P, True调用 ZUPT 的时候z np.zeros(3)h nom.vH是[0 I 0 0 0]R np.eye(3) * 1e-4。这里有两个工程细节值得展开。第一用solve而不是inv。np.linalg.inv(S)在数值上比解线性方程组差不少尤其当S条件数大的时候。S是对称正定的理论上可以用 Cholesky 分解速度更快更稳np.linalg.solve内部对一般矩阵已经用了 LU 分解够用。第二用 Joseph 形式更新协方差。教科书上的P (I - KH) P在理论上等价但数值上不保证P对称正定长时间跑会出现负特征值。Joseph 形式P (I-KH) P (I-KH)^T K R K^T在数值上稳定得多代价是多两次矩阵乘法。这个选择在嵌入式平台上要权衡但如果是 PC 端或中高端处理器没有理由不用。4.4 数值稳定性三个必做的卫生习惯除了上面说的对称化和 Joseph 形式还有三件事我强烈建议做。四元数每步归一化前面说过了成本极低。协方差矩阵的对角线做下界保护比如np.maximum(np.diag(P), 1e-12)防止某个状态的方差被压到 0之后所有观测都推不动它。这个在小Q大R的参数组合下经常出现症状是滤波器过于自信观测进来只肯动一点点感觉像权重调得太低。定期检查 P 的最小特征值负数就说明数值已经开始烂了早发现早处理。注意Φ I FΔt 0.5F²Δt²里的F²在 15 维下是 225 次乘加看起来不多但在 1 kHz 的嵌入式实现里很多人会选择只保留一阶项并在 100 Hz 以上做滤波中间用简单的姿态积分补足。这个取舍完全合理二阶项的主要收益在低采样率场景。5. 实测中踩过的坑与排查速查表5.1 姿态相关问题从慢慢歪到突然炸姿态缓慢漂移是最高频的问题但原因有好几种要分开判断。只有偏航角yaw慢慢漂横滚和俯仰很稳这是正常现象不是 bug。因为加速度计能观测横滚和俯仰通过重力方向但没有任何传感器能观测偏航除非你加了磁力计或者 GNSS 双天线。所以偏航漂移是系统本身不可观测不是滤波器写错了。想让偏航稳住加磁力计做航向观测或者用视觉/GNSS 提供航向参考。横滚和俯仰也漂而且漂得比较快八成是加速度计观测没生效或者重力向量的符号/坐标系错了。检查方法很简单把设备静止放置看加速度计输出的方向和你代码里g_world的方向是否一致。如果竖直放置时加速度计输出[0, 0, 9.81]机体系 z 轴向上而你的世界系是 ENUz 轴向上那么静止时a_world应该接近 0如果算出接近 19.6就是重力符号反了。姿态在静止时缓慢单向旋转右乘/左乘约定混用或者陀螺零偏的符号错了。前者前面讲过后者的判断方法是看零偏估计值是否发散。如果陀螺零偏估计到了 0.1 rad/s 这种明显不合理的量级说明它在补偿一个系统性的符号错误而不是真实的偏置。姿态突然跳变通常是四元数双覆盖没处理或者某次观测残差异常大导致增益矩阵爆炸。前者在 180 度附近出现后者伴随着协方差突然收缩。解决办法是加卡方检验和残差限幅。5.2 协方差相关问题从过度自信到直接发散协方差相关的症状分两个极端。滤波器不收敛观测进来几乎不动P太小。原因可能有三Q给得太小R给得太大或者某个状态的P被压成了接近 0。先看残差如果残差长期保持很大且不下降就是P太小或者Q太小如果残差小但估计就是不准那是观测模型有问题不是协方差问题。滤波器输出剧烈震荡数值发散Q给得太大或者P出现负特征值。把Q调小一个数量级试试同时检查对称化和 Joseph 形式有没有加。还有个小概率原因是采样时间戳出错比如某两次 IMU 数据时间相同或者倒序导致dt为负或者为零。这种 bug 在协方差上表现为突然的、无规律的跳变。5.3 时间同步与外参位置一跳一跳的元凶这类问题的症状很有辨识度观测量一进来状态就跳跳完以后慢慢衰减下一帧又跳。根源通常是时间戳没对齐。IMU 是高频的100 到 1000 Hz相机、GNSS、轮速计往往是低频的10 到 50 Hz。如果低频观测的时间戳和 IMU 的积分时刻差了 10 ms而系统正在以 10 m/s 移动位置残差就会凭空多出 10 cm速度残差多出好几米每秒滤波器会误以为速度突变产生一次错误修正。解决办法是把观测时间戳和 IMU 时间戳统一到同一个时钟基准并且做插值对齐。在实践中我一般会保留一小段 IMU 数据缓存把观测时刻的 IMU 数据插值出来先预测到这个时刻再更新然后再把剩余时间推完。外参问题同理。IMU 通常不装在设备质心上存在杆臂lever arm。设备旋转时IMU 感受到的加速度里包含向心和切向分量如果你忽略了杆臂这部分加速度会被错误地当成真实运动。杆臂在低速情况下影响很小但如果设备频繁旋转比如手持设备、无人机杆臂带来的误差可以到几十厘米量级值得标定。标定方法一般用旋转台或者手写一段包含多轴旋转的动作把杆臂作为状态一起优化Kalibr 这类工具就是干这个的。5.4 问题排查速查表症状最可能的原因首选排查动作只有偏航漂移航向不可观测正常现象加磁力计或双天线 GNSS 观测横滚俯仰也漂重力符号或坐标系约定错误静止时检查 a_world 是否接近 0静止时姿态单向旋转扰动约定混用或零偏符号错检查注入用的是左乘还是右乘姿态在 180° 附近跳变四元数双覆盖未处理残差计算前做符号翻转观测进来几乎不动P 或 Q 太小R 太大残差和 P 对角线一起看输出剧烈震荡Q 太大或 P 有负特征值减小 Q加对称化和 Joseph 形式每次观测都跳一下时间戳未对齐或杆臂未标定打印观测时刻与积分时刻的差值静止时速度不为零ZUPT 阈值太严或零偏估计慢放宽静止检测阈值检查 R_zupt出现 NaN四元数归一化除零或 dt 为零检查 exp_map 的小角度分支和 dt 保护位置缓慢单向漂移加速度计零偏未被观测修正加入零速或位置观测检查 ba 可观测性6. 工程经验与参数调优心得6.1 噪声参数怎么给初值先粗后细别一上来就标定很多人一上来就想把 Allan 方差标得漂漂亮亮结果卡在数据采集上一卡就是一周。我建议的顺序是先用典型值把滤波器跑起来看曲线是否合理再去标定。初始协方差P0的设置逻辑是我对我自己的初始状态有多不确定。位置初始一般设得比较小静止启动时 1e-4 量级速度也是。姿态的初始协方差取决于你是否有初始对准如果有加速度计做初始水平对准横滚俯仰可以设 1e-4 rad²偏航设 1e-2 rad²因为没法对如果完全没有对准全部设大一点让滤波器自己去收敛。零偏的初始协方差可以设得比较大比如陀螺 0.01 rad/s 平方给滤波器足够的空间去估计。Q矩阵应该反映真实器件的噪声水平宁可给大一点也别给小。给小的后果是滤波器过度自信观测一来就是大跳变给大的后果是估计有点抖但至少不发散。这个取舍在天平上明显偏向给大。R矩阵就是观测噪声的协方差。GNSS 位置观测的R取水平精度平方但要注意 GNSS 的误差不是各向同性的而且有很强的时序相关性简单的对角R会低估相关误差的影响。工程上的折中是给R乘一个膨胀因子1.5 到 3 倍或者根据卫星数和 HDOP 动态调整。6.2 观测的鲁棒化卡方检验和残差限幅真实数据里总有外点。GNSS 在城市峡谷里会突然给你一个偏出 50 米的位置视觉特征点会匹配错误磁力计会被电机干扰。不做鲁棒化处理这些外点会直接污染状态。卡方检验是最常用的手段算马氏距离d r^T S^{-1} r如果d超过卡方分布的某个分位数就拒绝这一帧观测。常用阈值是一维观测用 6.6399% 分位三维观测用 11.3499% 分位二维用 9.21。这些数字可以查卡方分布表也可以用scipy.stats.chi2.ppf(0.99, dof)现算。残差限幅是更粗暴但有效的补充直接对残差做r np.clip(r, -3*sigma, 3*sigma)。它的问题是会引入偏差外点不是被拒绝而是被压缩但在实时系统里容错性更好。我一般两个都用卡方检验拒绝明显离谱的残差限幅处理边缘情况。提示拒绝率要监控。如果卡方检验拒绝了超过 10% 的观测说明不是外点问题而是你的P或者R严重不匹配。这个时候应该去调参数而不是把阈值放宽掩耳盗铃。6.3 可观测性与退化场景知道什么时候不能信ES-EKF 有一个绕不开的局限它只能观测到系统可观测的方向。算一下可观测性矩阵当然最严谨但工程上更实用的方法是记住几条经验。不加任何外部观测的纯惯导只有位置、速度、姿态中受重力约束的部分可观测。横滚和俯仰能被加速度计约束住偏航完全不可观测速度在短时间内可观测因为加速度计能测到变化但零偏会慢慢把速度带跑。这就是为什么纯惯导必须加 ZUPT 或者外部位置。ZUPT 场景静止时速度可观测水平姿态可观测偏航仍然不可观测除非加磁力计零偏通过长期静止可以估出来一部分但零偏和重力方向存在耦合静置时间越长估计越准。GNSS 位置观测位置和速度可观测姿态主要通过重力约束偏航在运动过程中可以通过航向变化间接观测到。但如果车辆一直直线行驶偏航的可观测性很弱这也是为什么 GNSS/INS 组合导航在长直线路段偏航容易漂。视觉观测姿态和位置都能被强约束前提是特征点足够多而且有合适的视差。单目纯旋转时尺度不可观测这一点和 ES-EKF 无关是几何本身的性质。写 ES-EKF 的时候如果发现加了观测以后对应状态几乎不动先别怀疑滤波器先想想这个状态在当前场景下是不是真的可观测。我吃过这个亏在一个纯直线运动的测试里调了一整天的偏航参数最后发现根本就是不可观测白忙一场。最后提一个小技巧也是我最近比较喜欢用的把估计出来的协方差和实际误差对着看。跑一段真值可获取的数据比如室内动捕或者高精度组合导航作为参考把P的对角线开根号得到标准差和真实误差曲线画在一起。理想情况下真实误差应该落在 3σ 带里 95% 以上的时间如果经常跑出去说明P太小或者观测模型有偏差如果 3σ 带宽得离谱说明P太大滤波器其实在瞎猜。这个一致性检验比任何单项调参都有用因为它一次性把Q、R、P0、观测模型、时间同步全部检验了一遍。