做机器人定位和SLAM的同行迟早都会撞上ESKF这堵墙。IMU能给出高频的角速度和加速度但积分就飘摄像头和激光雷达能给出低频但绝对值可靠的观测又好死不死需要和IMU做紧耦合。把这两类数据揉到一起最成熟、最标准的思路之一就是ESKF也就是Error-State Kalman Filter。网上关于ESKF的代码不少但真正愿意把“四元数运动学怎么推、误差状态方程每一项怎么来”讲透的教程真心不多大部分人抄完公式遇到yaw慢漂、bias不收敛就直接卡住。这篇文章我想拉着你从头过一遍ESKF的核心推导四元数运动学、误差状态传播、观测更新和重置每一步都写清楚“为什么是这个形式”再附上工程里的实操经验和踩坑记录。适合正在做VIO、LIO、组合导航或者刚入门想搞懂IMU定位原理的朋友读过之后至少能看懂主流开源框架在干什么而不是只会调参。1. 为什么是误差状态而不是直接滤全量状态1.1 IMU测量模型和坐标约定先建立共同的底子。IMU输出的所谓“加速度”其实不是刚体在世界系下的加速度而是比力是加速度计感受到的、扣除重力部分之后的本体系力。工程上最常用的测量模型写出来是a_m R^T (a_w - g) b_a n_a陀螺仪则更直接ω_m ω b_g n_g其中R是把body系转到world系的旋转矩阵a_w是world系下的真实加速度g是重力向量b_a和b_g是加速度计和陀螺仪的零偏n_a和n_g是测量白噪声。这里坐标系约定很重要我默认使用ENU世界系那么g就是[0, 0, -9.8]^T如果你用NED就得反过来。很多推导对不上号、程序里跑出来的轨迹反着飘八成是重力方向这个符号弄反了。注意一个容易混淆的点IMU不能直接测姿态也不能直接测位移。你所谓“水平放置时加速度计读数约等于重力反方向”本质上是比力模型里R^T(-g)这一项在起作用。理解了这个模型后面所有ESKF公式才能看顺。1.2 全量状态的非线性问题和误差状态的“降维打击”如果直接把IMU原始状态拿去写卡尔曼滤波会遇到几个很麻烦的问题。第一个是旋转状态是四元数四元数有单位约束直接当成向量加噪声、求均值结果会破坏单位性还得反复归一化。第二个是系统强烈非线性EKF的线性化点离真实状态稍微远一点一阶泰勒展开就失效扰动一大协方差估计就失真。ESKF的思路是“曲线救国”状态分成两部分一个叫标称状态nominal state一个叫误差状态error state。标称状态用IMU原始数据做高频积分相当于一个全量、无约束、带bias补偿的kinematic积分器误差状态才是滤波器真正估计和更新的对象。因为误差通常很小旋转部分可以放心用三维旋转向量表示没有万向锁没有单位约束线性化精度极高。最后把估计出的误差合并回标称状态误差清空进入下一轮。这种区分本质上是把一个长时间、大范围的非线性问题拆成了高频的非线性标称积分和低频的、几乎线性可加的误差估计。工程上ESKF这个方案能稳定工作极度依赖一个前提bias估计和姿态估计不能差太远。误差状态线性化只在真值落在标称状态附近时才靠谱。所以第一次初始化时用静止数据做重力对齐、估bias初值不是“锦上添花”而是“必须做的第一步”。2. 四元数运动学旋转变化到底怎么随角速度走2.1 四元数基本运算法则我们要推误差状态方程逃不掉四元数。这里统一用Hamilton约定标量在前形如q [w, x, y, z]纯向量写成实部为0的四元数[0, v]。两个四元数的乘法定义是q1 ⊗ q2 [s1 s2 - v1·v2, s1 v2 s2 v1 v1 × v2]单位四元数的共轭等于逆q* [s, -v]旋转向量v用四元数旋转表示为q ⊗ [0, v] ⊗ q*。从旋转向量θ到四元数的指数映射写为Exp(θ) [cos(||θ||/2), sin(||θ||/2) θ/||θ||]小角度下就有关键近似Exp(δθ) ≈ [1, 0.5 δθ]^T这个近似是整个误差状态四元数处理的地基后面所有δθ的出现都从这里来。建议想透彻理解ESKF的人先把四元数乘法和这个指数映射练成条件反射再往下看。2.2 从旋转向量极限到运动学方程 q̇ 0.5 q ⊗ ω四元数运动学方程可以直接从旋转的定义出发推。设t时刻姿态为q(t)在很小的Δt内body系里有一个角速度ω相当于在原有的旋转基础上再局部旋转一个增量。旋转向量形式下这个增量是ωΔt对应四元数增量Exp(ωΔt)并且这个增量是从body本身系下叠加上去的所以在四元数层面应该右乘q(tΔt) ≈ q(t) ⊗ Exp(ωΔt) ≈ q(t) ⊗ [1, 0.5 ωΔt]^T展开并减去q(t)除以Δt取极限q̇ 0.5 q ⊗ [0, ω]^T写成四元数乘法矩阵形式就是q̇ 0.5 Ω(ω) q其中Ω(ω)是左乘角速度四元数对应的4×4矩阵。这里要特别注意“右乘”这个操作的含义它对应的是body系本体系下的旋转增量。如果搞成左乘那是世界系下施加增量推导结果会差一个符号后续误差方程也会全乱。工程中做四元数积分时我非常建议直接用离散旋转四元数Exp(ωΔt)去乘而不是用连续微分方程再去数值积分。这样做能天然保持四元数模长为1漂移小代码也简单。真的非要数值积分每一步之后记得归一化不然后面误差会越积越大定位结果直接不能看。3. 误差状态运动学从误差四元数出发一步步推导3.1 误差状态的维度与定义ESKF的状态一般取五块15维标称状态x [p, v, q, b_a, b_g] 误差状态δx [δp, δv, δθ, δb_a, δb_g]其中姿态误差用局部扰动定义q q̂ ⊗ δqδq ≈ [1, 0.5 δθ]^T之所以选局部扰动而不是全局扰动是因为IMU积分过程里角速度本身就是定义在body系的局部扰动和运动学方程天然匹配而且误差姿态在标称姿态附近的协方差解释也最干净。位置和速度的误差就是简单相加p p̂ δpv v̂ δvbias误差也是相加b_a b̂_a δb_ab_g b̂_g δb_g。3.2 姿态误差方程的核心推导别看最后公式很短中间全是对消这是全篇含金量最高的一个推导值得盯紧公式算一遍。真实姿态和标称姿态都满足四元数运动学q̇ 0.5 q ⊗ ωq̂̇ 0.5 q̂ ⊗ ω̂因为q q̂ ⊗ δq求导展开q̇ q̂̇ ⊗ δq q̂ ⊗ δq̇代入两个运动学方程0.5 q̂ ⊗ δq ⊗ ω 0.5 q̂ ⊗ ω̂ ⊗ δq q̂ ⊗ δq̇左乘q̂的逆再把δq̇单独提出来δq̇ 0.5 (δq ⊗ ω - ω̂ ⊗ δq)现在把δq ≈ [1, 0.5 δθ]^T代进去利用四元数乘法公式展开。这里你会看到标量部分全是二阶小量可以丢掉向量部分经过两个叉乘项合并最后得到δθ̇ -[ω̂]× δθ δω其中δω ω - ω̂。这个结果抵消得很漂亮如果误差姿态为零即使角速度误差存在它也会直接以积分形式注入δθ而标称角速度ω̂对误差姿态的作用是一个叉乘矩阵相当于把误差姿态带着转。正是因为叉乘矩阵项存在误差姿态方程才是旋转耦合的忽略它会让你估计出来的姿态误差协方差完全失真。实际中角速度误差的来源要写全。按本章开头的测量模型真实角速度ω ω_m - b_g - n_g而标称角速度ω̂ ω_m - b̂_g于是δω -δb_g - n_g代入后就得到工程实现里最终用的姿态误差方程δθ̇ -[ω̂]× δθ - δb_g - n_g3.3 速度、位置和bias的误差方程速度误差推导同样是“真值减标称”的思路。真实速度满足v̇ R f g其中f是本体系比力真值g是世界系重力。把R R̂(I [δθ]×)和f f̂ - δb_a - n_a代入展开后只保留一阶项δv̇ -R̂ [f̂]× δθ - R̂ δb_a - R̂ n_a这里有一个容易看晕的符号-R̂ [f̂]× δθ这一项是因为旋转误差改变了比力在世界系下的投影方向。想象一下姿态偏了哪怕一度加速度在世界系下投影出来的方向就偏了长期积分就是厘米级甚至米级的位置误差。后面那项-R̂ δb_a则是加速度计零偏误差直接通过旋转矩阵投影到位移空间这也是为什么bias估计不准几乎立刻反映为轨迹漂移。位置误差最简单δṗ δvbias本身工程上常用随机游走建模即δḃ_a n_baδḃ_g n_bg也就是说在预测阶段bias误差的均值保持不变但它的不确定性随时间线性增长。这句话翻译成代码行为就是滤波跑的越久、没有观测更新时bias的方差越大观测一来它对bias的修正幅度就越猛。3.4 连续时间误差状态方程汇总把上面几个式子拼成完整矩阵形式就是ESKF误差状态运动学的连续时间模型δẋ F_c δx G_c w其中w [n_g, n_a, n_ba, n_bg]^T是噪声向量F_c的具体分块是F_c [ -[ω̂]× 0 0 -I 0 ] [ -R̂[f̂]× 0 0 -R̂ 0 ] [ 0 I 0 0 0 ] [ 0 0 0 0 0 ] [ 0 0 0 0 0 ]噪声耦合矩阵G_c把四组噪声分别映射到对应方程由于协方差传播中负号会被平方消去G_c具体写成单位阵还是负单位阵不影响最终协方差但公式里要保持符号一致。看到这个15维矩阵不用怕后面离散化只是一阶近似代码里实际用到的也就是每一小块3×3矩阵的加减乘。4. 离散化从连续微分方程到能写代码的预测步4.1 标称状态的离散积分预测步对标称状态直接用IMU测量做数值积分。最简单又足够用的是一阶欧拉实际工程中建议用中值积分或者Runge-Kutta尤其角速度变化很快的场景欧拉积分会让姿态误差显著增大。一阶欧拉形式如下p̂_{k1} p̂_k v̂_k Δt 0.5 (R̂_k f̂_k g) Δt^2 v̂_{k1} v̂_k (R̂_k f̂_k g) Δt R̂_{k1} R̂_k Exp(ω̂_k Δt) b̂_{a,k1} b̂_{a,k} b̂_{g,k1} b̂_{g,k}这里f̂_k和ω̂_k是经过bias估计补偿后的比力和角速度。旋转更新一定要用四元数指数映射乘法然后归一化不要直接q 0.5 q ⊗ ω Δt后者每步都会引入模长漂移长时间跑下来姿态基准会越来越歪。另外一个实际经验如果IMU频率是200Hz到1000Hz预测步每次只推一个Δt就够了但如果你的状态估计频率要和相机帧率对齐可能需要对多个IMU样本做积分下采样这时候别在ESKF外部把IMU平均成低频而是把中间每次IMU测量都走一遍标称传播只在相机观测时刻做滤波更新。平均IMU看起来省计算实际上会丢信息bias可观测性会变差。4.2 误差状态协方差传播矩阵离散误差状态转移矩阵取一阶近似F_x I F_c Δt其中F_c就是上一节那个15×15矩阵。把它展开就是F_x [ I - [ω̂]×Δt 0 0 -IΔt 0 ] [ -R̂[f̂]×Δt I 0 -R̂Δt 0 ] [ 0 IΔt I 0 0 ] [ 0 0 0 I 0 ] [ 0 0 0 0 I ]这个矩阵的意义是误差状态经过Δt后在各个维度上如何相互耦合、自身如何演变。注意第1行第4列-IΔt表示陀螺仪bias误差在Δt内直接累计成姿态误差第2行第4列-R̂Δt表示加速度计bias误差在Δt内直接累计成速度误差。这两个耦合项就是为什么ESKF能在紧耦合中主动估计bias的根本原因——观测更新里位置/速度残差会顺着这两个通道去修正bias。噪声协方差离散化也需要近似。连续噪声谱密度Q_c在不同IMU里差异很大没有一键统一答案。工程上先按传感器手册给的白噪声密度和随机游走密度填初值再用Allan方差或标定工具校准。离散化一步近似为Q_d ≈ G_c Q_c G_c^T Δt很多初学ESKF的人都会卡在这个“离散化”上我到底要不要精确算矩阵指数我的建议是对Q_d和F_x都先用一阶近似跑通整个系统后再精细。一阶近似引入的误差远小于你IMU噪声参数没标准引入的误差。先用简单的把滤波逻辑调到自洽再去抠精度。4.3 初始化与重力对齐ESKF能不能收敛开局就定了ESKF需要有一个大致准确的初始姿态和bias初值否则误差状态线性化前提不成立。最常用的办法是静止初始化把IMU平放或任意静止放置几秒取加速度计平均值方向来推初始roll和pitch因为静止时加速度计读到的就是重力反方向。具体做法是设平均加速度为ā取重力参考为g [0, 0, -9.8]^TENU系。用ā和g做叉乘可以求旋转轴用点乘求夹角就能构建初始四元数把body系的“重力方向”旋转到world系的“负z方向”。这一步做完roll和pitch就对齐了。yaw没有绝对参考设成0即可后续靠视觉、激光或磁力计去校正。加速度计bias初值通常直接取静止时加速度计读数模长减去9.8后的一部分残余更稳的做法是把陀螺仪bias取静止时角速度平均值加速度计bias先设0由滤波器后台慢慢估计。这里有个很常见的问题如果初始姿态算错了2度ESKF的确也能勉强跑但yaw会漂得更快而且bias会往错误方向收敛去补偿姿态误差。你最后看到“状态不发散但轨迹歪了”很多时候不是滤波器的锅是初始重力对齐那一步就没做对。5. 观测更新与重置闭环修正和误差归零5.1 观测模型与H矩阵怎么搭ESKF的观测来源可以是GPS位置、轮速、视觉重投影残差、激光点云配准残差等。关键不是观测形式而是把观测残差线性映射到误差状态。以最简单的3D位置观测为例z p v_pos残差r z - p̂由于p p̂ δp残差在误差状态下的期望就是δp因此量测雅可比直接是H [0, 0, I, 0, 0]这简直不要太清爽全量状态EKF里H至少要和一堆四元数块纠缠。但对于姿态观测就要小心。假设我们观测到姿态q_m用残差r q_m ⊗ q̂*取对数后的向量部分r ≈ 2 * vec(q_m ⊗ q̂*)这个残差在误差状态的雅可比不是简单的一个单位阵而是和δq的定义方向相关。工程上最稳妥的做法是在代码里定义好“真实四元数 标称四元数 ⊗ 误差四元数”这一约定后统一写成q_m q̂ ⊗ Exp(δθ) ⊗ q_noise r log(q_m ⊗ q̂*) ≈ δθ对前面那个2倍关系取决于你采取的旋转向量定义很多框架直接用2 * vec(...)来减小线性化误差。这里不展开太多矩阵给出结论如果你观测的是绝对姿态H对应δθ那三列取单位阵即可但前提是残差计算方向和误差四元数定义方向一致。方向差一个符号你最直观的表现就是更新之后姿态不是收敛而是震荡。5.2 更新、注入与重置更新方程就是标准卡尔曼滤波形式S H P H^T R K P H^T S^{-1} δx K r得到误差状态后把它注入标称状态p̂ ← p̂ δp v̂ ← v̂ δv q̂ ← q̂ ⊗ Exp(δθ) b̂_a ← b̂_a δb_a b̂_g ← b̂_g δb_g注入后必须做四元数归一化。然后最重要的“重置”来了误差状态清空为0但协方差不能直接保留原样而是要经过一个线性变换δx_new 0P ← G P G^T因为真实状态没变你把误差融进了标称状态相当于坐标原点挪了位置误差状态的协方差自然要跟着变换。G矩阵在误差很小的时候姿态子块近似为G_rot ≈ I - [0.5 δθ]×其余子块取单位阵实际代码里如果每次更新的δθ非常小很多人直接把G当单位阵用在大多数场景下确实不影响大局。但在bias快速变化或者观测频率很低、δθ偏大的时候忽略G会让P的估计偏高下一轮增益就不那么自信。写代码时别偷懒这个矩阵只有6×6的内容算起来很快。6. 工程实战避坑、标定与调试经验6.1 新手最容易掉的坑yaw慢漂和bias不收敛“基于IMU的位姿解算yaw仍会慢漂”是几乎每个人都会遇到的高频问题。这里要说清楚一个底层事实在只有IMU、没有绝对航向观测的系统里yaw是弱不可观测的慢漂是正常的不是bug。ESKF只能通过加速度计和运动加速度的关系间接估计部分姿态和bias但绝对航向没有信息来源只能靠视觉、激光、磁力计或轮速等外部传感器注入。如果你的系统里明明有视觉或激光yaw还漂那先检查三件事。第一外参标定对不对尤其是IMU和相机/雷达之间的旋转外参外参错了视觉和IMU给出的航向增量互相矛盾滤波器会顶着干yaw自然漂。第二IMU角速度bias初值估得准不准静止初始化时一定要取足够长的时间平均最好imu热身几秒再采集因为很多MEMS陀螺仪bias在上电初期会明显漂移。第三过程噪声里的陀螺仪随机游走给得太小导致滤波器过于相信IMU积分yaw的方差缩得太紧更新很难把它拉回来。排查思路我一般是这样先砍掉所有观测只跑标称积分看yaw能维持多久不严重漂移大致摸清IMU本身质量再接上观测看更新残差的符号和幅值是否合理最后再调噪声参数。别上来就乱调Q和R那是自欺欺人。6.2 IMU标定和内外参标定别省这一步ESKF对IMU内参的敏感度非常高。加速度计和陀螺仪的尺度因子、轴间非正交角、bias这堆内参如果不校准误差状态模型里的“噪声”只是吸收了所有未建模项而不是真正的随机噪声滤波结果会次优甚至发散。常用做法是用转台做六位置法标定加速度计用高精度转台或角速度参考标定陀螺仪手头没有转台就用多位置静止法至少把加速度计尺度因子和bias粗标一遍。外参标定同样绕不开。lidar和IMU之间、相机和IMU之间的旋转和平移外参本质上是传感器坐标系的刚体变换。以相机IMU联合标定为例Kalibr这类工具的核心思想是相机给出的连续帧运动轨迹和IMU积分出的运动轨迹在正确外参下应该一致于是把外参作为待估计量放进一个非线性优化问题里求最优旋转平移。实际标定时建议让IMU充分激励六个自由度都动一动旋转和平移分开来运动不要只平移或只旋转。如果你恰好用车辆动力学软件做仿真比如在CarSim里设置IMU传感器也一样要让其在标定工况下充分运动多设计大角度转向和加减速工况否则外参标定结果会非常病态。6.3 参数初始化和调试节奏的心得ESKF噪声参数就那么几组陀螺仪白噪声、加速度计白噪声、陀螺仪bias随机游走、加速度计bias随机游走、初始协方差P和观测噪声R。初次调参时我建议按这个顺序先设传感器的白噪声与加速度计/陀螺仪数据手册一致bias随机游走先给一个稍大的值让滤波器在前期比较“敢”去修bias然后固定Q调R让位置/姿态残差大致和传感器精度匹配最后回过头微调bias随机游走观察bias估计是否稳定、不发抖。还有一个特别好用的调试习惯对滤波器做“闭环注入测试”。在仿真里给IMU数据加一个已知的bias阶跃比如第10秒让陀螺仪bias突然跳变0.1 rad/s然后观察ESKF能不能在几秒内把这个阶跃估计出来。如果估计得很慢、甚至震荡说明bias随机游走Q给小了或观测更新频率太低如果估计出来了但速度误差瞬间冲高说明Q给大了滤波器修bias太激进。这个测试能帮你把参数整定从玄学变成工程学。刚开始写ESKF代码时最容易忽略的是crazy sign convention测量模型里加速度计bias的符号、误差四元数用左扰动还是右扰动、残差方向是z-h还是h-z任一环节差个负号算法表现会是天差地别。解决这类问题的唯一可靠办法就是亲手从连续时间模型推到离散方程哪怕只推一次之后再碰到符号问题也能用逻辑判断而不是靠试。我个人手推一遍之后再去看MSCKF、VINS-Mono这些框架里的代码很多原来觉得“莫名其妙”的负号和系数瞬间就通了。这个功夫值得花。