误差状态卡尔曼滤波ESKF：从推导到工程落地一次讲透

刚接触组合导航或视觉惯性里程计的人大概率都会在某个晚上对着屏幕怀疑人生明明卡尔曼滤波都学过了怎么一到姿态估计就崩Euler角奇异、四元数归一化约束、雅可比线性化误差……问题一个接一个。后来我碰到误差状态卡尔曼滤波ESKFError-State Kalman Filter才真正体会到什么叫“把问题拆开揉碎”。这篇文章不搞虚的直接从推导出发把ESKF为什么要这么设计、每一步数学上在干什么、工程上怎么落地一次讲透。ESKF的核心思想很朴素把滤波器要估计的状态拆成“名义状态”和“误差状态”。名义状态是一个直观、稳定、好理解的大状态位置、速度、姿态、零偏误差状态则是描述“名义状态离真实状态还差多少”的小偏差。名义状态用IMU测量直接积分推进大偏差、非线性、奇异性的问题都交给它去背误差状态则始终是个小量用卡尔曼滤波线性更新线性化精度自然就高。这套架构在无人机、自动驾驶、AR/VR设备里被广泛使用适合所有要做惯性导航、位姿估计、传感器融合的开发者。1. 为什么要绕这么大一圈ESKF的设计动机1.1 直接EKF到底哪里不好用先回顾一下最朴素的想法把位置、速度、姿态、陀螺零偏、加速度计零偏全部塞进一个状态向量然后用扩展卡尔曼滤波直接估计这个思路在理论上没什么问题实践起来却处处是坑。第一个坑是姿态表示。姿态如果写成欧拉角会碰到万向锁如果写成旋转矩阵状态向量里要带9个数约束6个自由度还必须在滤波更新后重新做正交化如果写成四元数虽然紧凑且数值稳定但四元数有归一化约束模长为1不是普通向量空间。卡尔曼滤波的整个推导都建立在状态空间是线性向量空间的前提上归一化约束一旦被破坏滤波器的协方差估计就慢慢失真。第二个坑是线性化误差。EKF的更新公式依赖泰勒展开的一阶项泰勒展开成立的前提是误差足够小。可是姿态在大机动过程中变化很快直接滤波时线性化参考点往往离真实值很远一阶近似误差被放大滤波器很容易发飘。第三个坑是噪声模型的适配问题。IMU零偏通常建模为随机游走随机游走本身是一个缓慢的、非线性的过程。直接在原状态里估计bias和姿态之间高度耦合雅可比计算复杂一旦近似不到位整个收敛过程会变得很别扭。1.2 ESKF的三个核心优势ESKF把误差从大状态里剥离出来很多麻烦自动化解。姿态处理方面旋转矩阵R被当作“名义状态”直接积分更新积分过程在流形上走不会离开SO(3)误差旋转用一个三维向量δθ描述它就是so(3)上的李代数元素维度很小没有冗余约束问题。线性化精度方面误差状态在每一帧更新后会被重新“清零”下一帧的误差重新从零附近出发所以泰勒展开的参考点始终贴近真实值。误差再大也不过是几度几厘米的数量级一阶近似非常可靠。降维方面姿态误差只有3维比旋转矩阵的9维和四元数的4维都更紧凑协方差不会引入冗余方向上的伪不确定性。我用一个生活化类比名义状态像一个人按导航走路的大致路线误差状态像每一步踩偏的微小偏差。你只管修正每一步的偏差而不是每次都要重新规划整条路线。ESKF就是这么省的。2. 误差状态是什么坐标系、符号约定与状态拆分2.1 先把符号体系理顺推导之前约定一套清晰、前后一致的符号非常重要。这里我用惯性系世界系下标w、机体系下标tbody系不加下标的物理量一般默认在body系。名义位置 p名义速度 v名义旋转 Rp和v在世界系下R是body系到世界系的旋转矩阵。陀螺仪测量 ω_m加速度计测量 a_m在body系下。陀螺零偏 b_g加速度计零偏 b_a在body系下。随机游走过程噪声 n_bg、n_ba。陀螺测量噪声 n_g、加速度计测量噪声 n_a。IMU测量模型按标准惯导惯例写ω_m ω b_g n_g a_m a b_a n_a其中ω是真实角速度body系a是真实比力body系。注意很多资料里把加速度计测量写成 a_m R^T(a_w - g) b_a n_a 的形式两种写法等价关键是你得清楚自己的符号体系。我这里用“比力在body系”的写法后面推导会更紧凑。2.2 名义状态与误差状态的数学定义真值状态和名义状态的关系定义为p_t p δp v_t v δv R_t R exp(δθ^∧) b_gt b_g δb_g b_at b_a δb_a其中exp是SO(3)的指数映射δθ是三维的旋转误差向量δθ^∧是δθ的反对称矩阵。这里有一个重要选择旋转误差是定义在右侧的右扰动即真实的旋转等于名义旋转右乘一个小旋转。右扰动还是左扰动没有绝对的对错但推导出的结论形式会不同混用是工程里最常见的翻车原因。本文全程使用右扰动。有一个细节值得多说两句为什么不把旋转误差写成简单的加法 δR R_t - R因为矩阵加法和旋转群结构不兼容两个旋转矩阵相加大概率不再是一个旋转矩阵。而 exp(δθ^∧) 是SO(3)上的小旋转R乘以它仍在SO(3)上这才是在流形上做操作的正确姿势。类比一下地球表面是个球面你想表达“从当前位置往东偏一点”不能直接用平面坐标加上去得沿着球面走一小段弧exp映射干的就是这件事。2.3 名义状态运动学用测量值硬推名义状态的微分方程直接来自IMU测量ṗ v v̇ R(a_m - b_a) g Ṙ R(ω_m - b_g)^∧这三个式子没什么花哨的。加速度计测到的比力减去零偏再旋转到世界系叠加重力就是世界系下的加速度。陀螺仪测到的角速度减去零偏就是名义旋转的瞬时角速度。名义状态里b_g和b_a在预测阶段通常认为不变所以 ḃ_g 0ḃ_a 0。这套名义状态运动学是整个ESKF的“炮架”它的行为像一辆没有反馈的航迹推算车短时间很精确长时间必然漂移。误差状态滤波器的任务就是不断给这辆漂移的车纠偏。3. 连续时间误差状态方程推导全过程3.1 位置误差和速度误差位置误差最直接δṗ ṗ_t - ṗ v_t - v δv速度误差需要把真值方程和名义方程做差δv̇ v̇_t - v̇ R_t a_t g - [R(a_m - b_a) g]由测量模型反过来解出真值比力 a_t a_m - b_gt - n_a。又因为 b_gt b_g δb_g所以a_t a_m - b_g - δb_g - n_a (a_m - b_a) - δb_a - n_a代入得到δv̇ R exp(δθ^∧)(a_m - b_a - δb_a - n_a) - R(a_m - b_a)对 exp(δθ^∧) 做一阶泰勒展开 exp(δθ^∧) ≈ I δθ^∧并忽略二阶小量δv̇ ≈ R[ (I δθ^∧)(a_m - b_a - δb_a - n_a) - (a_m - b_a) ] R[ δθ^∧(a_m - b_a) - δb_a - n_a ] -R(a_m - b_a)^∧ δθ - R δb_a - R n_a这里用到了叉积性质 δθ^∧ a -a^∧ δθ。最终的速度误差方程δv̇ -R(a_m - b_a)^∧ δθ - R δb_a - R n_a3.2 旋转误差李代数的微积分旋转误差的推导是整个ESKF最微妙的部分。我们已知 R_t R exp(δθ^∧)对两边求时间导数。左边Ṙ_t R_t Ω_t R exp(δθ^∧) ω_t^∧右边用乘积法则d/dt [R exp(δθ^∧)] Ṙ exp(δθ^∧) R d/dt exp(δθ^∧) R(ω_m - b_g)^∧ exp(δθ^∧) R exp(δθ^∧)(δθ̇)^∧一阶近似下 d/dt exp(δθ^∧) ≈ exp(δθ^∧)(δθ̇)^∧这个近似对应高阶小量被舍弃。两边相等后左乘 exp(-δθ^∧)exp(-δθ^∧) ω_t^∧ exp(δθ^∧) (ω_m - b_g)^∧ (δθ̇)^∧再对左边做一阶近似。这一步计算要小心利用伴随性质 exp(-δθ^∧) ω_t^∧ exp(δθ^∧) 等价于旋转向量 ω_t 被旋转一个微小角度一阶展开得到ω_t^∧ - [(δθ × ω_t)]^∧ (ω_m - b_g)^∧ (δθ̇)^∧其中 δθ × ω_t ω_t^∧ δθ。整理向量部分ω_t ω_m - b_g ω_t^∧ δθ δθ̇真值角速度 ω_t ω_m - b_gt - n_g ω_m - b_g - δb_g - n_g。代入δθ̇ ω_m - b_g - δb_g - n_g - (ω_m - b_g) - ω_t^∧ δθ -ω_t^∧ δθ - δb_g - n_g最后用名义角速度近似真值角速度误差是高阶小量得到δθ̇ -(ω_m - b_g)^∧ δθ - δb_g - n_g这个带负号的形式是右扰动约定下的标准结果。如果你换用左扰动 R_t exp(δθ^∧) R得到的会是 δθ̇ -R^T(ω_m - b_g - ...)? 形式完全不同所以“全程保持同一个约定”这句提醒真的不是废话。3.3 零偏误差零偏建模为随机游走δḃ_g n_bg δḃ_a n_ba名义状态里的b_g、b_a在预测阶段保持常量真实零偏在随机游走两者的差就被这些过程噪声“推动”。3.4 连续时间误差状态方程组汇总把上面所有结果集中写在一起δṗ δv δv̇ -R(a_m - b_a)^∧ δθ - R δb_a - R n_a δθ̇ -(ω_m - b_g)^∧ δθ - δb_g - n_g δḃ_g n_bg δḃ_a n_ba这是15维误差状态向量δx [δp, δv, δθ, δb_g, δb_a]^T 的连续时间模型。观察这个方程组会发现所有误差状态方程都是线性或近似线性的且系数矩阵只依赖名义状态和测量值。这正是ESKF的杀手锏非线性的剧烈运动全部由名义状态承载误差状态这个小家伙永远活在“近似线性”的舒适区里。4. 离散化与协方差传播从微分方程到预测步骤4.1 一阶欧拉离散化实际工程里IMU在离散时刻输出数据需要把连续方程离散化。以时间间隔Δt离散用最常用的一阶欧拉近似δp_{k1} δp_k δv_k Δtδv_{k1} δv_k [-R_k(a_m - b_a)^∧ δθ_k - R_k δb_a,k] Δt - R_k n_a Δtδθ_{k1} [I - (ω_m - b_g)^∧ Δt] δθ_k - δb_g,k Δt - n_g Δtδb_g,k1 δb_g,k n_bg Δtδb_a,k1 δb_a,k n_ba Δt注意这里 δθ 的传播其实还隐含一个更精细的旋转名义积分过程中的小旋转扰动。对大多数应用一阶近似足够。如果精度要求更高可以用精确的指数映射形式但雅可比会更复杂实际收益在IMU频率较高时非常有限。4.2 雅可比矩阵F_x的构造从离散方程组直接读出误差状态转移矩阵F_x [ I IΔt 0 0 0 ] [ 0 I -R(a_m-b_a)^∧Δt -RΔt 0 ] [ 0 0 I-(ω_m-b_g)^∧Δt -IΔt 0 ] [ 0 0 0 I 0 ] [ 0 0 0 0 I ]注意符号顺序状态向量是[δp, δv, δθ, δb_g, δb_a]所以第3行第4列对应δθ对δb_g的雅可比是 -IΔt第2行第4列是0第2行第5列是 -RΔt因为δb_a前的系数矩阵是 -R。很多博客或开源代码里这几个符号常出差错写代码前先自己在纸上推一遍比出bug后对着屏幕发呆高效得多。4.3 噪声雅可比与协方差更新误差状态方程里与噪声相关的项V [ 0 0 0 0 ] [ -RΔt 0 0 0 ] [ 0 -Δt 0 0 ] [ 0 0 Δt 0 ] [ 0 0 0 Δt]噪声向量 w [n_a, n_g, n_bg, n_ba]^T各自的协方差组成对角阵 W diag(σ_a²I, σ_g²I, σ_bg²I, σ_ba²I)。离散噪声协方差Q_d V W V^T预测步骤的协方差传播P_{k1} F_x P_k F_x^T Q_d这里有个工程说法的差异值得一提如果把噪声雅可比V里的- RΔt直接写在F_x的某个扩展维度里也可以写成增广矩阵形式但分离开的好处是你能清晰地看到哪些项来自测量噪声哪些项来自过程噪声调参时更有方向。IMU频率影响巨大——Δt越大Q_d增长越快滤波器对测量的信任程度越低表现就是估计轨迹更“笨重”。5. 观测更新与误差注入闭环的关键一环5.1 观测模型长航时IMU必然漂移靠观测把漂移拉回来。常见的观测有GPS位置、气压计高度、相机重投影误差。以最简单的GPS位置观测为例y p_t n p δp n在ESKF框架下观测方程是关于误差状态的线性方程z y - p δp n所以观测矩阵H针对误差状态为H [I3, 0, 0, 0, 0]这是ESKF的另一大便利观测模型天然是误差状态的线性函数不需要再额外线性化。对更复杂的观测比如视觉重投影同理会得到对应误差状态的雅可比原理一样。5.2 卡尔曼更新直接用标准卡尔曼更新公式S H P H^T R_obs K P H^T S^{-1} δx^ K z P^ (I - K H) P这里R_obs是观测噪声协方差。注意更新后我们得到的是误差状态的修正量δx^而不是直接的名义状态修正量。这一步对很多初学者来说很怪明明滤波器输出的K z是对误差状态的修正为什么名义状态不直接用 y 减名义状态原因在于我们把误差状态设计成卡尔曼滤波器的“待估计量”观测模型直接消费误差状态逻辑是闭环的残差z 观测 - 名义状态预测修正的是误差状态误差状态再传给名义状态。5.3 误差注入更新完成后把误差状态的估计合并回名义状态ESKF闭环的最后一步p ← p δp^ v ← v δv^ R ← R exp(δθ^) b_g ← b_g δb_g^ b_a ← b_a δb_a^然后把误差状态全部清零δp ← 0δv ← 0δθ ← 0δb_g ← 0δb_a ← 0。这正是“误差状态”这个设计的精髓每一帧都在纠正“名义状态与真实状态的偏差”然后清零重新让误差从零开始累积。有个细节要讲清楚协方差P在注入后并不清零。P是在估计误差状态的不确定性——清零误差状态均值不代表误差状态的不确定性归零。新增的不确定性观测更新也无法完全消除已经体现在P^里了。有些实现会在注入后对P做一次“重置修正”这是ESKF教程中经常提到的“误差重置”问题你需要在P中考虑误差坐标变换若不处理严格说会有微小的统计失真实际工程中很多人直接把P^当作新协方差用也跑得很稳。提示误差注入后必须清零误差状态均值但P保留。如果发现滤波器协方差持续被低估导致发散优先检查是否在注入后对P做了不合适的重置或者噪声矩阵是否给太小。6. 工程落地经验从推导到代码的六个细节6.1 左扰动还是右扰动选定一个就不要动我见过不止一次同一个项目里旋转误差一会儿用R_t R exp(δθ^∧)一会儿用R_t exp(δθ^∧) R结果协方差跑成负数。符号约定不是数学的对错问题是工程的脏问题。在代码文件顶部用注释写死本文件统一使用右扰动。写代码前把雅可比全部按这个约定推一遍。6.2 δθ的更新窍门小角度近似别乱用误差角度δθ经卡尔曼更新后可能是一个不超过几度的向量。exp(δθ^∧)可以直接用Rodrigues公式计算也可以用一阶近似I δθ^∧。如果系统只有几度的误差一阶近似够用但如果你发现注入后姿态偶尔抖动把exp换成完整指数映射通常能改善稳定性。计算量差异微不足道。6.3 初值噪声与可观测性ESKF对初始协方差P0不那么敏感但对过程噪声Q非常敏感。IMU噪声和零偏随机游走的物理参数可以从器件手册查但实测数据更可靠。一个常见做法是把IMU静止放置几分钟统计加速度计和陀螺仪的方差、零偏变化率用这些数据反推σ_a、σ_g、σ_bg、σ_ba。别用拍脑袋的“经验值”你的滤波器发散还是收敛很可能就卡在这些量级上。6.4 雅可比中零偏与旋转的耦合从第4节的方程组可以看到δθ与δb_g的耦合项是 -IΔtδv与δb_a的耦合项是 -RΔt。这两个耦合在大多数场景下都不能省略尤其当IMU零偏漂移明显时。省略它们会直接低估bias变化对位置姿态的影响滤波器的bias估计就会“反应迟钝”表现是加速度计零偏缓慢地滑向错误方向而位置误差却被观测强行压制。6.5 观测更新的时机与频率ESKF预测步骤通常是IMU频率比如200Hz观测更新是传感器频率比如GPS 10Hz相机30Hz。这两者之间的时间对齐非常重要——GPS或者视觉的时间戳如果和IMU时间戳不对齐误差最多引入几个厘米甚至几十厘米的偏差。做紧耦合系统时时间戳同步是比滤波算法本身更折磨人的工程问题。经验法则观测数据应该在时间上内插到IMU帧或者用最新的有效观测在预测间插入更新。6.6 数值稳定性P矩阵随着时间推移容易失去对称正定性。每步更新后做P (P P^T) / 2 强制对称多花几微秒换来的是长期运行的稳定。矩阵分解尽量用方差表示避免直接用协方差做求逆例如使用信息滤波形式或者Cholesky分解求解K。这个建议在长时间运行机器人或无人机时尤其重要。6.7 一个典型的ESKF预测-更新伪代码# 每一步IMU数据到来 def predict(p, v, R, bg, ba, P, a_m, w_m, dt, Q, g): # 名义状态积分 acc a_m - ba gyro w_m - bg p p v * dt 0.5 * R acc * dt * dt 0.5 * g * dt * dt v v R acc * dt g * dt R R exp_so3(gyro * dt) # 误差状态协方差传播 F build_F(R, acc, gyro, dt) V build_V(R, gyro, dt) Q_d V Q V.T P F P F.T Q_d return p, v, R, bg, ba, P # 每步观测到来 def correct(p, v, R, bg, ba, P, y, H, R_obs): z y - p # 假设位置观测 S H P H.T R_obs K P H.T inv(S) dx K z # 误差状态修正 # 注入名义状态 p dx[0:3] v dx[3:6] R R exp_so3(dx[6:9]) bg dx[9:12] ba dx[12:15] # 清零误差状态均值保留协方差 P (I - K H) P P 0.5 * (P P.T) return p, v, R, bg, ba, P, dx7. 常见问题与排查技巧实录7.1 滤波器发散估计轨迹飞掉排查顺序先看数据时间对齐再看Q是否给得太小最后看H方向是否错误。有一个窍门把观测残差z打出来如果z的均值和方差与R_obs的量级明显不匹配大概率是观测模型或时间戳问题而不是滤波器本身的问题。7.2 姿态估计准位置发散位置发散很多时候是加速度计零偏没估计准或者重力分离错误。检查世界系重力向量是否配置正确、b_a的随机游走噪声是否设置过小。ESKF里b_a和重力耦合得非常紧密如果初始加速度计零偏给得不对位置会以二次曲线的形式漂移。7.3 协方差越跑越小或越跑越大协方差单调减少要怀疑H、R_obs给得和实际不符或者P的传播中漏掉了Q_d项。协方差单调增大要怀疑观测更新太稀疏、或者观测模型没更新。每次更新后画一下P的迹大概看一眼趋势能快速定位问题。7.4 为什么用了ESKF还是慢收敛多数时候不是滤波器的问题是快速运动中的IMU饱和问题或观测外点outlier没剔除。观测更新前加一个简单的马氏距离门限把离谱的观测值丢掉比在滤波器内部做任何鲁棒改进都有效。7.5 左扰动和右扰动混用会怎样表现是姿态误差估计出现系统性偏差协方差和实际误差长期不一致。这种bug比较隐蔽因为系统有时仍能收敛。排查方法是从头到尾检查一遍R_t R exp(δθ^∧)这个定义在预测、注入、雅可比推导中是否始终一致。8. 一些额外的扩展思路ESKF不只用于IMUGPS组合导航。在视觉惯性里程计里相机位姿参数化与IMU状态估计经常分别处理IMU部分用ESKF维持高频位姿推算视觉部分解算出带协方差的位姿观测再送入ESKF的观测更新。这种组合结构清晰、扩展性好也是很多开源VIO框架的实际做法。如果你想进一步深入可以研究ESKF与信息滤波器、ESKF与固定滞后平滑器、ESKF在SE(3)上的推广形式。它们的核心逻辑都和本文完全一样只是状态空间从SO(3)拓展到SE(3)或SE(2)误差状态维度变大公式形式更紧凑但直觉仍然是“名义状态走流形误差状态走向量空间”。我个人在实际操作中的体会是ESKF的推导并不难难的是一直保持符号一致和单位一致。每一行公式上一个人用的变量名可能跟你不一样单位可能不动声色地变过直接抄代码几乎必踩坑。静下心来亲手推一遍、手写一遍状态转移矩阵再对照开源代码逐项看比看十篇博客都有用。如果真心想把这套东西用在自己的机器人或者无人机上建议从最经典的两个组合开始练手位置观测模拟GPS加IMU以及单目相机位姿观测加IMU。前者让你熟悉预测-更新-注入的闭环后者会让你真正体会到ESKF在姿态观测融合中的优雅之处。