干SLAM或者机器人定位的兄弟应该都有这种体会手里拿着一个IMU三轴陀螺仪加三轴加速度计数据刷得飞快但真指望它单独算出位置来几秒钟就漂到天上去了。这玩意儿短时间局部特性极好长时间绝对不可靠于是就有了融合方案。而ESKFError-State Kalman Filter误差状态卡尔曼滤波就是目前工程里用得最多、也最稳的一套框架把IMU的原始数据揉进状态估计里配合相机、激光雷达或者GPS做修正最终输出一个相对可靠的位姿。网上讲ESKF的资料不少但大多是直接甩结论——给你一个状态转移矩阵F、一个观测矩阵H告诉你照着填就行。真正让你自己动手从四元数运动学开始推一遍误差状态方程的少之又少。我当年啃这块的时候连续一周在坐标系符号上翻车把左乘右乘搞混推出来的雅可比符号反了滤波器直接发散。这篇文章就把我当时踩过的坑全部摊开从IMU测量模型讲起一步步把四元数运动学、真实状态/标称状态/误差状态的定义、误差状态运动学方程的每一项推导、离散化、观测更新、状态注入和协方差重置全部写清楚。适合正在做VIO、LIO、组合导航或者单纯想搞懂“ESKF为什么长这样”的朋友最好有一点线性代数和卡尔曼滤波的基础没有的话我把涉及的基础概念也会顺带解释一下。1. IMU测量的数学本质与坐标系约定1.1 陀螺仪和加速度计到底在测什么IMU的原始输出看上去很简单陀螺仪给三个轴的角速度加速度计给三个轴的比力。但很多初学者没搞清楚一个关键点——加速度计测的不是加速度是“比力”specific force也就是单位质量上受到的惯性力减去重力加速度之后的量。具体来说加速度计输出的测量值 a_m 可以写成a_m a b_a n_a这里的 a 是载体真正的运动加速度在机体坐标系下的表达扣除重力后b_a 是加速度计biasn_a 是测量白噪声。这还没完如果你把牛顿第二定律写完整真实情况下载体在世界系下的加速度 a_W 和比力之间有个非常关键的重力耦合关系a_W R * (a_m - b_a - n_a) g其中 R 是机体坐标系到世界坐标系的旋转矩阵g 是世界系下的重力向量大小约9.81方向竖直向下或向上取决于你的坐标系约定。也就是说加速度计测量的是“抵抗重力之外的比力”我们拿到原始数据后第一件事就是要考虑如何从里面分离出真实运动加速度而分离的方法依赖于当前的姿态估计。这就是为什么IMU数据和姿态是耦合的——姿态错了重力投影错了加速度积分出来的位置全是错的。陀螺仪那边相对简单测量的是机体相对惯性系的角速度在机体系下的投影同样带bias和白噪声ω_m ω b_g n_g这里 ω 是真实角速度b_g 是陀螺仪biasn_g 是白噪声。陀螺仪最麻烦的不是白噪声而是bias的缓慢变化——bias不是常数它本身也在随机游走所以后面要在ESKF里给bias单独建模估计它的变化这是整套滤波器能不能长时间稳定的关键之一。1.2 坐标系与符号约定推ESKF的公式时符号不统一是最大的灾难。我先把我用的这套约定定死后面所有推导都基于此世界坐标系 W固定在地面附近重力 g 在这个系下是常向量。如果你用ENU东北天约定重力就是 (0, 0, -9.81)用NED北东地重力是 (0, 0, 9.81)。无所谓只要全程序统一就行我习惯用ENU因为ROS里默认的就是这个。机体坐标系 B固定载体的坐标系IMU的三轴就定义在B系里。R 表示从B系到W系的旋转矩阵也就是 p_W R * p_B。姿态四元数 q 表示同一个旋转满足 p_W q ⊗ p_B ⊗ q*。注意我这里用的是Hamilton四元数约定虚部在后乘法是 q1 ⊗ q2 [q1_wq2_w - q1_v·q2_v, q1_wq2_v q2_w*q1_v q1_v×q2_v]。另外所有误差状态矩阵的推导都假设误差是小量所以所有二阶及以上的小量乘积全部被丢掉。这是ESKF之所以能work的核心——误差状态的线性化只在小误差的前提下成立一旦误差变大线性化误差就会把滤波器带歪这也是为什么ESKF需要相对可靠的观测来定期修正不能长时间纯靠IMU预测。1.3 为什么裸IMU数据算不动定位我见过不少新手拿到IMU的第一反应是按照课本那样直接积分陀螺仪积分得到姿态姿态把加速度转到世界系减去重力后积分两次得到位置。结果就是姿态在小范围晃两下还能看稍微转几个大角度或者持续跑个半分钟位置就发散到无法直视。这里面除了白噪声积分后会随机游走这个众所周知的原因外还有两个容易被忽略的杀手第一个是bias导致的二次项发散。陀螺仪的bias虽然很小但姿态误差会随时间线性增长而姿态误差又会导致重力向量投影错了方向这个错误的重力分量被当作真实加速度积分于是位置误差随时间二次方增长速度误差线性增长。这就是纯IMU积分“几秒可看、几十秒报废”的根本原因——不是噪声惹的祸是bias通过姿态误差间接放大了。第二个是初始对准问题。姿态的初始值必须从加速度计和外部信息来估计因为你光拿一个静止的加速度计只能知道重力方向从而对齐pitch和roll但yaw是完全不可观的。没有一个好的初始姿态积分一开始就在错误的方向上累积误差。所以ESKF的思路就来了我不追求直接用IMU积分出一个绝对精准的位姿而是把IMU当作一个高频率的短期预测器用绝对观测GPS、视觉、激光雷达来做长期修正。同时在模型里显式地把bias作为状态量估计把bias的估计误差、姿态误差、速度误差、位置误差打包成一个高维误差向量用卡尔曼滤波来估计这个误差的均值和协方差然后不断回馈修正标称状态。这样既保留了IMU高频特性又杜绝了误差的无限累积。2. 四元数运动学推导与离散化2.1 四元数基础在进入ESKF之前四元数运动学这块必须彻底搞明白因为后面所有误差状态方程都围绕它展开。四元数里有几个公式我会反复用到单位四元数q [w, x, y, z]满足 w² x² y² z² 1。共轭四元数q* [w, -x, -y, -z]表示逆旋转。旋转向量到四元数给定角度-轴表示 θ θ*u对应的四元数 q [cos(θ/2), sin(θ/2)*u]。小角度近似当 θ 很小时q [cos(θ/2), sin(θ/2)*u] ≈ [1, (1/2)θu] [1, (1/2)*θ]。这是四元数误差状态推导中最核心的武器任何小角度的姿态误差都可以用一个三维向量 δθ 表示误差四元数 δq ≈ [1, (1/2)*δθ]。四元数与旋转矩阵的换算关系这个每个开源库都有现成函数但你自己要会查会验特别是当你要从旋转矩阵反推四元数的时候符号容易出问题。还有一个非常重要的约定姿态误差是用左乘还是右乘。在ESKF里我定义真实姿态 q_t 等于标称姿态 q 右乘一个微小误差四元数 δqq_t q ⊗ δq这意味着误差是在机体局部坐标系下定义的相当于全局坐标系的旋转先转q再在机体坐标系里转δq。为什么用右乘而不用左乘因为在惯性导航里误差状态定义在局部坐标系下时误差状态的协方差几何结构更接近流形的切空间线性化精度更好而且观测更新时增量修正的姿态误差更新公式也更简单。如果你在某个资料里看到 q_t δq ⊗ q 的写法那是全局误差定义推导结果会差一个旋转矩阵的相似变换千万别混用。2.2 连续时间运动学方程四元数运动学的连续时间形式是最基本的姿态递推公式q̇(t) 0.5 * q(t) ⊗ ω(t)这里的 ω(t) 是角速度向量但在四元数乘法里要写成纯四元数 [0, ωx, ωy, ωz] 的形式。怎么理解这个公式你可以想象成姿态四元数的变化率等于当前姿态绕瞬时角速度方向的旋转。这个公式不是凭空蹦出来的它的推导起点是旋转向量的微分在 t 时刻我们保持角速度 ω 不变经过一个无穷小时间 dt姿态从 q(t) 变成了 q(t dt) q(t) ⊗ exp(0.5 * ω * dt)把 exp 做一阶泰勒展开 exp(x) ≈ 1 x就得到 q(t dt) ≈ q(t) ⊗ [1, 0.5ωdt]于是q̇(t) (q(tdt) - q(t)) / dt q(t) ⊗ [0, 0.5*ω] 0.5 * q(t) ⊗ ω在实际应用里因为IMU给的是离散采样我们更常用的是离散递推式q_{k1} q_k ⊗ Exp(ω * Δt)其中 Exp(θ) 是对旋转向量 θ 的指数映射等价于先求角度 θ ||θ|| 和单位轴 u θ/||θ||再构造四元数 [cos(θ/2), sin(θ/2)*u]。如果角速度乘以采样周期 Δt 足够小也就是每秒转的角度不大可以用一阶近似q_{k1} ≈ q_k ⊗ [1, 0.5 * ω * Δt]2.3 离散化与代码实现在实际工程里一阶近似有个上限当角速度在100Hz采样间隔内的角度增量超过大约10度时一阶近似的误差就开始变得不可忽略。不过对大多数机器人和车辆场景IMU采样率在100到400Hz正常运动下每帧角度增量不到2度一阶近似完全够用。如果你要做高动态的飞行器或者高转速机械臂就必须用完整的指数映射也就是先把角速度乘以增量时间得到旋转向量再用罗德里格斯公式或者四元数的exp函数转成四元数避免归一化带来的误差累积。我自己在代码里实现的时候习惯写两个函数一个是一阶近似版本用于快速原型一个是精确版本用于最终部署。伪代码如下// 一阶近似版本适合增量角10度 Eigen::Quaterniond quatIntegrationFirstOrder( const Eigen::Quaterniond q, const Eigen::Vector3d omega, double dt) { Eigen::Quaterniond dq; dq.w() 1.0; dq.vec() 0.5 * omega * dt; // 归一化防止数值漂移 dq.normalize(); return (q * dq).normalized(); } // 精确版本适合大角速度场景 Eigen::Quaterniond quatIntegrationExact( const Eigen::Quaterniond q, const Eigen::Vector3d omega, double dt) { Eigen::Vector3d theta omega * dt; double angle theta.norm(); if (angle 1e-8) { return q; } const Eigen::Vector3d axis theta / angle; Eigen::Quaterniond dq( std::cos(angle * 0.5), std::sin(angle * 0.5) * axis.x(), std::sin(angle * 0.5) * axis.y(), std::sin(angle * 0.5) * axis.z()); return (q * dq).normalized(); }这里有一个工程上面很容易被忽略的坑四元数经过长序列乘法之后因为浮点误差会慢慢偏离单位模长所以每次乘法之后都要 normalize 一下否则姿态会缓慢“收缩”最终导致旋转矩阵不正交后面的所有计算全部污染。3. ESKF误差状态方程推导3.1 三种状态的定义ESKF的核心思想是把状态量拆成三份这份“拆”是整个滤波器设计的精髓真实状态 x_t系统真正的状态包含真实的位置、速度、姿态、bias等。我们永远无法直接拿到它它带有一切噪声和扰动。标称状态 x不考虑过程噪声、忽略测量噪声情况下由IMU积分得到的“无噪声轨迹”。它是我们最终输出的结果是滤波器的“主人”。误差状态 δx真实状态和标称状态之间的偏差。这是卡尔曼滤波真正要估计的对象——我们不是在估计绝对状态而是在估计“我当前标称状态离真实状态偏了多少”。这种拆分带来的好处非常明显。第一误差状态是小量其运动学方程可以安全地线性化卡尔曼滤波要求的高斯假设在误差状态下远比在原始状态下成立得好——想想看位置和姿态动辄几十上百的量级哪里像高斯分布但误差状态永远是接近于零的小量线性化误差被压到极小。第二误差状态的维度通常比全状态低比如姿态误差用3维向量而不是4维四元数计算效率更高。第三也是最重要的一点误差状态的协方差矩阵描述的是“标称状态周围的确定性”这比直接对状态本身做协方差估计要自然得多尤其对于四元数这种流形上的量直接加一个高斯噪声往往会让它脱离流形破坏了单位模长约束但把误差定义成流形上的小量就能完美规避这个问题。具体的状态定义如下真实位置 p_t p δp 真实速度 v_t v δv 真实姿态 q_t q ⊗ δq其中 δq ≈ [1, 0.5*δθ] 真实陀螺bias b_g,t b_g δb_g 真实加计bias b_a,t b_a δb_a 真实重力 g_t g δg这里 p, v, q, b_g, b_a, g 是标称状态没有噪声时的积分结果δp, δv, δθ, δb_g, δb_a, δg 是误差状态。之所以把重力也拉进来当状态是因为实际系统里重力的模长不是精确知道的、初始对准也可能存在微小偏差把这些不确定性吸收到误差状态里让滤波器自己去估计是提高稳定性的常用做法。3.2 误差状态运动学逐项推导现在进入全篇的核心环节——推导误差状态的运动学方程。目标是得到形如δẋ F * δx G * n的线性方程其中 n 是过程噪声向量。位置误差 δp这个最简单直接对定义求导即可δṗ p_ṫ - ṗ v_t - v δv没有任何非线性项干净利落。速度误差 δv这一项是整个推导里最繁琐的也是最多人出错的地方。先写真实速度的导数v_ṫ R_t * (a_m - b_a,t - n_a) g_t标称速度的导数v̇ R * (a_m - b_a) g两者相减并把 R_t R * Exp(δθ) ≈ R * (I [δθ]×) 代入展开后丢掉所有二阶小量得到δv̇ -R * [a_m - b_a]× * δθ - R * δb_a δg - R * n_a这里 [a]× 是向量 a 的反对称矩阵。这个式子的物理意义非常清晰速度误差的来源有三个——加速度方向投影错了由姿态误差导致、加速度计bias估计错了、重力估计错了。其中第一项里的十字架反对称阵表示叉积它的来由是旋转矩阵对δθ的雅可比R(I[δθ]×)作用在向量上多出来的 [δθ]× a 就是 a 被姿态误差旋转后产生的偏差。特别注意我在推导速度误差时加速度项用的是 a_m - b_a也就是用标称bias对测量值做了补偿。这意味着标称状态方程里积分的时候必须用补偿后的加速度否则这里的叉积项就不对。姿态误差 δθ姿态误差的推导最绕我这里用一个小技巧与其硬算四元数的导数不如直接用角速度的叠加性质。想象一下真实姿态的旋转顺序是先转标称姿态 q再转一个小的误差姿态 δq。从世界系看真实角速度 ω_t 应该等于标称角速度 ω_nom 加上误差姿态的变化率在全局系下的表达。但更方便的做法是直接对 q_t q ⊗ δq 求导。先把两个运动学写出来q_ṫ 0.5 * q_t ⊗ (ω_m - b_g,t - n_g) 0.5 * q ⊗ δq ⊗ (ω_m - b_g - δb_g - n_g) q̇ 0.5 * q ⊗ (ω_m - b_g)对 q_t q ⊗ δq 两边求导用链式法则q_ṫ q̇ ⊗ δq q ⊗ δq̇把 q̇ 代入q_ṫ 0.5 * q ⊗ (ω_m - b_g) ⊗ δq q ⊗ δq̇把前面 q_ṫ 的表达式也替换0.5 * q ⊗ δq ⊗ (ω_m - b_g - δb_g - n_g) 0.5 * q ⊗ (ω_m - b_g) ⊗ δq q ⊗ δq̇两边左乘 q* 并约掉0.5和q整理δq̇ 0.5 * [δq ⊗ (ω_m - b_g - δb_g - n_g) - (ω_m - b_g) ⊗ δq]这个式子看起来很啰嗦核心在于计算两个纯虚四元数与一个小角度四元数的乘积差。利用 δq ≈ [1, 0.5*δθ]并把每个乘积拆成标量部分和向量部分忽略二阶小量之后向量部分就得到δθ̇ ≈ -[ω_m - b_g]× * δθ - δb_g - n_g这个式子说人话就是姿态误差的导数受三个因素控制——当前角速度把姿态误差“旋”走反对称矩阵项陀螺仪bias误差直接灌入姿态误差陀螺仪白噪声也是如此。bias误差和重力误差陀螺仪bias和加速度计bias本身被建模成随机游走也就是它们的导数是一个白噪声δb_ġ n_bg δb_ȧ n_ba重力误差如果建模成常量加小扰动就是δġ n_gδ如果你确定重力是精确已知的可以把 δg 整个去掉把相关行列全部砍掉节省计算量。我个人的偏好是保留它因为很多场景下初始姿态的微小误差就等效于重力的微小偏差留着它滤波器会更健壮。3.3 线性化与离散化把上面的式子拼在一起误差状态的连续时间线性系统就是δṗ δv δv̇ -R*[a_m - b_a]× * δθ - R*δb_a δg - R*n_a δθ̇ -[ω_m - b_g]× * δθ - δb_g - n_g δb_ġ n_bg δb_ȧ n_ba δġ 0写成矩阵形式d/dt [δp; δv; δθ; δb_g; δb_a; δg] F * [δp; δv; δθ; δb_g; δb_a; δg] G * n其中连续时间雅可比 F 的左上角3x3为零阵第一行第二列为单位阵第二行第三列为 -R*[a_m - b_a]×第二行第五列为 -R第二行第六列为单位阵第三行第三列为 -[ω_m - b_g]×第三行第四列为 -I其余大部分为0。噪声转移矩阵 G 则是把对应的噪声项映射到 δv-R*n_a、δθ-n_g、δb_gn_bg、δb_an_ba这几行。离散化的标准做法是对 F 做矩阵指数F_d exp(F * Δt) ≈ I FΔt一阶近似实际绝大多数工程实现就这么干因为FΔt的量级很小。噪声协方差矩阵的离散需要把连续白噪声的协方差 Q_c 通过 Q_d G * Q_c * Gᵀ * Δt 来近似。这一个步骤有一个我踩过两次的坑离散化的时候F矩阵里包含的 R、a_m、ω_m 都必须用当前时刻的标称状态值而不是初始值或者常值。有些简化版本在教学里把 F 当成定常的但实际工程中用变系数矩阵的线性化效果差异巨大尤其在转弯、加减速频繁的场景下。每帧IMU数据到达时都重新计算 F 和 Q_d这也是ESKF的“预测”步骤基本都在做矩阵乘法的原因。4. 观测更新、状态注入与协方差重置4.1 观测模型与卡尔曼增益ESKF的观测更新比标准KF稍复杂因为观测通常直接作用在“真实状态”上而我们要修正的是“误差状态”。以最常见的GPS位置观测为例z p_t n p δp n其中 z 是GPS测量值n 是观测噪声。把这个式子重写成误差状态的观测方程y z - p δp n于是观测矩阵 H 就是在误差状态向量的15个维度里只有对应δp的那个3x3块是单位阵其余全是零。写成代码就是Eigen::Matrixdouble, 3, 15 H; H.setZero(); H.block3, 3(0, 0) Eigen::Matrix3d::Identity(); // 对应 δp接下来就是标准的卡尔曼更新公式不过全部作用在误差状态上S H * P * Hᵀ R K P * Hᵀ * S⁻¹ δx K * y P (I - K * H) * P这里 R 是观测噪声协方差GPS的话大概给个 (1m)² 甚至更大如果做视觉融合R 就要根据特征点的像素噪声和深度不确定性来估算。算法工程师真正的调参功夫一半花在 Q_c 的设定上另一半就花在这个 R 的设定上。值得强调的一点是观测方程写的是 y z - h(x)这个“−”是真实世界传感器坐标系下的减法不要随便在不同坐标系之间混用。GPS的z和标称状态p都在世界系下这样H才简单如果观测是相机给出的相对位姿那H就要重新推导对应误差状态里的一部分量模板完全不同。4.2 状态注入与误差重置卡尔曼更新得到了误差状态 δx 的估计值现在把这个误差“注入”标称状态让标称状态修正到离真实状态更近的地方。这个过程很多人写代码时不够重视但它和推导一样容易翻车。位置、速度、bias的注入就是简单的加法p ← p δp v ← v δv b_g ← b_g δb_g b_a ← b_a δb_a g ← g δg姿态的注入不能用加法而要用四元数乘法q ← q ⊗ Exp(δθ)也就是把估计出来的姿态误差旋转到标称姿态上。Exp(δθ) 就是构造小角度四元数 [1, 0.5*δθ]然后左乘到 q 上右乘对应局部误差因为我们当初定义 q_t q ⊗ δq。这里如果有人不小心写了 q ← q δθ 这种代码四元数就会脱离流形模长漂移然后整个滤波器崩溃不夸张。注入完成后误差状态需要清零δx ← 0但是协方差不能直接保留因为注入操作本身是有不确定性的——注入时我们用了 δx 的估计值但真实误差并不完全等于估计值所以协方差也要做一个对应的变换。标准的做法是引入一个雅可比 G用相似变换修改协方差P ← G * P * GᵀG 的具体形式推导起来比较繁琐但15维误差状态的G大体上是单位阵除了姿态那块需要微调。实际工程实现里很多代码库在 δx 很小的时候直接忽略 G 的重置步骤用一个近似“P保持原样”的做法这在观测频率高、误差确实很小的时候问题不大但如果某次观测噪声异常大、δθ 估计值偏大忽略 G 会造成协方差过度自信后续几帧滤波器会陷入震荡甚至发散。严谨的做法是把这个 G 的完整形式写进去。4.3 一个完整的预测-更新周期把所有东西串起来ESKF一个IMU周期的伪代码大致如下// 标称状态预测每帧IMU数据 omega_corr omega_m - b_g; a_corr a_m - b_a; q quatIntegrationExact(q, omega_corr, dt); v (q * a_corr g) * dt; p v * dt; // 误差状态协方差预测 F computeF(q, a_corr, omega_corr); // 15x15 连续时间雅可比 Fd I F * dt; Qd computeQd(q, a_corr, omega_corr, Qc, dt); P Fd * P * Fd.transpose() Qd; // 观测更新例如GPS到达时 H computeH(); y z - p; S H * P * H.transpose() R; K P * H.transpose() * S.inverse(); delta_x K * y; P (I - K * H) * P; // 状态注入 p delta_x.segment3(0); v delta_x.segment3(3); q q * so3Exp(delta_x.segment3(6)); b_g delta_x.segment3(9); b_a delta_x.segment3(12); g delta_x.segment3(15); // 误差状态清零 delta_x.setZero(); // 可选但推荐的协方差重置 P G * P * G.transpose();这个流程看起来简单实际操作时最难调试的是时间戳同步IMU、GPS、相机各自有自己的发布时间如果观测到来时的标称状态不是当前IMU推进到的时刻就必须先做时间对齐否则整个滤波器的精度大打折扣。我自己的项目里专门写了一个时间同步的模块把所有传感器数据都统一到同一个时钟基准上这个钱很值得花。5. 工程实践标定、重力对齐与参数调优5.1 初始姿态与重力对齐所有融合算法工程化第一步就是解决“初始状态从哪来”的问题。ESKF的误差状态可以初始化为0但标称状态的初始值必须有一定准确度否则滤波器启动初期会经历一段收敛过程甚至可能因为误差太大而导致线性化失效根本收敛不了。最常用的初始姿态对齐方法就是利用加速度计。把IMU静止放置或匀速直线运动加速度计测量的就是重力反方向在机体系下的投影。roll和pitch可以直接算出来roll atan2(-a_m_y, -a_m_z) pitch atan2(a_m_x, sqrt(a_m_y² a_m_z²))但yaw完全不可观这就是热词里“imu重力对齐”的内在逻辑。很多初学者测试ESKF在桌面上静置发现yaw缓慢漂移还以为代码写错了其实这是物理上的客观规律纯IMU无法确定绝对航向。工程上的处理方式有几种用磁力计给yaw一个初始值注意磁力计受环境和铁磁干扰影响大如果做视觉或激光融合用第一帧多视角几何解算出的相对旋转来初始化yaw或者在启动时人为给定一个固定初始航向接受yaw有一个整体偏置后续通过其他传感器慢慢校正。另外一个细节就算没有绝对航向修正静止状态下ESKF的yaw并不会像纯IMU积分那样快速漂移——因为ESKF的标称状态每次都会被观测修正但这个修正主要修正位置、速度、roll、pitch的误差yaw的误差会在姿态误差状态里“躺”着直到某个时刻有能提供绝对航向的观测进来。所以你在测试纯位置观测比如GPS时会看到yaw最终收敛到一个值但收敛速度非常慢这是正常现象不是bug。5.2 随机游走协方差怎么设置ESKF里最影响手感的一组参数是过程噪声协方差 Q_c它描述的是你对IMU测量噪声和bias随机游走的信任程度。给的太小滤波器会过度信任IMU预测协方差越走越窄观测更新被压制长期漂移无法矫正给的太大滤波器会过度信任观测每次观测跳来跳去高频IMU的优势就废了。工程上一个相对靠谱的做法是去查IMU芯片的datasheet把陀螺仪白噪声密度通常单位是 deg/s/sqrt(Hz)、加速度计白噪声密度m/s²/sqrt(Hz)、角速度随机游走deg/s²/sqrt(Hz)、加速度随机游走m/s³/sqrt(Hz)转换成离散时间协方差。但datasheet给的是理想环境下的典型值实测往往差2到5倍所以更实用的思路是先按datasheet给一个初始值然后做Allan方差分析Allan variance从采集的静止数据里估计实际噪声参数。Allan方差分析是IMU噪声分析的标准工具可以分离白噪声、bias随机游走、量化噪声等把这个工具加到你的标定流程里能省下大量靠肉眼调参的时间。我自己常用的初始参数矩阵结构大概长这样陀螺仪白噪声方差给在n_g对应位置量级在1e-6到1e-5之间单位rad²/s加速度计白噪声方差在n_a对应位置量级1e-4到1e-3单位m²/s⁴bias随机游走方差非常小1e-9到1e-8因为bias变化极其缓慢。具体数值真的要看传感器照抄别人的参数往往效果很差。5.3 为什么yaw还是慢漂这是热词里出现的问题“基于imu的位姿解算 yaw 仍会慢漂”。即便用了ESKF、即便加了观测yaw依然可能在一个较长时间尺度上漂移。从可观性的角度来解释在只有位置观测或视觉平移观测的情况下系统对yaw的可观性非常弱——直观理解是你看到一个物体向前走了1米它到底是“朝正北走1米”还是“朝偏东0.5度方向走1米”几乎无法区分只有轨迹出现转弯、加速度方向变化足够丰富时yaw的信息才逐渐被激发出来。实操中的解决方案有几种。第一种是在观测里直接加入航向观测比如GPS的航向角多天线RTK或者测速航向、视觉SLAM的绝对朝向先验本质上是在H矩阵里把yaw对应的行直接加进去。第二种是用东北天系的磁力计但磁力计校准是个头疼的问题地磁偏角和软硬铁校准不到位会引入系统偏差。第三种是改变运动策略开车过程中多做一些有激励的转弯让滤波器通过轨迹约束把yaw估计出来这本质上对应着持续激励条件persistent excitation。还有一点非常容易被忽略yaw慢漂也可能是IMU和主传感器外参标定不准导致的。如果IMU和相机或激光雷达之间的旋转外参有微小偏差那么视觉观测本身的坐标转换就有系统误差这种误差并不会被滤波器的bias完全吸收表现出来就是yaw长期缓慢偏移。所以当你在ESKF里排除掉参数和可观性问题后记得回头检查一下外参标定的残差。5.4 和外参标定、联合标定的关系ESKF性能的上限很大程度取决于“外参”是否准确这也是热词里“imu雷达外参标定”、“相机imu联合标定”、“kalibr相机imu联合标定”频繁出现的原因。ESKF本身要求IMU的测量值必须正确投影到主传感器坐标系下如果外参旋转矩阵差个0.5度看起来不多但在长时间预测-更新的循环里这个偏差会以类似bias的形式持续污染状态估计最终导致位置和航向的缓慢漂移。对于单目相机和IMU的联合标定kalibr是实践中的主流工具它用连续帧的图像和IMU数据同时估计相机内参、相机到IMU的外参包括旋转和平移、以及IMU自身的噪声参数。用的时候有几个注意点标定过程中IMU要充分激励六个自由度包括平移和旋转都要有不然外参的某些维度不可观标定板的角点检测质量直接影响外参精度建议用高分辨率相机且标定板不要太远。激光雷达和IMU的外参标定目前没有像kalibr那样统一强大的开源工具很多方案都是基于点到平面距离最优化的LIO-SAM这类框架里一般也带了一个粗略的外参校准模块但对初值敏感初值给不好容易收敛到局部极小。这些标定工作虽然不属于ESKF推导本身但是工程上绕不过去。你推导出来的方程再漂亮外参是错的、时间戳是歪的、噪声参数是瞎拍的最终精度照样烂。这也是为什么我把这一节放在最后——先把数学模型吃透再做扎实的工程标定两者缺一不可。最后再分享一个我在移植ESKF到不同传感器组合时的心得先把观测全部停掉只跑标称状态预测在RVIZ里看看IMU积分轨迹的发散形态是否平滑——如果发散是规律的抛物线说明bias估计或者重力对齐有问题如果发散是剧烈噪声状的说明白噪声参数或者采样率有问题。这一步能帮你把滤波器和传感器的问题分离开来比直接调一堆协方差矩阵瞎试高效得多。ESKF这套东西推导是纸面上的严谨调试是真刀真枪的工程希望你也能在烧掉几轮数据之后体会到误差状态框架那种“看着复杂、用起来真香”的感觉。