1. 从一次姿态角缓慢爬升说起ES-EKF 到底在解决什么问题我最早接触 ES-EKF 不是因为读论文而是因为被一个现象折腾了整整两周用普通扩展卡尔曼滤波做 IMU 和卫星定位的组合导航静态数据跑得漂漂亮亮一到动态数据里俯仰角就开始缓慢往上爬跑个一两分钟横滚也开始跟着偏最后整个姿态都糊了。查了一圈传感器标定、零偏、时间同步全都没问题问题出在状态量的定义方式上。这就是 ES-EKFError State Extended Kalman Filter误差状态扩展卡尔曼滤波要解决的核心痛点。它不直接让滤波器估计姿态、速度、位置这些大状态而是让滤波器去估计这些大状态上面的小误差再把误差叠加回标称状态。听起来像是绕了个弯但正是这个绕弯让滤波器在姿态这类带有约束的变量上变得稳定得多。这篇东西写给两类人看一类是已经写过 EKF但被四元数归一化、协方差失去正定性、姿态缓慢漂移这些问题卡住的人另一类是正在做惯性导航、视觉惯性里程计、多传感器融合想上一个靠谱的误差状态框架的人。下面我会把 15 维状态的排布、连续时间误差动力学、F 和 Q 的构造、注入与协方差重置这几块讲透然后把我踩过的坑一个个摊开。1.1 直接估计四元数的 EKF 会出什么毛病先说清楚问题的根源。姿态的旋转矩阵只有 3 个自由度但你要用 4 个数的四元数或者 9 个数的旋转矩阵来表示它就必然带一个约束四元数模长必须恒为 1旋转矩阵必须正交。约束意味着参数之间存在冗余也就意味着参数之间是相关的、不是独立的。普通 EKF 的做法是把四元数的四个分量直接当作状态量的四个维度然后给这四维配一个 4x4 的协方差块。问题立刻出现协方差矩阵描述的是变量之间的相关性可这个 4x4 块里有一个方向沿四元数本身的方向在物理上根本不存在自由度滤波器却给它分配了不确定度。迭代几次之后你会在协方差矩阵里看到模长方向上的方差在积累投影到姿态上就表现为姿态角在数值上不断爬升。更麻烦的是每一次状态更新之后如果你不强行归一化四元数模长就会慢慢偏离 1如果你强行归一化那归一化这个操作本身引入了非线性普通 EKF 的线性化根本没法正确描述这一步对协方差的影响。协方差和状态就开始互相不一致正定性也会在几十上百次迭代后被数值误差磨掉Cholesky 分解直接报错。我当时的做法是很粗暴地每步归一化然后也不管协方差结果就是静态看着还行动态一跑就发散。这不是代码写错了是建模方式从根上就不对。1.2 误差状态的定义真值等于标称复合误差ES-EKF 的破局点在于换一种参数化。它把真实状态拆成两部分$$x_t x \oplus \delta x$$其中 $x$ 是标称状态nominal state由 IMU 积分等大信号驱动是一个普通的、没有约束的大量$\delta x$ 是误差状态error state是一个始终在零点附近徘徊的小量。符号 $\oplus$ 表示复合对于姿态这类带约束的量它是一个乘性操作对于位置、速度、零偏这些普通向量它就是加法。姿态部分具体写成$$R_t R \cdot \text{Exp}(\delta\theta)$$这里 $R$ 是标称旋转矩阵$\delta\theta$ 是一个 3 维的小旋转向量$\text{Exp}(\cdot)$ 是罗德里格斯公式把旋转向量映射成旋转矩阵。注意 $\delta\theta$ 只有 3 维而姿态的真实自由度恰好也是 3 维——冗余被消掉了。这就是 ES-EKF 状态里姿态只占 3 维而不是 4 维的原因。滤波器全程只对 $\delta x$ 做最优估计$\delta x$ 的均值一直维持在零附近协方差就是它的真实不确定度。姿态的大变化由标称状态 $R$ 负责承载它靠 IMU 积分推进不参与最小二乘意义上的修正只在误差注入时被微调一下。这个分工让姿态的约束不再是状态量内部的事而是被挪到了标称和误差之间的复合关系里。1.3 一个能帮你记住这套结构的类比如果上面这段还有点抽象我一般用这个类比给自己解释把标称状态想象成一辆车的绝对位置把误差状态想象成里程计的相对增量。卫星定位给的是绝对坐标但它慢、有跳变里程计给的是相对位移它快、平滑但会累积漂移。你不会把两者当成同一个东西硬塞进一个模型里而是让里程计负责走卫星负责纠正走偏了多少。ES-EKF 里IMU 积分的标称状态就是那辆一直往前开的车误差状态就是我到底偏了多少这个量观测更新就是卫星定位来告诉你偏了多少。这个类比还能解释一件事为什么误差状态可以放心地做线性化。因为它一直在零点附近$\delta\theta$ 是小角$\sin\delta\theta \approx \delta\theta$所有非线性的高阶项都可以丢掉。普通 EKF 里姿态角在 ±180 度范围内到处跑你在哪儿线性化都有误差ES-EKF 里你永远在零点线性化误差项的大小被状态本身的定义压住了。2. 15 维误差状态向量怎么排每个块为什么是这个维数搞懂原理之后落地第一步就是把状态向量排出来。行业里最主流的配置是 15 维误差状态加了重力或者尺度之后可能到 16、18 维。我下面以 15 维、不含重力误差的版本为主讲因为它是大多数组合导航和 VIO 项目的起点。2.1 状态排列与坐标系约定15 维按顺序分成五块每块 3 维序号状态块符号物理含义误差定义1姿态误差$\delta\theta$机体系相对导航系的姿态偏差乘性局部扰动2速度误差$\delta v$导航系下的速度偏差加法3位置误差$\delta p$导航系下的位置偏差加法4陀螺零偏误差$\delta b_g$陀螺仪的常值零偏偏差加法5加计零偏误差$\delta b_a$加速度计的常值零偏偏差加法顺序很重要因为它直接决定了 F 矩阵、H 矩阵里每个块落在哪几行哪几列。我自己踩过的一个低级坑是推导的时候按姿态-速度-位置-陀螺-加计排写代码的时候顺手按姿态-位置-速度排结果 H 矩阵索引错了位置滤波器把位置观测量更新到了速度上表现是位置不变、速度狂跳。这种错误不会有任何报错只能靠对着残差曲线一点点看。坐标系约定同样要先钉死导航系我用的是东北天ENU机体系是前右下。重力向量在 ENU 下是 $[0, 0, -g]^T$$g$ 取值 9.81 还是本地重力取决于你对高度精度的要求一般组合导航取 9.80665 就够。这些约定必须在写第一行代码前定下来中途改会牵连到所有矩阵。2.2 姿态误差用局部扰动还是全局扰动姿态误差有局部local和全局global两种定义这是最容易选错的地方。局部扰动是误差作用在机体系右侧$$R_t R \cdot \text{Exp}(\delta\theta_{local})$$全局扰动是误差作用在左侧$$R_t \text{Exp}(\delta\theta_{global}) \cdot R$$两者都能用但在 ES-EKF 的常用实现里局部扰动是绝对主流。原因有两个一是局部扰动下误差运动学的推导结果里陀螺零偏误差 $\delta b_g$ 的系数是单位阵形式最干净二是局部扰动下误差注入和协方差重置这一步是平凡的不需要额外乘一个重置矩阵代码少一个出错点。如果用全局扰动注入和协方差重置就需要一个非平凡的 G 矩阵参与一旦忘了乘滤波器会慢慢失去一致性。所以除非你有特别理由我建议直接用局部扰动把复杂度降下来。这里还有个细节值得一提局部扰动下陀螺零偏误差和姿态误差在运动学里是直接耦合的$\dot{\delta\theta}$ 里含 $-\delta b_g$ 项。这意味着陀螺零偏一有偏差姿态就会持续积分漂移滤波器无法把它们完全分开只能靠观测约束一起估计。这也是为什么零偏标定做得越准后面的姿态质量越好。2.3 速度、位置和零偏误差为什么用简单加法速度、位置、零偏这三块都没有约束误差直接定义为标称值加误差$$v_t v \delta v, \quad p_t p \delta p, \quad b_t b \delta b$$这种加性定义的好处是线性和可逆注入的时候直接加重置协方差的时候就是恒等操作没有任何额外处理。它们和姿态误差的唯一区别就是姿态用的是乘性复合。零偏这块要补充一句这里假设的是常值零偏模型也就是零偏在传播过程中保持不变只受随机游走噪声驱动。真实 IMU 的零偏还包含温漂、非正交、尺度因子误差等如果你的应用对精度要求高可以在状态里再塞进去尺度因子和安装误差但那会把状态维数推上去可观测性也会变差。我的建议是分两步走先把常值零偏做扎实确认滤波器行为正常再考虑扩展。3. 传播步骤从连续误差动力学到 F 矩阵和 Q 矩阵传播这一步决定了滤波器的骨架。标称状态靠 IMU 积分推进误差状态靠 F 和 Q 推进。这里我把连续时间的误差微分方程逐项拆开说明每一项是从哪里来的然后再讲离散化和噪声矩阵。3.1 连续时间误差微分方程逐项拆解先列出标称状态的连续传播方程也就是 IMU 的机械编排$$\dot p v, \quad \dot v R(a_m - b_a) g, \quad \dot R R[\omega_m - b_g]_\times$$其中 $[\cdot]_\times$ 是反对称矩阵算子$a_m$ 和 $\omega_m$ 是加计和陀螺的原始测量$g$ 是重力向量。把真值代入、做一阶扰动展开得到误差状态的连续微分方程局部扰动版本$$\dot{\delta p} \delta v$$$$\dot{\delta v} -R[a_m - b_a]_\times \delta\theta - R,\delta b_a - R,n_a$$$$\dot{\delta\theta} -[\omega_m - b_g]_\times \delta\theta - \delta b_g - n_g$$$$\dot{\delta b_g} n_{bg}, \quad \dot{\delta b_a} n_{ba}$$我不打算把展开过程全写出来但有三项值得点透。第一项$\dot{\delta v}$ 里的 $-R[a_m - b_a]_\times \delta\theta$。这一项说明姿态误差会通过当前的比力耦合到速度误差上。物理直觉是姿态算错了重力就不可能被正确扣除残余的加速度就变成了速度漂移。所以姿态误差和速度误差是强耦合的这也解释了为什么姿态没估计好位置几乎不可能准。第二项$\dot{\delta\theta}$ 里的 $-[\omega_m - b_g]_\times \delta\theta$。这是姿态误差自身的旋转效应误差向量本身也是在旋转的坐标系里表达的所以它会随体系一起转。很多简化实现会把这一个二阶小量直接丢掉短时间没问题但长时间或高动态下会带来可感知的偏差。第三项零偏误差的导数是白噪声。这来自常值零偏假设零偏本身不变误差只由随机游走驱动。这一项的物理含义是我不知道零偏到底是多少所以它的不确定度只会慢慢变大。3.2 离散化一阶近似还是矩阵指数把上面的方程组写成矩阵形式 $\dot{\delta x} F,\delta x G,n$按照 $\delta\theta, \delta v, \delta p, \delta b_g, \delta b_a$ 的顺序F 矩阵是$$ F \begin{bmatrix} -[\omega]\times 0 0 -I 0 \ -R[a]\times 0 0 0 -R \ 0 I 0 0 0 \ 0 0 0 0 0 \ 0 0 0 0 0 \end{bmatrix} $$其中 $\omega \omega_m - b_g$$a a_m - b_a$零块都是 3x3。G 矩阵把噪声输入映射进来$n [n_g, n_a, n_{bg}, n_{ba}]^T$$$ G \begin{bmatrix} -I 0 0 0 \ 0 -R 0 0 \ 0 0 0 0 \ 0 0 I 0 \ 0 0 0 I \end{bmatrix} $$离散化有两种常见做法。最简单的一阶近似是 $F_d I F\Delta t$$\Delta t$ 是 IMU 采样间隔。它的优点是便宜缺点是 $\Delta t$ 大或者角速度高的时候误差明显。另一种是矩阵指数 $F_d \text{Exp}(F\Delta t)$精度高但需要 15x15 的矩阵指数运算代价大。我的实测结论是IMU 采样率在 100Hz 以上、角速度不超过 300 度每秒的情况下一阶近似完全够用如果采样率低于 50Hz或者你做的是高机动平台建议用 Padé 近似的矩阵指数或者至少用二阶展开 $F_d I F\Delta t \frac{1}{2}(F\Delta t)^2$。我踩过一次坑在 20Hz 的低成本 IMU 上用一阶近似单次旋转 90 度以上时协方差明显偏小滤波器变得过度自信。3.3 Q 矩阵的量纲对齐这里最容易出错传播部分真正让人翻车的地方是 Q 矩阵。离散噪声矩阵一般写成$$Q_d \approx G,Q_c,G^T\Delta t$$$Q_c$ 是连续时间噪声密度对角阵包含陀螺噪声密度、加计噪声密度、陀螺随机游走、加计随机游走四个部分。$G$ 映射之后$Q_d$ 的每个块才是真正叠加到协方差上的量。这里的关键是量纲。噪声密度是有单位的单位搞错的表现是滤波器参数怎么调都不对。举个最常见的错误把 IMU 数据手册上的角度随机游走直接当成陀螺噪声密度填进去。这两个东西差一个采样率的开方$$\sigma_{density} \frac{\sigma_{ARW}}{\sqrt{\Delta t}}$$如果你的数据手册给的是度每根号小时deg/√hr还要先换算成弧度制再换算到每根号秒。我见过太多人直接把 0.01 deg/√s 当噪声密度结果 Q 大了几百倍滤波器完全学不动所有观测量都被当成噪声姿态一动不动。下面这张表是我自己整理的量纲对照可以直接抄参数常见单位落到 $Q_c$ 前要做的换算说明陀螺噪声密度deg/√s → rad/√s乘 $\pi/180$白噪声项加计噪声密度mg/√Hz → m/s²/√Hz乘 $10^{-3}\times g$白噪声项陀螺随机游走deg/√hr → rad/√s乘 $\pi/180$ 再除 60零偏漂移项加计随机游走m/s³/√Hz直接使用零偏漂移项提示调 Q 的时候不要一次动四个参数先固定姿态相关的两个量把位置和速度观测的残差曲线调平再去动零偏的两个量。四个一起调你永远不知道是哪一项在起作用。4. 更新与注入重置这一行写错滤波器就废了传播只让不确定度长大更新才是把观测信息吃进来的地方。ES-EKF 的更新分成三个动作算增益和误差修正量、把修正量注入标称状态、重置协方差。第三个动作是最容易被省略的也是最容易出隐蔽 bug 的。4.1 GNSS 位置观测的线性化与 H 矩阵以卫星定位的位置观测为例。观测量是导航系下的三维位置观测方程就是$$z h(x_t) v_{gnss} p_t v_{gnss} p \delta p v_{gnss}$$对误差状态线性化得到的 H 矩阵只需要在位置那一块放单位阵$$H \begin{bmatrix} 0_{3\times3} 0_{3\times3} I_{3\times3} 0_{3\times3} 0_{3\times3} \end{bmatrix}$$如果我同时用速度观测就是在速度那一块也放单位阵。注意 H 是作用在误差状态上的不是作用在标称状态上的所以观测残差要写成 $z - p$用标称位置不是真值位置。这个细节很多人第一次写会写错用了真值当然不报错但滤波器的行为就不对了。残差还要做一点处理如果观测是经纬高先转成局部 ENU 坐标再做差如果是 ECEF也统一到一个坐标系里。混用坐标系是另一个高频 bug表现是滤波器收敛后残差有一个固定的偏置。4.2 卡尔曼增益与 Joseph 形式协方差更新标准更新公式$$S H P H^T R_{meas}, \quad K P H^T S^{-1}$$$$\delta\hat{x} K(z - h(x)), \quad P \leftarrow (I - KH)P(I - KH)^T K R_{meas} K^T$$最后那个协方差更新我用的是 Joseph 形式而不是简化的 $(I-KH)P$。原因是简化形式在数值上不保正定长时间跑下来会出现负特征值。Joseph 形式虽然多一次矩阵乘但它对任意 K 都保正定代价完全值得。如果你追求更稳可以在更新完之后再做一次强制对称化 $P \leftarrow (P P^T)/2$这一招几乎零成本能救回不少因为浮点舍入导致的轻微不对称。$S$ 的求逆是每次更新的性能瓶颈$S$ 只有 3x3 或 6x6直接求逆没问题。但要注意如果 $S$ 条件数很大说明观测和预测高度相关容易出问题这时候可以考虑用 UD 分解或者平方根滤波那是另一个话题了。4.3 注入和协方差重置那几行务必对着公式抄更新算出 $\delta\hat{x}$ 之后要把它注入标称状态。按局部扰动版本$$p \leftarrow p \delta\hat{p}, \quad v \leftarrow v \delta\hat{v}$$$$R \leftarrow R \cdot \text{Exp}(\delta\hat{\theta}), \quad b_g \leftarrow b_g \delta\hat{b}_g, \quad b_a \leftarrow b_a \delta\hat{b}_a$$然后是协方差重置。局部扰动下姿态的注入是 $R \leftarrow R,\text{Exp}(\delta\hat\theta)$新的误差满足 $R_t R^ \text{Exp}(\delta\theta^)$代入展开可以证明一阶近似下 $\delta\theta^ \delta\theta - \delta\hat\theta$也就是纯减法。所以重置矩阵在一阶意义下就是单位阵绝大多数实现直接写 $P \leftarrow P$ 或者干脆不写这一步。但我建议你还是保留一个显式的重置步骤哪怕它当前是恒等操作。因为在两种情况下它不是恒等的一是你用了全局扰动二是你要在重置时对姿态块做一点数值上的削峰。把它写成一个独立函数将来切换扰动约定的时候不用满世界找代码。这里有个我踩过的坑值得说注入之后标称状态变了但 $\delta\hat{x}$ 必须立刻清零否则下一轮传播会从非零的误差均值开始滤波器会持续叠加修正量位置和速度开始振荡。我在调试初期就犯过这个错误表现是滤波器刚开始收敛得很快然后突然开始慢慢振荡残差呈周期性的正弦形状。5. 工程落地初始化、协方差配置与数值保护理论推导正确不代表代码能跑。滤波器上线之后一半的问题出在初始化和协方差配置另一半出在数值稳定性上。5.1 初始协方差到底给多大初始协方差 $P_0$ 是你对初始状态有多不确定的直接表达。给太小滤波器过度自信观测拉不动它给太大滤波器对观测量 malnutrition 似的抓取前期抖得厉害。我的经验值组合导航ENU静止对齐状态块初始标准差说明姿态3~5 度静止对齐后可以给更小1~2 度速度0.5~1 m/s静止时可以给 0.1位置5~10 m取决于首次定位误差陀螺零偏0.5~1 deg/s按 IMU 数据手册上限给加计零偏0.05~0.2 m/s²按数据手册上限给方差就是这些标准差的平方不是直接填标准差。这个低级错误我见过不止一个人犯。另外 $P_0$ 必须对称正定可以构造完之后做一次对称化再丢给 Cholesky。5.2 静止对齐、重力向量与零偏初始化初始化阶段如果设备是静止的可以做一次静止对齐用加计的重力方向算出初始横滚和俯仰用磁力计或者已知航向给出初始偏航。这一步的精度直接决定了后续姿态的绝对基准。重力向量这块有个特别容易忽略的细节重力到底取多大。如果你的应用涉及长距离导航用局部重力模型比用常数 9.80665 更准如果只是短时间、小范围常数就够。但陀螺零偏的初始估计很关键因为它会直接积分到姿态上。如果条件允许静止时先算一段陀螺输出的均值作为零偏初值可以显著改善起步阶段的姿态精度。加计零偏的初始化比较麻烦因为静止时加计无法区分重力和零偏。常见的做法是先假设加计零偏为零让它靠后续观测慢慢估计或者用多位置标定单独做。我一般会把它放在状态里给一个较大的初始协方差让滤波器自己学。5.3 数值保护对称化、Cholesky 失败和失效检测跑到一定规模一定会遇到协方差矩阵出问题。我的清单是这样的每次传播和更新后强制对称化 $P \leftarrow (PP^T)/2$成本极低收益明显。用 Joseph 形式更新不用简化形式。计算 $S$ 之前检查条件数如果超过阈值我一般用 1e8先对 $S$ 做一点对角加载再求逆。如果 Cholesky 分解失败退回到特征值分解把负特征值截到一个小正数再重构。记录每一轮的残差和增益如果增益突然暴涨说明观测和预测严重不一致可能是观测跳变此时可以对观测做一次卡方检验超限就直接丢弃。注意卡方检验的门限不要设太严我一般用 95% 分位数能滤掉大部分跳变观测又不会把正常的观测也一起拒掉。门限太严会导致滤波器看不见真实观测逐渐漂移。失效检测同样重要。我会监控几个量连续多少次没有有效观测、姿态协方差的迹是否超过阈值、残差是否持续偏大。任何一个触发就切到单纯的 IMU 递推模式同时给用户一个明确的退化标志。宁可告诉用户我在退化运行也不要让滤波器悄悄算出错误的姿态。6. 上线之后怎么判断滤波器还活着一致性与可观测性检验代码跑通了不等于滤波器可信。我习惯在上线前做两类检验一致性检验和可观测性分析。6.1 NEES 和 NIS 检验怎么用NEESNormalized Estimation Error Squared衡量的是估计误差和协方差报告的不确定度是否匹配$$\text{NEES} \delta x^T P^{-1} \delta x$$标准的 NEES 应该落在卡方分布的置信区间内。实际做的时候你拿不到真值误差只能用一个更高精度的参考系统比如后处理解算的轨迹作为真值。如果 NEES 持续偏大说明滤波器低估了自己的不确定度也就是协方差偏小、过度自信如果持续偏小说明协方差偏大、参数太保守。NISNormalized Innovation Squared在更新时就能算不需要真值$$\text{NIS} r^T S^{-1} r, \quad r z - h(x)$$它衡量的是观测残差和理论残差协方差是否匹配。NIS 长期偏大通常意味着 Q 或 R 配置有问题或者存在未建模的误差源。我自己的做法是把 NEES 和 NIS 都打到日志里跑完一段数据之后画时间曲线看它们是否落在理论区间内。这项工作花不了多少时间但能提前发现大部分调参问题。6.2 哪些状态本质上不可观测可观测性这件事用一句话概括只有被观测方程直接或间接看到的状态才能被估计出来其余的状态只能靠先验维持会慢慢发散。在只有卫星定位位置观测的组合导航里有几个方向是典型不可观测或者弱可观测的偏航角在没有航向观测时基本不可观测滤波器只能靠陀螺积分维持长时间会漂加计零偏和姿态在静止时互相耦合很难单独分辨陀螺零偏和姿态误差在短时间窗口内高度相关需要足够长的观测时间长度才能分开。理解这一点之后你就不会再奇怪为什么我明明有位置观测姿态还是会漂。位置观测约束的是位置和速度对姿态的约束只通过比力耦合间接传递强度有限。要真正约束姿态你得有航向观测、或者做零速修正、或者用视觉观测。一个实用的做法是在可观测性弱的阶段主动把对应的过程噪声调小让滤波器更依赖先验而不是噪声观测在观测充足的阶段把过程噪声调大让滤波器更多地吸收观测。这种分段调参的做法在工程上很常见比一套参数从头用到尾效果好得多。6.3 从单传感器到多传感器ES-EKF 的扩展姿势最后说一下扩展。ES-EKF 的结构非常适合加传感器因为误差状态的线性化点固定在零点新加一个观测只需要写一个新的 H 矩阵和观测噪声不用动传播和注入的逻辑。我按顺序加传感器的经验是先做 IMU 加卫星定位确认基本框架稳定再加里程计或者轮速约束水平速度再加磁力计或者航向约束偏航最后加视觉或者激光做局部的绝对约束。每加一个都要重新做一遍 NIS 检验确认新增的观测真的在贡献信息而不是在互相打架。值得一提的是多个传感器同时更新的时候如果观测之间相关是需要小心处理的不能简单串行地做两遍标准更新。如果两路观测来自完全独立的源串行更新是可以的但如果它们共享了某种相关性比如同一时刻的卫星定位位置和速度最好合并成一个联合观测一次性更新。我个人在实际使用中最深的一个体会是ES-EKF 的代码量不比普通 EKF 大多少多出来的那部分主要是误差注入和协方差重置那几十行但就是这几十行把姿态稳定性从能跑提升到了能交付。我现在的习惯是只要状态里出现旋转量一律用误差状态框架不再纠结值不值——那是用两周调漂移换来的教训实在不想再交第二次学费。