误差状态卡尔曼滤波（Error-State Kalman Filter，ESKF）做惯性导航，这两年在组合导航、视觉惯性里程计、自动驾驶多传感器融合里几乎成了默认方案。我刚开始啃这套理论的时候，最大的障碍不是公式本身，而是资料普遍两种走极端：要么纯学术、满屏流形和李代数，要么直接丢给你一坨代码，完全不给“误差状态到底处理了什么本质问题”的解释。这篇先把ESKF的理论骨架按我自己的理解完整拆一遍，属于整个学习笔记系列的第一篇，后续会结合具体实现和实测数据持续更新。文章适合刚接触组合导航、读过一点卡尔曼滤波，但被四元数、姿态误差、协方差传播这些概念绕晕的读者。
1. 为什么非得用误差状态：全状态EKF的三个坑
1.1 四元数均值与单位约束的尴尬
如果你直接把常规EKF套到惯性导航上，状态里会有位置、速度、姿态四元数、陀螺零偏、加速度计零偏，姿态用四元数表示。四元数是个单位约束的四维向量，真实姿态是在四维空间的一个球面上，叫SO(3)流形。问题就出在这：EKF的核心假设是状态服从高斯分布，均值加协方差就能完整描述不确定性，但这个假设在四元数上天然不成立。
你可以做个试验，取两个姿态四元数做算术平均，结果大概率不是单位四元数。就算你强行归一化，那是在“球外”做的投影，不是统计意义上的最优均值。更麻烦的是，四元数本身的维度是4，但有效自由度只有3，对应的协方差矩阵是4×4但本质上是奇异的，约束关系让协方差变成病态矩阵。为了维持单位约束，不少人会在每步更新后手动归一化，这属于打补丁，不是从根本上解决问题，而且归一化会悄悄改变概率分布的形状。误差状态方案就绕开了这个问题：姿态误差用三维旋转向量 (\delta\theta) 表示，直接定义在SO(3)的切空间上，协方差天然是3×3的，不涉及多余维度，也没有约束要维护。
1.2 线性化误差的控制
EKF对非线性系统做一阶泰勒展开，线性化在“当前估计处”展开，展开点离真实状态越远，近似误差越大。全状态EKF里，如果某个状态估计偏了，比如姿态误差达到10度，状态转移和观测模型的一阶近似就会引入明显偏差，滤波器容易越跑越偏。
误差状态的思路完全不同：把状态拆成“标称状态”和“误差状态”，标称状态用精确的非线性运动学传播，误差状态被定义为标称状态与真实状态之间的小偏差。只要滤波器不发散，误差状态始终维持在零附近，哪怕姿态有30度的初始偏差，经过几次观测修正后，误差也会被压到很小。在一个小误差附近做线性化，一阶近似的精度就高得多，这是ESKF在工程上能稳定工作的核心原因。简单说：全状态EKF是在大范围区域内强行用直线近似曲线，ESKF是确保你始终在小邻域内用直线近似，后者的近似代价低得多。
1.3 双时间尺度带来的工程红利
IMU的频率通常是100Hz到1000Hz，而GPS、视觉里程计这类外部观测往往只有10Hz到50Hz。ESKF天然匹配这种频率差：高频的标称状态传播只做运动学积分，不涉及协方差更新，计算量很小；低频的误差状态更新才做矩阵运算。等于把“贵”的操作从高频路径挪到了低频路径，实时性压力一下就小了。
更重要的是，误差状态更新完会立刻“归零重置”，误差状态量级一直很小，意味着卡尔曼增益和协方差的数值都保持在舒适区，不容易出现数值不稳定。后面我会详细展开这个“重置”操作，它看着简单，实际是整个ESKF里最容易出暗病的环节。
2. ESKF的状态定义与IMU运动学：先把符号约定讲清楚
2.1 真实状态、标称状态与误差状态的嵌套关系
ESKF里同时维护三个层次的状态概念。真实状态 (x_t) 代表物理世界的真值，永远得不到；标称状态 (\hat{x}) 是滤波器实时维护的主体，用来做运动学积分和输出导航结果；误差状态 (\delta x) 是滤波器的“暗账”，记录标称状态与真实状态之间的偏差，卡尔曼滤波更新的是它。
具体到惯性导航，状态向量包含位置 (p)、速度 (v)、姿态四元数 (q)、陀螺仪零偏 (b_g)、加速度计零偏 (b_a)。三者的关系定义为：
[ p_t = \hat{p} + \delta p,\qquad v_t = \hat{v} + \delta v,\qquad q_t = \hat{q} \otimes \delta q,\qquad b_{g,t} = \hat{b}g + \delta b_g,\qquad b{a,t} = \hat{b}_a + \delta b_a ]
位置和速度、零偏都是加性关系，只有姿态特殊，用的是四元数乘法。这里有个必须事先统一的关键约定：姿态误差到底在左侧乘还是右侧乘。本文采用右扰动约定，即 (\delta q \approx [1, \frac{1}{2}\delta\theta]^T)，(\delta\theta) 是三维旋转向量。不同文献可能用左扰动，公式里会差一个正负号或者转置，对照代码时务必先确认这一点，否则移植公式过去全是坑。
2.2 IMU测量模型与连续运动学方程
先写IMU测量模型。陀螺仪测的是载体角速度加偏置再加噪声：
[ \omega_m = \omega + b_g + n_g ]
加速度计测的是比力，注意它测不到重力，反而把重力等效为反向加速度。测量方程写作：
[ a_m = R^T(a - g) + b_a + n_a ]
或者反过来写成真实加速度的表达式：(a = R(a_m - b_a - n_a) + g)。这里的 (R) 是载体系到世界系的旋转矩阵，(g) 是世界系下的重力向量，(n_g)、(n_a) 是陀螺和加速度计的白噪声。模型里默认零偏都是缓变随机游走，这个后面会体现在误差状态的噪声项里。
标称状态的连续时间运动学写成：
[ \dot{\hat{p}} = \hat{v} ]
[ \dot{\hat{v}} = \hat{R}(a_m - \hat{b}_a) + g ]
[ \dot{\hat{q}} = \frac{1}{2} \hat{q} \otimes (\omega_m - \hat{b}_g) ]
为什么直接用测量值减去当前零偏估计作为输入？因为我们没有真值信号，只能把测量当作“带噪声的系统输入”。IMU噪声和零偏估计误差不在这里处理，而是全部放进误差状态的动态方程里。这种“明账”和“暗账”分离的思路贯穿整个ESKF。
2.3 误差状态连续方程是怎么推出来的
把真实状态关系代入真实运动学方程，减去标称运动学方程，忽略二阶以上小量，就得到误差状态微分方程。这个过程我自己手推过一遍，非常值得做，能帮你彻底摆脱“背公式”的状态。关键结果是：
[ \delta\dot{p} = \delta v ]
[ \delta\dot{v} = -\hat{R}[a_m - \hat{b}a]{\times}\delta\theta
- \hat{R}\delta b_a - \hat{R}n_a ]
[ \delta\dot{\theta} = -[\omega_m - \hat{b}g]{\times}\delta\theta
- \delta b_g - n_g ]
[ \delta\dot{b}g = w{bg},\qquad \delta\dot{b}a = w{ba} ]
速度误差项里的 (-\hat{R}[a_m - \hat{b}a]{\times}\delta\theta) 是姿态误差引起的比力方向偏差，(-\hat{R}\delta b_a) 是加速度计零偏误差；姿态误差项里的 (-[\omega_m - \hat{b}g]{\times}\delta\theta) 是角速度测量误差引起的姿态变化率偏差，(-\delta b_g) 是陀螺零偏误差。([]_\times) 表示三维向量的反对称矩阵，它把叉积变成了矩阵乘法。
这里要特别强调：(\delta v) 里那个负号很容易在推导时搞错，最好自己从 (q_t = \hat{q}\otimes\delta q) 出发推一遍，搞清楚旋转顺序。我当时对着讲义看三遍都没发现问题，手推一遍才发现之前符号抄反了。
3. 离散化与协方差传播：从连续方程到可执行代码
3.1 标称状态的离散更新
连续方程要落到代码里，第一步是离散化。标称状态的更新频率跟着IMU走，通常用欧拉积分或者中点法。最朴素的欧拉积分写出来是：
[ \hat{p}_{k+1} = \hat{p}_k + \hat{v}_k \Delta t
- \frac{1}{2}\left(\hat{R}k(a{m,k} - \hat{b}_a) + g\right)\Delta t^2 ]
[ \hat{v}_{k+1} = \hat{v}_k + \left(\hat{R}k(a{m,k} - \hat{b}_a) + g\right)\Delta t ]
[ \hat{q}_{k+1} = \hat{q}k \otimes \exp\left((\omega{m,k} - \hat{b}_g)\Delta t\right) ]
(\exp(\cdot)) 在这里是旋转向量到四元数的指数映射，小角度时可近似成 ([\omega\Delta t]) 对应的四元数。工程上更推荐用中点法甚至四阶龙格库塔处理姿态和速度积分，尤其在车辆高动态场景下，欧拉积分在高频响应时误差累积会比较明显。但不管是哪种积分，标称状态的计算路径都是确定的、可复现的，这也方便你先单独跑通这一层再上完整滤波器。
3.2 误差状态转移矩阵的组装
误差状态是线性时变系统，可以写成 (\delta\dot{x} = F\delta x + Gn)。把上面的误差方程整理成矩阵形式，连续时间的 (F) 矩阵是15×15的块矩阵：
[ F = \begin{bmatrix} 0 & I & 0 & 0 & 0\ 0 & 0 & -\hat{R}[a_m - \hat{b}a]{\times} & -\hat{R} & 0\ 0 & 0 & -[\omega_m - \hat{b}g]{\times} & 0 & -I\ 0 & 0 & 0 & 0 & 0\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} ]
噪声输入矩阵 (G) 对应的是速度项里的 (-\hat{R}n_a)、姿态项里的 (-n_g)、零偏项里的 (w_{bg}, w_{ba})：
[ G = \begin{bmatrix} 0 & 0 & 0 & 0\ -\hat{R} & 0 & 0 & 0\ 0 & -I & 0 & 0\ 0 & 0 & I & 0\ 0 & 0 & 0 & I \end{bmatrix} ]
离散化的误差状态转移矩阵写成：
[ \Phi_k = \exp(F\Delta t) \approx I + F\Delta t ]
一阶近似在工程上通常够用，因为误差状态本身很小，加上姿态误差的交叉项影响有限。如果你比较谨慎，可以取二阶展开，代价是代码量和计算量略增。我一开始用的一阶近似，实测下来没遇到因为这一步导致的精度问题。
3.3 噪声协方差的离散化
连续时间噪声的协方差矩阵定义为：
[ Q_c = \mathrm{diag}\left(\sigma_a^2 I_3,\ \sigma_g^2 I_3,\ \sigma_{bg}^2 I_3,\ \sigma_{ba}^2 I_3\right) ]
离散化后的噪声协方差用一个近似公式：
[ Q_d \approx \Phi_k G Q_c G^T \Phi_k^T \Delta t ]
这个公式把连续白噪声在积分步长内近似为离散等效噪声。如果追求更高精度，可以用积分形式 (Q_d = \int_0^{\Delta t}\Phi(\tau)GQ_cG^T\Phi^T(\tau)d\tau) 数值求解，理论上更合理，但实际中一阶近似已经能满足多数组合导航需求。还有一个实用小点：协方差矩阵在每步传播后理论上是对称的，但浮点运算可能破坏对称性，做一些对称化处理 (P = (P + P^T)/2) 能避免后续卡尔曼增益计算出现病态。
4. 观测更新与误差注入：修正不是直接加到状态上
4.1 观测模型、残差与卡尔曼增益
外部观测的通用形式是 (z = h(x_t) + v)，(v) 是观测噪声。把 (x_t = \hat{x} \oplus \delta x) 代入，在标称状态附近线性化，残差就变成关于误差状态的线性函数：
[ y = z - h(\hat{x}) = H\delta x + v ]
这里的 (H) 是观测对误差状态的雅可比矩阵。举几个常见例子：GPS位置观测对应 (H = [I_3, 0, 0, 0, 0])；轮速或视觉测速对应的速度观测是 (H = [0, I_3, 0, 0, 0])；磁力计或航向观测则对应姿态块上的特定组合。
卡尔曼增益照公式算：
[ K = P H^T \left(H P H^T + R\right)^{-1} ]
更新后的误差状态均值是：
[ \delta x = K y ]
注意这里没有减去预测的误差状态，因为预测阶段误差状态的均值始终是零。协方差更新用标准式：
[ P = (I - KH)P ]
4.2 状态注入：四元数用乘法，其余用加法
拿到 (\delta x = [\delta p, \delta v, \delta\theta, \delta b_g, \delta b_a]) 之后，把它修正回标称状态。位置、速度、零偏直接加：
[ \hat{p} \mathrel{+}= \delta p,\quad \hat{v} \mathrel{+}= \delta v,\quad \hat{b}_g \mathrel{+}= \delta b_g,\quad \hat{b}_a \mathrel{+}= \delta b_a ]
姿态必须用四元数乘法：
[ \hat{q} = \hat{q} \otimes \exp(\delta\theta) ]
为什么要绕这一下？因为如果直接把 (\delta\theta) 加到四元数上，四元数马上失去单位约束，状态就“掉出流形”了。乘法形式保证 (\hat{q}) 更新后仍是合法四元数。这也是ESKF的核心优雅之处：标称状态的姿态始终在流形上走，只有误差状态在切空间里做线性操作。
4.3 必须做的误差重置与协方差G变换
误差状态更新完，下一步是把 (\delta x) 清零，为下一轮做准备。但这里有个非常隐蔽的坑：清零之前，当前协方差 (P) 描述的是“旧误差状态”的不确定性，清零之后误差状态的定义基准变了，协方差必须同步变换到“新误差状态”的坐标系下。这个变换矩阵 (G) 定义为：
[ G = \begin{bmatrix} I_3 & 0 & 0 & 0 & 0\ 0 & I_3 & 0 & 0 & 0\ 0 & 0 & I_3 - [\frac{1}{2}\delta\theta]_{\times} & 0 & 0\ 0 & 0 & 0 & I_3 & 0\ 0 & 0 & 0 & 0 & I_3 \end{bmatrix} ]
然后做：
[ P \leftarrow G P G^T ]
位置、速度、零偏块的雅可比是单位阵，只有姿态块因为旋转误差与真实误差之间的雅可比不为单位阵，才需要这个修正。当 (\delta\theta) 很小时 (G) 非常接近单位阵，所以很多简化实现直接忽略这一步也能跑，但长期看协方差会对不上真实误差，可能出现“滤波器很自信但实际误差很大”的假收敛状态。我在某个项目里就遇到过这种问题，现象是定位结果看起来平滑，一对比真实轨迹才发现误差已经悄悄涨到十几米，排查下来就是重置G矩阵丢失造成的。
5. 工程实现要点与参数调优
5.1 初始化流程
ESKF对初始化比全状态EKF更敏感，因为误差状态方程依赖“误差很小”这个前提。如果初始姿态都差着30度，线性化近似会非常吃力。常规做法是先在静止状态下采集几秒IMU数据：
- 陀螺仪输出均值作为陀螺零偏初值 (\hat{b}_g)；
- 加速度计输出均值除以重力模长得到重力方向，算出横滚角和俯仰角初值；航向角在没有磁力计或外部航向时可先置0；
- 加速度计零偏初值如果无法精确标定，可以先设0，但要把初值协方差设得比较大，让滤波器自己去估计；
- 陀螺零偏的初始协方差根据传感器指标设，一般设到对应角速率误差的平方级别。
我在实际项目里的做法还要加一步“静止检测”：用滑动窗口计算IMU输出的方差，方差低于阈值才允许进入初始化。初始化期间必须保证设备完全静止，否则重力对齐和零偏估计都会被污染，后面ESKF就算能收敛，也要花很长一段路才能把错误掰回来。
5.2 过程噪声怎么定：别拍脑袋
ESKF调试中最影响手感的就是四个噪声参数：加速度计白噪声 (\sigma_a)、陀螺白噪声 (\sigma_g)、陀螺零偏随机游走 (\sigma_{bg})、加速度计零偏随机游走 (\sigma_{ba})。这几个参数如果不匹配，会导致滤波器要么过度信任IMU、输出漂得离谱，要么过度信任外部观测、输出跟着观测噪声乱抖。
| 参数 | 物理含义 | 典型量级（参考） |
|---|---|---|
| (\sigma_a) | 加速度计噪声密度 | (10^{-3} \sim 10^{-2} \ \text{m/s}^2/\sqrt{\text{Hz}}) |
| (\sigma_g) | 陀螺噪声密度 | (10^{-4} \sim 10^{-3} \ \text{rad/s}/\sqrt{\text{Hz}}) |
| (\sigma_{bg}) | 陀螺零偏随机游走 | (10^{-6} \sim 10^{-5} \ \text{rad/s}^2/\sqrt{\text{Hz}}) |
| (\sigma_{ba}) | 加速度计零偏随机游走 | (10^{-5} \sim 10^{-4} \ \text{m/s}^3/\sqrt{\text{Hz}}) |
这些参数最靠谱的来源是 Allan 方差分析，或者用转台做离线标定。没有标定条件时，先按数据手册给的值设一个初值，然后做一组简单实验：静止状态下看滤波输出的姿态是否稳定，匀速直行看速度反馈是否平滑。过程噪声设小了，残差序列会长期偏离零均值，设大了协方差会偏大、滤波结果抖动明显。调参不是玄学，核心诊断工具就是看 innovation 序列的统计特性。
5.3 可观测性视角下的传感器组合
搞ESKF不能只会套公式，还得理解哪些状态到底能不能被观测到。以IMU+GPS这个常见组合为例：水平姿态（横滚角、俯仰角）可以通过加速度计“观测”重力方向来估计，所以长时间不漂；航向角则完全不可观测，纯惯性下会随陀螺零偏和积分误差缓慢漂移，要靠磁力计或外部航向观测约束。陀螺零偏在持续转动激励时估计得比较准，长时间匀速直线运动时几乎不可观测，零偏估计会“冻结”。
这个结论直接决定系统设计：纯IMU不加外部观测，ESKF只能用统计最优的方式把IMU积分误差描述清楚，并不能根治漂移。加速度计零偏更是难兄难弟，它和姿态误差存在耦合，只有车辆有纵向加速度激励的时候才更可观测。理解了这层，你就能明白为什么视觉惯性系统里一定要有足够的加速度和角速度激励，纯悬停状态对零偏估计完全不友好。
5.4 实现避坑清单
以下每一条都是我踩过或者看别人踩过的坑，整理成清单：
- 坐标系约定要统一。IMU加速度计输出的比力方向，世界系下重力向量取正值还是负值，不同代码库的表达不一样，必须先搞清楚。
- IMU和外部传感器的时间戳必须对齐。哪怕只有几十毫秒的固定延迟，都会在残差里引入系统性偏差，表现的信号是“滤波器始终有固定大小的innovation”。
- 四元数乘法的左右顺序要和误差定义一致。右扰动下是 (\hat{q}\otimes\exp(\delta\theta))，别在某个环节混成左乘。
- 全程用双精度运算。单精度浮点在长时间积分下的累积误差会让姿态精度明显下降。
- 每步对 (P) 做对称化，必要时做特征值裁剪，确保正定性。
- 加速度计读数如果被归一化成重力倍数，要通过数据手册或离线测试确认，别稀里糊涂把 (g) 写成9.8又乘一遍。
6. 调试经验与常见问题速查
6.1 典型故障现象与解决方向
调试ESKF时，我总结了一套“现象→原因→排查方向”的速查表，实际用起来比硬看公式高效得多。
| 现象 | 可能原因 | 排查方向 |
|---|---|---|
| 静止时姿态缓慢漂移 | 陀螺零偏初值不准，或 (\sigma_{bg}) 设得过大、零偏没被约束住 | 重新做静止初始化，调小 (\sigma_{bg}) |
| 位置发散非常快 | 加速度计零偏未估计，或初始姿态误差偏大 | 检查重力方向是否对齐，引入速度/位置观测 |
| 有外部观测但P持续变小且状态不修正 | 更新后忘了把误差状态注入到标称状态，只剩协方差变化 | 检查注入代码，打印注入前后的位置差值 |
| 协方差不对称或出现负特征值 | 长期未对称化，或重置G矩阵缺失 | 补对称化，补G变换 |
| 四元数模长偏离1 | 标称状态更新时用了加性修正而非乘法 | 修正姿态更新逻辑 |
| 残差均值始终不为零 | 时间戳对齐有问题，或观测模型偏差 | 做延迟标定，检查H矩阵 |
6.2 我的标准调试流程
我调试ESKF不会直接拿真机数据硬啃，而是按下面这个顺序来，基本能快速定位问题。
第一步，先做仿真。用一段已知轨迹生成带噪声的IMU数据，先只跑标称状态的前向积分，看位置姿态的漂移幅度是否符合理论预测。如果前向积分这一步就不对，滤波器再怎么调也没用。
第二步，在仿真环境里跑完整ESKF，逐步加入外部观测。先只加GPS位置，看水平姿态和位置能否稳定；再加入测速观测，看能否进一步压住速度漂移；最后加航向观测，检查航向收敛情况。每加一种观测，都停下来看对应的innovation序列是否满足零均值。
第三步，切换到真实数据后，先做静止数据测试。静止时加速度计均值应该大致等于重力，陀螺均值应该接近零偏附近，这个阶段发现的异常十有八九是标定问题而不是滤波算法问题。
第四步，看协方差的收敛情况。仿真中对真实误差归一化，统计误差落在3σ以内的比例，超过95%就说明协方差标定基本靠谱。真实场景没有真值，但可以对比高精度参考系统，看滤波输出的不确定度是否和实际误差匹配。
6.3 几个真正提升效率的习惯
最后分享几个调试中非常管用的习惯。一个是把误差状态的各个分量拆出来单独画曲线，不要只盯着总的位置误差，这样可以快速看出漂移主要来自姿态误差还是零偏误差。另一个是在代码里把每个环节的中间量都留log接口，尤其是 (K y) 的值和注入后的状态增量，调试时直接对比这些量能省很多时间。再一个是调参时保持只改一个参数的纪律，一次动两三个参数会把问题搅在一起，反而更难判断是哪个环节出了问题。
这篇文章是我对整个ESKF理论框架的一次系统梳理，属于持续更新系列的起点。后续我计划补三块内容：一是GNSS、视觉、磁力计等不同观测模型的紧耦合细节，二是IMU标定与Allan方差分析的实际操作，三是ESKF在视觉惯性里程计里对外参和延迟的估计方法。我自己在协方差重置那个G矩阵上栽过大跟头，那次教训让我把所有公式重新手推了一遍，所以这篇里凡是涉及符号约定的地方我都尽量写清楚了。如果你在实现中也遇到类似“看着收敛但实际误差不小”的情况，大概率也要回头检查一下误差状态定义和重置步骤。先记到这，下次更新见。
