1. 走上ESKF这条路之前标准卡尔曼在IMU融合里的三个硬伤先从一个我实际踩过的坑说起。好几年前我在做一个室内移动机器人的定位模块硬件配置很简单一个消费级IMU、一个低频UWB定位基站期望输出20Hz左右的平滑位置。最开始图省事直接用标准的扩展卡尔曼滤波去融合状态量选了位置、速度、姿态四元数和两个零偏。跑起来之后发现一个特别诡异的现象滤波跑着跑着四元数的模长会慢慢偏离1然后在某次更新之后姿态突然跳变一下接着位置也跟着发散。当时我以为是代码里哪里忘了归一化翻来覆去找了两天才意识到这不是简单的工程失误而是用四元数当状态向量本身就有问题。这就是误差状态卡尔曼滤波Error State Kalman FilterESKF想要解决的核心问题。这套方法最早在惯性导航领域被大量使用后来随着视觉惯导里程计VIO和组合导航系统的普及又回到了机器人圈子的视野里。它不是什么全新的滤波理论而是对EKF的一种改进用法不是直接对完整状态做滤波而是把状态拆成名义状态和误差状态把标准卡尔曼滤波作用在误差上。你不需要懂很深的理论就能把ESKF用起来但如果你不想在工程里踩一堆莫名其妙的坑最好还是先搞清楚它到底解决了什么问题。1.1 四元数维度和约束问题先看姿态表示。一个三维刚体的姿态自由度是3。用旋转矩阵表示需要9个数用四元数表示需要4个数但它们都有一个共同点这些表示方式不是自由向量而是带约束的。旋转矩阵要求正交且行列式为1四元数要求模长为1。标准卡尔曼滤波的所有推导都假设状态是一个欧几里得向量空间的元素你可以对状态做加减法、乘以矩阵、做高斯分布假设。可四元数不是这样的你把两个四元数加起来模长大概率就不是1了你没法在四元数空间里直接定义一个有意义的高斯噪声。你可能会说那我滤波完之后做一次归一化不就行了很多初学者的确这么干。但问题在于卡尔曼滤波器内部的协方差矩阵表达的是状态估计值的不确定性如果你强行把状态投影回约束流形上这个投影操作会对协方差产生什么影响你完全不知道。归一化这一步破坏了滤波器对不确定性的描述短期看没事长期跑下来协方差和实际误差就会越来越不匹配最终导致滤波发散。ESKF对这个问题处理得非常优雅真实状态被拆成名义状态和误差状态名义状态在流形上运动负责承接非线性动力学而误差状态是一个小量可以安全地当作平面向量来处理。1.2 线性化精度问题标准EKF的线性化点是当前状态估计值。如果状态估计值和真值之间误差很大那么围绕估计值做一阶泰勒展开得到的雅可比矩阵并不能很好地代表系统在这一刻的真实动态。尤其在IMU积分这种强非线性系统里姿态误差会通过旋转矩阵耦合进位置和速度的预测中一个小的姿态误差在几秒内就可能被放大成很大的位置漂移。ESKF的思路是把大误差留给名义状态的非线性积分去处理误差状态几乎总是保持在一个很小的邻域内因此对误差状态做线性近似精度远高于对完整状态做线性近似。用弗拉基米尔·贝洛维奇经常说的一句话就是误差状态的线性化误差比真实状态的线性化误差小一个数量级。1.3 可观测性和退化问题还有一个工程上的实际问题:直接对完整状态做EKF时状态向量通常包含位置、速度、姿态、陀螺仪零偏、加速度计零偏这一共16维。但很多场景下你手里的观测根本不足以同时把这些量都约束住。比如室内只有位置观测的时候姿态和零偏的可观测性就很弱。ESKF并没有从数学上改变系统的可观测性但它给了你一个非常自然的工具去控制这种情况——你可以对待估计的误差状态做降维处理只对当前可观测的误差状态做更新其余部分继续靠名义状态积分去推。这种灵活性在实际工程里非常有用我后面会细说。2. ESKF的核心思想把状态拆成大体量和小误差两半ESKF的数学框架看起来有点绕但本质上是这样一个关系x_true x_nominal ⊕ x_error这里⊕表示流形上的复合运算。以惯性导航为例真实状态由以下部分组成位置 p三维速度 v三维姿态 q四元数加速度计零偏 b_a三维陀螺仪零偏 b_g三维名义状态同样包含这五项它们的区别在于名义状态完全由IMU测量积分而来不考虑测量噪声和零偏的细节影响只是用当前的零偏估计值去补偿IMU原始测量。误差状态则用来表达名义状态和真实状态之间的差距。2.1 真实状态、名义状态、误差状态的定义用符号来写就是这样位置误差δp p_true - p_nom速度误差δv v_true - v_nom姿态误差δθ log(q_nom^{-1} ⊗ q_true)这是一个三维旋转向量零偏误差δb_a b_a_true - b_a_nomδb_g b_g_true - b_g_nom这里最关键的是姿态误差。它被定义为名义姿态到真实姿态之间的旋转向量是一个三维量。这样整个误差状态向量就是15维的δx [δp, δv, δθ, δb_a, δb_g] ∈ R^15你注意到没有误差状态是普通向量它对加减法封闭可以放心地用高斯分布去描述它的不确定性。这是ESKF整个算法能够成立的基石。2.2 为什么误差状态可以安全地线性化误差状态的运动学方程推导出来后会呈现一个非常漂亮的线性形式。原因在于当δθ是一个小量时旋转矩阵可以展开为一阶近似。如果你去看误差状态的连续时间方程会发现它只在一两个地方出现状态的乘积而且那些乘积都是小量乘积可以直接忽略。这意味着预测方程可以写成δx_dot F_c * δx w其中F_c是一个15x15的矩阵w是噪声项。这正是卡尔曼滤波最经典的形式不需要像EKF那样在每一时刻重新计算非线性函数的雅可比矩阵。即使要算也只是一个简单的矩阵推导一次就能固定下来。2.3 误差状态的注入与重置每次滤波更新完成后误差状态会被注入回名义状态这一步叫reset。操作是把名义状态和误差状态重新复合然后把误差状态置为零同时更新协方差矩阵。注意这个重置操作不是简单地置零它会导致协方差矩阵产生一个变换因为名义状态变了误差状态的原点也变了。如果不做这一步协方差调整滤波器同样会出问题。这个细节在教科书里经常被一笔带过但在工程实现中是个重要的坑我后面会专门讲。这个过程其实很像一个反馈控制器名义状态是前馈积分误差状态是反馈校正卡尔曼滤波负责决定这个反馈强度。3. 从连续时间到离散化ESKF的完整推导过程先明确我们讨论的是最经典的IMU外部观测场景。IMU提供加速度计和陀螺仪的原始测量用来做状态预测外部观测GNSS、UWB、视觉位姿等用来对预测结果进行修正。3.1 IMU测量模型和误差状态运动学IMU的测量模型可以写成加速度计测量a_m R^T (a - g) b_a n_a陀螺仪测量ω_m ω b_g n_g其中a是物体在世界系下的真实加速度g是重力向量R是当前姿态b_a和b_g是零偏n_a和n_g是测量白噪声。名义状态的连续时间方程是ṗ_nom v_nomv̇_nom R_nom * (a_m - b_a_nom) gq̇_nom q_nom ⊗ [0, ω_m - b_g_nom] / 2ḃ_a_nom 0ḃ_g_nom 0这里名义状态认为零偏不变全部由滤波更新去修正。误差状态的连续时间方程可以通过对真实状态和名义状态做差分推导出来。这里跳过复杂的推导过程直接给出在机器人领域最常用的形式。定义F_c矩阵中用到两个3x3反对称矩阵[a]×表示a的反对称矩阵误差状态方程δṗ δv δv̇ -R_nom * [a_m - b_a_nom]× * δθ - R_nom * δb_a - R_nom * n_a δθ̇ -[ω_m - b_g_nom]× * δθ - δb_g - n_g δḃ_a n_ba δḃ_g n_bg这个方程组就是整个ESKF预测部分的核心。它已经是一个线性方程组矩阵F_c的形式是F_c [ 0 I 0 0 0 ] [ 0 0 -R*[a]× -R 0 ] [ 0 0 -[ω]× 0 -I ] [ 0 0 0 0 0 ] [ 0 0 0 0 0 ]其中a a_m - b_a_nomω ω_m - b_g_nom。这个矩阵的稀疏性非常好在实际实现中可以有两种选择直接用稀疏矩阵乘法或者把这个15x15矩阵分块乘到9维的核心状态上。考虑到GPS/视觉融合场景下的实时性要求建议直接按分块乘写省去不必要的零矩阵乘法。3.2 离散化用中值法处理角速度积分连续时间方程要落地到代码里必须离散化。最常用的做法是中值法用当前时刻和上一时刻的IMU测量平均作为整体时间段内的等效测量值。这样比简单的欧拉法精度高不少而且代码量增加很小。对于状态转移矩阵可以直接用一阶近似F_d I F_c * Δt二阶近似F_d I F_c * Δt 0.5 * F_c^2 * Δt^2从实际效果看在IMU频率为100-200Hz、单步时间5-10毫秒的情况下一阶近似已经足够。只有在IMU频率低于50Hz或者运动特别剧烈时才需要二阶近似。我一般默认用一阶近似把算力留给更重要的协方差更新。离散化后的误差状态预测方程变成δx_k1 F_d * δx_k w_k协方差更新P_k1 F_d * P_k * F_d^T Q_dQ_d是离散化的过程噪声协方差。根据连续时间噪声的功率谱密度可以用如下近似Q_d ≈ F_d * G_c * Q_c * G_c^T * F_d^T * Δt其中G_c是噪声输入矩阵Q_c是连续时间的噪声功率谱密度矩阵。3.3 预测协方差的更新过程噪声Q_d的构造需要特别注意。常用的方法是把IMU测量噪声和零偏随机游走分成两部分Q_d Q_meas Q_biasQ_meas来源于加速度计和陀螺仪的测量白噪声Q_bias来源于零偏的随机游走。在实际工程中测量噪声和零偏随机游走的方差数值可能差好几个数量级这会让协方差矩阵P的条件数变得很大。一个实用的做法是在协方差更新时用double类型计算并且每隔一段时间对P做一次对称化处理防止数值误差破坏对称性。4. 观测更新怎么把GPS/视觉的测量打进误差状态预测部分只依赖IMU任何外部信息都通过更新方程进入系统。ESKF在更新方程上的优势在这里体现得很明显——因为误差状态是线性的观测模型只需要关心误差状态到测量残差这一层线性关系不需要对原状态做任何雅可比推导。4.1 观测模型的一般形式假设外部观测器和状态之间满足z h(x_true) v我们把它分解成z h(x_nom ⊕ δx) ≈ h(x_nom) H * δx v于是测量残差为y z - h(x_nom)对应的观测矩阵H ∂h / ∂δx | δx0。这一步推导通常比直接EKF要简单因为δx的维度低而且h往往只在少数维度上依赖误差状态。4.2 位置和速度观测的具体雅可比最常见的观测是GNSS/UWB的位置观测。这种情况h(x) p_true因此z p_nom δp v y z - p_nom H [I_3x3, 0, 0, 0, 0]就这么简单。如果你有速度观测比如轮式里程计或视觉光流速度H的第二块是I_3x3。对于姿态观测比如视觉定位输出四元数情况稍微复杂。观测模型是z_q q_true q_nom ⊗ q(δθ)残差可以通过计算z_q与q_nom的旋转差得到δz log(z_q ⊗ q_nom^{-1})这里δz本身就是一个三维旋转向量它直接就是δθ的一个含噪观测。所以H矩阵在第三块是I。这个性质非常清爽旋转残差天然就是误差状态的一部分不需要额外推导复杂的雅可比这是标准EKF做不到的。4.3 更新后的状态合成与协方差处理拿到H、y、观测噪声R后标准卡尔曼更新公式直接套用K P * H^T * (H * P * H^T R)^{-1} δx K * y P (I - K * H) * P这里要注意K计算使用的是预测协方差P而P的单位是误差状态的协方差这个点必须想清楚因为它和标准EKF里P的含义不完全一样。更新完的δx要注入名义状态p_nom ← p_nom δpv_nom ← v_nom δvq_nom ← q_nom ⊗ q(δθ)b_a_nom ← b_a_nom δb_ab_g_nom ← b_g_nom δb_g然后误差状态清零协方差做一次reset变换G I, G[3:6, 3:6] I - [0.5 * δθ]× 实际上是根据姿态误差的注入方式确定 P ← G * P * G^T我见过的不少实现会直接跳过这一步在协方差较大时这个近似会导致滤波性能下降。严谨的做法还是保留。5. 工程实现骨架从矩阵到能跑的C代码理论讲得再多最终还是要落到代码。我在这里给一个精简但完整的ESKF核心实现框架基于Eigen库适用于GNSSIMU融合场景。5.1 核心数据结构和SO3运算#include Eigen/Dense #include Eigen/Geometry struct ImuMeasurement { Eigen::Vector3d acc; Eigen::Vector3d gyro; double timestamp; }; struct ErrorState { Eigen::Vector3d dp; Eigen::Vector3d dv; Eigen::Vector3d dtheta; Eigen::Vector3d dba; Eigen::Vector3d dbg; }; class ESKF { public: // 名义状态 Eigen::Vector3d p_ Eigen::Vector3d::Zero(); Eigen::Vector3d v_ Eigen::Vector3d::Zero(); Eigen::Quaterniond q_ Eigen::Quaterniond::Identity(); Eigen::Vector3d ba_ Eigen::Vector3d::Zero(); Eigen::Vector3d bg_ Eigen::Vector3d::Zero(); // 误差状态协方差 Eigen::Matrixdouble, 15, 15 P_ Eigen::Matrixdouble, 15, 15::Identity(); // 噪声参数应考虑从配置读取 double noise_acc_ 0.01; // 加速度计噪声标准差 double noise_gyro_ 0.001; // 陀螺仪噪声标准差 double noise_acc_bias_ 0.001; // 加速度计零偏随机游走 double noise_gyro_bias_ 0.001; // 陀螺仪零偏随机游走 };SO3的操作直接用Eigen的Quaterniond和AngleAxis就可以。我自己封装了一个小函数方便做旋转向量到四元数的转换static Eigen::Quaterniond Vec2Quat(const Eigen::Vector3d vec) { double angle vec.norm(); if (angle 1e-12) return Eigen::Quaterniond::Identity(); Eigen::Vector3d axis vec / angle; return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); }5.2 Predict函数的实现Predict函数接收两个相邻IMU测量用中值法预测名义状态并且更新误差状态协方差void Predict(const ImuMeasurement imu_main, const ImuMeasurement imu_prev) { double dt imu_main.timestamp - imu_prev.timestamp; // 中值法 Eigen::Vector3d acc 0.5 * (imu_main.acc imu_prev.acc) - ba_; Eigen::Vector3d gyro 0.5 * (imu_main.gyro imu_prev.gyro) - bg_; // 名义状态预测 Eigen::Quaterniond dq Vec2Quat(gyro * dt); q_ (q_ * dq).normalized(); Eigen::Vector3d a_world q_ * acc Eigen::Vector3d(0, 0, -9.81); v_ a_world * dt; p_ v_ * dt; // 误差状态转移矩阵 Eigen::Matrixdouble, 15, 15 F Eigen::Matrixdouble, 15, 15::Identity(); Eigen::Matrix3d I3 Eigen::Matrix3d::Identity(); Eigen::Matrix3d R q_.toRotationMatrix(); F.block3, 3(0, 3) I3 * dt; F.block3, 3(3, 6) -R * Skew(acc) * dt; F.block3, 3(3, 9) -R * dt; F.block3, 3(6, 6) -Skew(gyro) * dt; F.block3, 3(6, 12) -I3 * dt; // 离散噪声协方差 Eigen::Matrixdouble, 15, 15 Q Eigen::Matrixdouble, 15, 15::Zero(); Q.block3, 3(3, 3) R * (noise_acc_ * noise_acc_ * I3) * R.transpose() * dt * dt; Q.block3, 3(6, 6) noise_gyro_ * noise_gyro_ * I3 * dt * dt; Q.block3, 3(9, 9) noise_acc_bias_ * noise_acc_bias_ * I3 * dt; Q.block3, 3(12, 12) noise_gyro_bias_ * noise_gyro_bias_ * I3 * dt; // 协方差更新 P_ F * P_ * F.transpose() Q; // 对称化 P_ 0.5 * (P_ P_.transpose()); }其中Skew函数用来构造反对称矩阵static Eigen::Matrix3d Skew(const Eigen::Vector3d v) { Eigen::Matrix3d m; m 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return m; }这里有一个值得注意的地方位置和速度的初始值matters很大。如果初始位置/速度不准协方差P的初始值应该设置对应的不确定度不要直接设为零矩阵。零协方差会被卡尔曼增益公式放大成位置观测完全不可信的效果导致滤波器一开始就剧烈调整反而起不到平滑作用。5.3 UpdateWithGNSS的实现GNSS位置更新是最典型的场景bool UpdateWithGNSS(const Eigen::Vector3d pos_gnss, double timestamp) { // 观测残差 Eigen::Vector3d y pos_gnss - p_; // 观测矩阵 Eigen::Matrixdouble, 3, 15 H; H.setZero(); H.block3, 3(0, 0) Eigen::Matrix3d::Identity(); // 观测噪声 Eigen::Matrix3d V Eigen::Matrix3d::Identity() * gnss_noise_; // 卡尔曼增益 Eigen::Matrixdouble, 15, 3 K; Eigen::Matrixdouble, 3, 3 S H * P_ * H.transpose() V; K P_ * H.transpose() * S.inverse(); // 误差状态更新 Eigen::Matrixdouble, 15, 1 dx K * y; ErrorState es; es.dp dx.block3, 1(0, 0); es.dv dx.block3, 1(3, 0); es.dtheta dx.block3, 1(6, 0); es.dba dx.block3, 1(9, 0); es.dbg dx.block3, 1(12, 0); // 注入名义状态 p_ es.dp; v_ es.dv; q_ (Vec2Quat(es.dtheta) * q_).normalized(); ba_ es.dba; bg_ es.dbg; // 协方差更新 Eigen::Matrixdouble, 15, 15 I Eigen::Matrixdouble, 15, 15::Identity(); P_ (I - K * H) * P_; // 误差状态重置的协方差修正 Eigen::Matrixdouble, 15, 15 G Eigen::Matrixdouble, 15, 15::Identity(); G.block3, 3(6, 6) I - Skew(0.5 * es.dtheta); P_ G * P_ * G.transpose(); P_ 0.5 * (P_ P_.transpose()); return true; }这个实现看起来不长但每一个小块都有明确的含义。如果你想把ESKF从GNSS融合改成视觉位姿融合只需要改H和y两部分其他代码几乎可以原样复用。这也是ESKF在工程上比标准EKF更受欢迎的原因之一算法的骨架是稳定的换传感器只需要换观测模型的适配层。6. 调参和踩坑我在实际项目里遇到的问题6.1 零偏随机游走的噪声参数怎么定ESKF里的过程噪声参数有一部分是IMU数据手册直接给的比如加速度计测量噪声的功率谱密度。但零偏随机游走这一项很多IMU数据手册给的是零偏稳定性单位是deg/h需要换算成随机游走的功率谱密度。一个常见的工程做法是把数据手册给的零偏稳定性数值除以sqrt(3600)近似当作离散的随机游走标准差。但这里有个非常实际的问题这个值通常只是近似。我遇到过好几次同一个IMU型号在不同批次的产品上随机游走参数能差3倍。如果你的滤波器对零偏收敛特别敏感最可靠的办法是采集一段静止或匀速运动的数据用Allan方差分析工具比如imu_utils或pyallan去拟合出实际的噪声参数。6.2 协方差矩阵的对称性一个小操作省好多bug这看起来像是一个小问题但实际影响非常大。卡尔曼滤波的协方差更新公式P (I - KH)P在理想代数下是严格对称的。但在浮点运算下经过几十上百次迭代非对称项会逐渐累积最终可能导致下三角矩阵里的某个元素变成负数然后卡尔曼增益计算出负的方差滤波器直接崩掉。我在代码里会在每次更新P之后做一次对称化处理P_ 0.5 * (P_ P_.transpose());这个操作成本几乎可以忽略但它能保证协方差矩阵一直保持数学上的合法性。这不是炫技纯粹是工程上的保命操作。6.3 误差状态清零之后别忘了重置P的耦合项在大多数ESKF实现中注入操作完成后误差状态变量会被清零。但这里有一个很多人会忽略的点注入操作改变了名义状态而名义状态是误差状态线性化的参考点。虽然误差状态本身被清零了但P矩阵里那些表示误差状态之间相关性的项在新的参考点下的数值应该和旧参考点下的数值不同。为了修正这一点常规做法是引入一个雅可比矩阵G它描述了误差状态在新旧参考点之间的变换关系。对于姿态误差的注入G.block3, 3(6, 6) I - Skew(0.5 * δθ)这个近似在δθ较小时精度足够。如果δθ很大比如超过10度更好的做法是用完整的SO(3)左雅可比矩阵。我一般会监控δθ的范数如果发现每次更新的角度偏差超过一定阈值就该怀疑是不是观测噪声参数设置过大了。6.4 初值和坐标系对齐的影响ESKF不像EKF那样直接对初始状态求逆但它依然对初始状态很敏感。初始姿态误差如果大于30度误差状态的线性化假设就失效了滤波器在刚开始的几十步里可能输出震荡的结果。我通常会在正式滤波开始前利用静止时刻的加速度计读数初始化俯仰和横滚角利用重力方向再用磁力计或外部观测初始化航向角。这一步看似简单但从根上避免了系统在大初始误差下挣扎的情况。7. 什么时候选ESKF什么时候别选方案取舍参考ESKF并不是万能药。我在实际项目中总结了一些选型经验可以作为参考。ESKF非常合适用来处理IMU和外部低频传感器GNSS、UWB、视觉里程计融合。状态预测频率和更新频率解耦这是它的天然优势——你可以在200Hz上预测在10Hz上更新并且两者的代码逻辑完全独立。同时它的计算量比UKF和粒子滤波低一个量级在嵌入式平台上跑起来毫无压力。如果你的系统几乎没有外部更新全靠IMU推导那ESKF帮助也不大。这种情况下误差状态的协方差会持续增长滤波器本身并不能阻止积分发散。这时候你应该考虑的是做零速修正ZUPT或者增加传感器约束。如果你的系统是纯视觉SLAM后端有回环检测和全局优化那么ESKF通常只作为IMU的前端预测模块最终的全局位姿估计还是用因子图优化来做。ESKF的价值在于提供一个高频、低延迟的里程计输出以及给优化后端提供可靠的相对约束信息。还有一类场景是纯姿态估计比如无人机飞控的姿态解算。ESKF在这里显得有点重——Mahony互补滤波和它的变体在姿态精度、计算开销、调参便利性上都更合适。ESKF的优势在于它同时估计位置、速度、姿态和零偏如果你只需要姿态用15维状态向量是杀鸡用牛刀。如果你使用的IMU噪声特别大、运动特别剧烈ESKF的线性化假设也会受到挑战。误差状态虽然是小量但它不会自己保证是小量——卡尔曼增益如果太大误差状态的一次更新就可能跳出一个很大的值破坏线性化假设。解决办法是限制单次更新的最大角度变化或者在更新前后检查误差状态范数超阈值时缩减增益。这也是我在工程中会主动加的一道保护逻辑。回到文章最开始那个UWB定位的项目。后来我把融合算法从标准EKF换成了ESKF同一份数据、同样的噪声参数定位轨迹的平滑程度和稳定性都提升了不止一个台阶。最关键的是姿态和位置之间的耦合问题被彻底绕过去了四元数归一化检查从这个项目里消失了。从那以后凡是涉及IMU外部观测的融合任务我的默认方案就是ESKF教科书里的误差状态运动学推导我到现在还能默写出来。这套方法的好用程度用过的人都懂。