尧图网站建设 
1. 从一次姿态漂移说起为什么需要ESKF去年帮一个做四足机器人的朋友调他们的步态控制器遇到一个非常典型的问题机器人静止站立时姿态角还算稳一旦开始小跑俯仰角在十几秒内就能漂出去五六度导致足端落点计算全乱套。他们当时用的是最朴素的互补滤波陀螺仪积分加加速度计修正参数调得再细也压不住动态加速度带来的干扰。后来换成误差卡尔曼滤波器Error-State Kalman FilterESKF同样的IMU数据姿态漂移直接压到了零点几度以内。这件事让我意识到很多做机器人、无人机、AR/VR的朋友都卡在同一个地方知道卡尔曼滤波能融合IMU但真到写代码的时候要么被四元数、旋转矩阵、李群李代数绕晕要么照着论文抄完发现跑起来效果还不如互补滤波。问题往往不在数学本身而在于没有理解ESKF“为什么这样设计”以及“代码里每一步到底在干什么”。这篇内容就是把我自己从零实现ESKF的完整过程拆开来讲。不堆公式不抄论文重点放在工程实现中真正会踩的坑状态量怎么定义、误差状态为什么用15维而不是16维、噪声矩阵怎么填、观测更新时那个让人头大的雅可比怎么推、以及C代码里哪些地方最容易写错。适合已经了解卡尔曼滤波基本概念、想动手实现一个能跑通的IMU状态估计器的朋友。读完你应该能自己写出一个可用的ESKF并且知道每个参数背后的物理意义。2. ESKF的状态定义15维误差状态到底怎么来的2.1 名义状态与误差状态的分离逻辑ESKF最核心的思想是把系统状态拆成两部分名义状态Nominal State和误差状态Error State。名义状态就是系统当前对自身状态的“最佳估计”误差状态描述的是真实状态与名义状态之间的偏差。为什么要这么拆直接对状态做卡尔曼滤波不行吗可以但会有两个麻烦。第一姿态用四元数表示时存在单位模约束标准卡尔曼滤波的线性更新会破坏这个约束导致四元数“跑偏”不再归一化。第二姿态的误差通常很小而四元数的四个分量在数值上并不小直接滤波时数值精度和线性化误差都不好控制。ESKF的做法是名义状态用四元数正常积分误差状态用一个小角度的旋转向量通常是三维来表示姿态偏差。这样误差状态始终在零点附近线性化非常准确而且更新完误差状态后直接把它“注入”名义状态再清零不会破坏四元数的归一化。具体来说我用的名义状态是16维// 名义状态 struct NominalState { Eigen::Vector3d p; // 位置 Eigen::Vector3d v; // 速度 Eigen::Quaterniond q; // 姿态四元数 Eigen::Vector3d ba; // 加速度计零偏 Eigen::Vector3d bg; // 陀螺仪零偏 Eigen::Vector3d g; // 重力向量 };误差状态是15维// 误差状态位置3 速度3 姿态3 加速度零偏3 陀螺零偏3 // 注意重力不参与误差状态因为重力是常量 Eigen::Matrixdouble, 15, 1 dx;这里有个容易困惑的点为什么姿态误差是3维而不是4维因为误差四元数的虚部在小角度下近似等于旋转向量的一半而实部约等于1所以只需要用三维的旋转向量就能完整描述姿态误差。这个近似在误差角小于10度时精度非常好而ESKF的误差状态正常情况下确实很小。2.2 连续时间下的状态微分方程名义状态的微分方程直接来自IMU的运动学模型。加速度计测量的是比力specific force陀螺仪测量的是角速度考虑零偏后// 名义状态积分简化版实际用中值积分 void integrateNominal(NominalState s, const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt) { Eigen::Vector3d a_world s.q * (acc - s.ba) s.g; s.p s.v * dt 0.5 * a_world * dt * dt; s.v a_world * dt; Eigen::Vector3d omega gyro - s.bg; Eigen::Quaterniond dq(1, 0.5*omega.x()*dt, 0.5*omega.y()*dt, 0.5*omega.z()*dt); s.q (s.q * dq).normalized(); }误差状态的微分方程则是名义状态方程的线性化结果。这里不展开完整推导直接给出工程上常用的形式// 误差状态转移矩阵 F15x15 Eigen::Matrixdouble, 15, 15 F Eigen::Matrixdouble, 15, 15::Identity(); F.block3,3(0, 3) Eigen::Matrix3d::Identity() * dt; // dp/dv F.block3,3(3, 6) -skew(s.q * (acc - s.ba)) * dt; // dv/dtheta F.block3,3(3, 9) -s.q.toRotationMatrix() * dt; // dv/dba F.block3,3(6, 6) Eigen::Matrix3d::Identity() - skew(gyro - s.bg) * dt; // dtheta/dtheta F.block3,3(6, 12) -Eigen::Matrix3d::Identity() * dt; // dtheta/dbg其中skew()是反对称矩阵函数。这个F矩阵是ESKF预测步骤的核心写错一个符号后面全崩。2.3 噪声矩阵Q的物理含义与填法过程噪声矩阵Q描述的是IMU测量噪声和零偏随机游走对误差状态的影响。很多教程直接给一个对角阵就完事但实际调参时如果不理解每个元素的物理意义根本不知道该往大调还是往小调。我的Q矩阵按15维误差状态排列主要非零块如下噪声项对应状态典型量级物理来源速度噪声v0.01~0.1 m/s²加速度计白噪声姿态噪声θ0.001~0.01 rad陀螺仪白噪声加速度零偏游走ba1e-4~1e-3零偏不稳定性陀螺零偏游走bg1e-5~1e-4零偏不稳定性Eigen::Matrixdouble, 15, 15 Q Eigen::Matrixdouble, 15, 15::Zero(); double sigma_a 0.05; // 加速度计噪声密度 double sigma_g 0.005; // 陀螺仪噪声密度 double sigma_ba 1e-3; // 加速度零偏游走 double sigma_bg 1e-4; // 陀螺零偏游走 Q.block3,3(3, 3) Eigen::Matrix3d::Identity() * sigma_a * sigma_a * dt * dt; Q.block3,3(6, 6) Eigen::Matrix3d::Identity() * sigma_g * sigma_g * dt * dt; Q.block3,3(9, 9) Eigen::Matrix3d::Identity() * sigma_ba * sigma_ba * dt; Q.block3,3(12, 12) Eigen::Matrix3d::Identity() * sigma_bg * sigma_bg * dt;注意Q矩阵里速度噪声和姿态噪声乘的是dt²零偏游走乘的是dt。这是因为白噪声积分后方差与dt²成正比而随机游走的方差与dt成正比。这个细节写错的话滤波器在不同频率下表现会完全不一样。3. 预测与更新ESKF的完整代码链路3.1 预测步骤协方差传播与状态积分预测步骤做两件事名义状态按IMU数据积分误差状态协方差按F矩阵传播。void predict(const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt) { // 1. 积分名义状态 integrateNominal(nominal, acc, gyro, dt); // 2. 构建F矩阵见上一节 Eigen::Matrixdouble, 15, 15 F buildF(acc, gyro, dt); // 3. 协方差传播 P F * P * F.transpose() Q; // 4. 强制对称数值误差会导致P不对称 P 0.5 * (P P.transpose()); }这里有个实操中非常重要的细节协方差矩阵P必须强制对称。浮点运算累积误差会让P逐渐失去对称性虽然理论上不影响但实际跑久了可能出现负特征值导致滤波器发散。我一般每步都做一次对称化计算量可以忽略。另一个细节是名义状态积分用中值积分比欧拉积分精度高很多。具体做法是保存上一时刻的IMU数据用两次测量的平均值来积分Eigen::Vector3d acc_mid 0.5 * (acc_prev acc_curr); Eigen::Vector3d gyro_mid 0.5 * (gyro_prev gyro_curr);在200Hz的IMU频率下欧拉积分和中值积分的差异可能不明显但如果IMU频率降到100Hz以下或者机器人运动比较剧烈中值积分的优势就体现出来了。3.2 观测更新以GPS位置观测为例观测更新是ESKF里最容易写错的部分核心难点在于观测雅可比矩阵H的推导。这里以GPS提供的位置观测为例观测方程是z p n其中z是GPS测量位置p是名义状态中的位置n是观测噪声。因为观测直接对应误差状态中的位置部分所以H矩阵非常简单// 观测维度3x,y,z位置误差状态维度15 Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 0) Eigen::Matrix3d::Identity();然后走标准卡尔曼更新流程void updatePosition(const Eigen::Vector3d z, const Eigen::Matrix3d R) { Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 0) Eigen::Matrix3d::Identity(); Eigen::Vector3d r z - nominal.p; // 残差 Eigen::Matrix3d S H * P * H.transpose() R; Eigen::Matrixdouble, 15, 3 K P * H.transpose() * S.inverse(); dx K * r; P (Eigen::Matrixdouble, 15, 15::Identity() - K * H) * P; P 0.5 * (P P.transpose()); injectErrorState(); }3.3 误差状态注入最容易出bug的一步更新完误差状态dx后需要把它“注入”名义状态然后把dx清零。这一步看起来简单但姿态部分的注入方式很容易写错void injectErrorState() { nominal.p dx.segment3(0); nominal.v dx.segment3(3); // 姿态注入用误差旋转向量构造四元数右乘到名义四元数上 Eigen::Vector3d dtheta dx.segment3(6); Eigen::Quaterniond dq(1, 0.5*dtheta.x(), 0.5*dtheta.y(), 0.5*dtheta.z()); nominal.q (nominal.q * dq).normalized(); nominal.ba dx.segment3(9); nominal.bg dx.segment3(12); dx.setZero(); }关键点姿态误差注入是右乘而不是左乘。因为误差状态是在机体坐标系下定义的右乘对应的是机体坐标系下的微小旋转。如果写成左乘在小误差下可能看不出问题但误差稍大时姿态会往反方向修正滤波器直接发散。我第一次实现时就踩了这个坑当时用左乘跑了一下午姿态角一直在震荡以为是Q矩阵参数不对调了半天才发现是注入方向反了。这个教训告诉我ESKF的每个符号都有物理意义不能凭感觉写。4. 实测调参从能跑到好用的关键细节4.1 初始协方差P0的设置策略初始协方差P0反映的是对初始状态的置信度。很多人直接设成单位阵结果滤波器启动后前几秒姿态剧烈跳动。合理的做法是根据实际传感器的初始不确定性来设P.setZero(); P.block3,3(0, 0) Eigen::Matrix3d::Identity() * 1.0; // 位置不确定度1m² P.block3,3(3, 3) Eigen::Matrix3d::Identity() * 0.1; // 速度不确定度0.1(m/s)² P.block3,3(6, 6) Eigen::Matrix3d::Identity() * 0.01; // 姿态不确定度约5.7度 P.block3,3(9, 9) Eigen::Matrix3d::Identity() * 0.01; // 加速度零偏 P.block3,3(12, 12) Eigen::Matrix3d::Identity() * 0.001; // 陀螺零偏如果系统启动时是静止的可以先做几秒钟的零偏估计把ba和bg的初始值设准对应的P0也可以设小一些。我一般会让系统静止2秒取这2秒陀螺仪和加速度计的平均值作为初始零偏效果比直接设零好很多。4.2 观测噪声R的标定方法观测噪声R的设定直接影响滤波器对观测的信任程度。以GPS为例如果R设得太大滤波器几乎不修正位置纯靠IMU积分漂移R设得太小GPS的跳变会直接传到姿态上。我的经验是R的对角线元素取观测传感器标称精度的平方。比如GPS水平精度1.5米那R的水平分量就设2.25。但实际使用中GPS的噪声往往不是高斯的会有多路径效应导致的野值。这时候需要在更新前做卡方检验double chi2 r.transpose() * S.inverse() * r; if (chi2 7.815) { // 3自由度卡方分布95%置信度阈值 return; // 拒绝这次观测 }这个简单的门限判断能过滤掉大部分GPS野值比调R参数有效得多。4.3 零偏估计的收敛判断ESKF相比普通卡尔曼滤波的一个优势是能在线估计零偏。但零偏估计需要足够的激励才能收敛如果机器人一直静止或匀速运动零偏是不可观的。判断零偏是否收敛有个实用技巧观察P矩阵中零偏对应的对角元素。当这些元素降到初始值的1/10以下时说明零偏估计已经比较稳定了。如果跑了很久P还不降要么是运动激励不够要么是Q中零偏游走设得太大。// 监控零偏收敛情况 double ba_uncertainty std::sqrt(P(9,9) P(10,10) P(11,11)); double bg_uncertainty std::sqrt(P(12,12) P(13,13) P(14,14)); std::cout ba uncertainty: ba_uncertainty , bg uncertainty: bg_uncertainty std::endl;5. 那些让我熬夜的坑ESKF实现中的典型错误5.1 坐标系混乱导致的“幽灵漂移”ESKF涉及至少三个坐标系世界坐标系、机体坐标系、IMU坐标系。如果IMU安装时与机体有旋转偏差而代码里没有做外参补偿姿态估计会一直存在一个固定偏差。更隐蔽的问题是重力的方向。有些IMU的加速度计输出是“比力”静止时读数是9.81指向天有些是-9.81。如果符号搞反重力向量g的初始值就错了滤波器会试图用姿态去补偿这个错误导致姿态缓慢漂移。我的做法是在代码里显式定义清楚// 世界坐标系Z轴向上重力向量为(0, 0, -9.81) // 加速度计测量的是比力静止时输出(0, 0, 9.81) Eigen::Vector3d g_world(0, 0, -9.81);然后在初始化时用静止段的加速度计读数来对齐初始姿态// 用静止时的加速度计读数估计初始roll和pitch Eigen::Vector3d acc0 meanAccStatic; double roll std::atan2(acc0.y(), acc0.z()); double pitch std::atan2(-acc0.x(), std::sqrt(acc0.y()*acc0.y() acc0.z()*acc0.z()));5.2 四元数更新时的归一化陷阱四元数在每次乘法后都应该归一化但归一化的时机有讲究。如果在误差注入后立即归一化而误差旋转向量本身不是精确的单位四元数会引入微小误差。正确做法是名义四元数积分后归一化误差注入时构造的dq不需要归一化小角度下模长接近1注入后再归一化// 积分后 nominal.q.normalize(); // 注入时 Eigen::Quaterniond dq(1, 0.5*dtheta.x(), 0.5*dtheta.y(), 0.5*dtheta.z()); nominal.q nominal.q * dq; nominal.q.normalize(); // 注入后归一化5.3 数值精度问题float还是doubleEigen默认用double但在嵌入式平台上为了性能可能会用float。我的建议是协方差矩阵P和卡尔曼增益K的计算必须用double即使名义状态用float。因为P矩阵的条件数可能很大float的精度不够会导致K矩阵计算出现数值问题。如果平台性能实在有限至少要把P和K的计算放在double下算完后再转回float存储。这个开销在200Hz下完全可以接受。5.4 更新频率不匹配的处理IMU通常跑200Hz以上而GPS可能只有10Hz视觉里程计30Hz。不同频率的观测怎么处理我的做法是把预测和更新解耦// 主循环每次IMU数据到来时做预测 void imuCallback(const ImuMsg msg) { double dt msg.timestamp - last_timestamp; eskf.predict(msg.acc, msg.gyro, dt); last_timestamp msg.timestamp; } // GPS回调独立线程或事件触发 void gpsCallback(const GpsMsg msg) { eskf.updatePosition(msg.position, R_gps); }这样预测始终以IMU频率运行观测到来时随时更新不需要做复杂的时间对齐。唯一需要注意的是多线程访问ESKF对象时要加锁或者用消息队列把观测数据转到IMU线程处理。6. 从ESKF到多传感器融合的扩展思路6.1 加入视觉观测的注意事项视觉里程计或SLAM系统通常提供位置和姿态观测。位置观测的H矩阵和GPS一样但姿态观测的H矩阵需要特别注意// 姿态观测z q n观测的是四元数 // 但误差状态中的姿态误差是旋转向量需要转换 Eigen::Matrixdouble, 4, 15 H_q Eigen::Matrixdouble, 4, 15::Zero(); // 四元数对旋转向量的雅可比 Eigen::Matrixdouble, 4, 3 J_q_theta; J_q_theta -0.5*dx_q.x(), -0.5*dx_q.y(), -0.5*dx_q.z(), 0.5*dx_q.w(), 0, 0, 0, 0.5*dx_q.w(), 0, 0, 0, 0.5*dx_q.w(); H_q.block4,3(0, 6) J_q_theta;这个雅可比矩阵推导起来比较繁琐而且四元数观测的残差计算也要注意符号。如果视觉姿态观测的坐标系和IMU不一致还需要做外参旋转。6.2 零速修正ZUPT的实用技巧对于足式机器人或行人导航脚触地时可以认为速度为零。ZUPT是ESKF里性价比最高的观测更新实现简单且效果显著void updateZUPT() { Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 3) Eigen::Matrix3d::Identity(); // 观测速度 Eigen::Vector3d r -nominal.v; // 残差测量速度0减去估计速度 Eigen::Matrix3d R Eigen::Matrix3d::Identity() * 0.01; // 零速噪声很小 // 标准更新... }ZUPT的关键是准确检测触地时刻。我一般用加速度计的模长和陀螺仪的模长做判断加速度接近9.81且陀螺接近0时认为触地。检测窗口取触地前后各50ms能有效抑制速度漂移。6.3 磁力计观测的融合策略磁力计可以提供航向角观测但磁干扰是常态。我的策略是只在磁力计读数与当前估计航向差异小于30度时才做更新否则直接跳过。这样既能利用磁力计抑制航向漂移又不会被室内磁干扰带偏。double yaw_meas std::atan2(mag.y(), mag.x()); double yaw_est getYawFromQuaternion(nominal.q); double yaw_diff normalizeAngle(yaw_meas - yaw_est); if (std::abs(yaw_diff) M_PI / 6) { updateYaw(yaw_meas, R_mag); }7. 代码组织与工程化建议7.1 类结构设计一个可维护的ESKF实现应该把状态、预测、更新分离清楚。我的类结构大致如下class EskfEstimator { public: void init(const Eigen::Vector3d acc0, const Eigen::Vector3d gyro0); void predict(const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt); void updatePosition(const Eigen::Vector3d pos, const Eigen::Matrix3d R); void updateZUPT(); void updateYaw(double yaw, double R); NominalState getState() const { return nominal_; } private: NominalState nominal_; Eigen::Matrixdouble, 15, 1 dx_; Eigen::Matrixdouble, 15, 15 P_; Eigen::Matrixdouble, 15, 15 Q_; void injectErrorState(); Eigen::Matrixdouble, 15, 15 buildF( const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt); };7.2 调试与可视化ESKF调试最有效的手段是把关键量记录下来画图。我一般会记录姿态角随时间变化、速度估计、零偏估计、P矩阵对角线、每次更新的残差和卡方值。用Python的matplotlib画出来一眼就能看出滤波器是否正常。如果姿态角出现高频抖动通常是Q中姿态噪声太大如果姿态角缓慢漂移可能是零偏估计没收敛或重力方向设错如果更新时姿态跳变检查H矩阵和残差的符号。7.3 实时性优化在嵌入式平台上15x15矩阵的乘法和求逆是主要计算量。几个优化点F矩阵大部分是零可以用稀疏矩阵或手动展开乘法S矩阵只有3x3或4x4求逆开销很小P矩阵的对称性可以利用只计算上三角如果更新频率低于预测频率可以把多次预测合并在树莓派上200Hz的预测加10Hz的位置更新CPU占用不到5%。即使是在STM32F4上优化后也能跑到100Hz以上。8. 一些个人体会ESKF这个算法我前前后后实现了三四遍每一遍都会发现之前没注意到的细节。第一遍照着论文抄跑起来姿态发散第二遍搞清楚了坐标系和符号能跑了但精度一般第三遍认真调了Q和R做了零偏在线估计效果才真正出来。最大的体会是ESKF的数学推导可以看论文但工程实现必须自己踩坑。论文里不会告诉你四元数注入要右乘不会告诉你P矩阵要强制对称不会告诉你GPS野值要用卡方检验过滤。这些细节才是决定滤波器能不能用的关键。另外不要迷信“最优估计”。ESKF在模型准确、噪声高斯的前提下确实是最优的但实际系统中IMU有刻度因数误差、轴间耦合、温度漂移这些都不在模型里。所以调参时不要追求理论最优而是根据实际数据反复试。我通常会用同一段数据跑不同参数对比姿态和位置的均方误差选一个在实际场景中表现最稳的参数组合。最后分享一个实用建议如果你的应用对姿态精度要求高但对位置要求不高可以把位置和速度从误差状态中去掉只保留姿态和零偏做成9维或6维的ESKF。状态维度降低后计算量更小姿态估计的收敛也更快。这个简化在云台稳定、AR/VR头显等场景中非常实用。 从零实现ESKF：15维误差状态与IMU姿态融合实战
1. 从一次姿态漂移说起为什么需要ESKF去年帮一个做四足机器人的朋友调他们的步态控制器遇到一个非常典型的问题机器人静止站立时姿态角还算稳一旦开始小跑俯仰角在十几秒内就能漂出去五六度导致足端落点计算全乱套。他们当时用的是最朴素的互补滤波陀螺仪积分加加速度计修正参数调得再细也压不住动态加速度带来的干扰。后来换成误差卡尔曼滤波器Error-State Kalman FilterESKF同样的IMU数据姿态漂移直接压到了零点几度以内。这件事让我意识到很多做机器人、无人机、AR/VR的朋友都卡在同一个地方知道卡尔曼滤波能融合IMU但真到写代码的时候要么被四元数、旋转矩阵、李群李代数绕晕要么照着论文抄完发现跑起来效果还不如互补滤波。问题往往不在数学本身而在于没有理解ESKF“为什么这样设计”以及“代码里每一步到底在干什么”。这篇内容就是把我自己从零实现ESKF的完整过程拆开来讲。不堆公式不抄论文重点放在工程实现中真正会踩的坑状态量怎么定义、误差状态为什么用15维而不是16维、噪声矩阵怎么填、观测更新时那个让人头大的雅可比怎么推、以及C代码里哪些地方最容易写错。适合已经了解卡尔曼滤波基本概念、想动手实现一个能跑通的IMU状态估计器的朋友。读完你应该能自己写出一个可用的ESKF并且知道每个参数背后的物理意义。2. ESKF的状态定义15维误差状态到底怎么来的2.1 名义状态与误差状态的分离逻辑ESKF最核心的思想是把系统状态拆成两部分名义状态Nominal State和误差状态Error State。名义状态就是系统当前对自身状态的“最佳估计”误差状态描述的是真实状态与名义状态之间的偏差。为什么要这么拆直接对状态做卡尔曼滤波不行吗可以但会有两个麻烦。第一姿态用四元数表示时存在单位模约束标准卡尔曼滤波的线性更新会破坏这个约束导致四元数“跑偏”不再归一化。第二姿态的误差通常很小而四元数的四个分量在数值上并不小直接滤波时数值精度和线性化误差都不好控制。ESKF的做法是名义状态用四元数正常积分误差状态用一个小角度的旋转向量通常是三维来表示姿态偏差。这样误差状态始终在零点附近线性化非常准确而且更新完误差状态后直接把它“注入”名义状态再清零不会破坏四元数的归一化。具体来说我用的名义状态是16维// 名义状态 struct NominalState { Eigen::Vector3d p; // 位置 Eigen::Vector3d v; // 速度 Eigen::Quaterniond q; // 姿态四元数 Eigen::Vector3d ba; // 加速度计零偏 Eigen::Vector3d bg; // 陀螺仪零偏 Eigen::Vector3d g; // 重力向量 };误差状态是15维// 误差状态位置3 速度3 姿态3 加速度零偏3 陀螺零偏3 // 注意重力不参与误差状态因为重力是常量 Eigen::Matrixdouble, 15, 1 dx;这里有个容易困惑的点为什么姿态误差是3维而不是4维因为误差四元数的虚部在小角度下近似等于旋转向量的一半而实部约等于1所以只需要用三维的旋转向量就能完整描述姿态误差。这个近似在误差角小于10度时精度非常好而ESKF的误差状态正常情况下确实很小。2.2 连续时间下的状态微分方程名义状态的微分方程直接来自IMU的运动学模型。加速度计测量的是比力specific force陀螺仪测量的是角速度考虑零偏后// 名义状态积分简化版实际用中值积分 void integrateNominal(NominalState s, const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt) { Eigen::Vector3d a_world s.q * (acc - s.ba) s.g; s.p s.v * dt 0.5 * a_world * dt * dt; s.v a_world * dt; Eigen::Vector3d omega gyro - s.bg; Eigen::Quaterniond dq(1, 0.5*omega.x()*dt, 0.5*omega.y()*dt, 0.5*omega.z()*dt); s.q (s.q * dq).normalized(); }误差状态的微分方程则是名义状态方程的线性化结果。这里不展开完整推导直接给出工程上常用的形式// 误差状态转移矩阵 F15x15 Eigen::Matrixdouble, 15, 15 F Eigen::Matrixdouble, 15, 15::Identity(); F.block3,3(0, 3) Eigen::Matrix3d::Identity() * dt; // dp/dv F.block3,3(3, 6) -skew(s.q * (acc - s.ba)) * dt; // dv/dtheta F.block3,3(3, 9) -s.q.toRotationMatrix() * dt; // dv/dba F.block3,3(6, 6) Eigen::Matrix3d::Identity() - skew(gyro - s.bg) * dt; // dtheta/dtheta F.block3,3(6, 12) -Eigen::Matrix3d::Identity() * dt; // dtheta/dbg其中skew()是反对称矩阵函数。这个F矩阵是ESKF预测步骤的核心写错一个符号后面全崩。2.3 噪声矩阵Q的物理含义与填法过程噪声矩阵Q描述的是IMU测量噪声和零偏随机游走对误差状态的影响。很多教程直接给一个对角阵就完事但实际调参时如果不理解每个元素的物理意义根本不知道该往大调还是往小调。我的Q矩阵按15维误差状态排列主要非零块如下噪声项对应状态典型量级物理来源速度噪声v0.01~0.1 m/s²加速度计白噪声姿态噪声θ0.001~0.01 rad陀螺仪白噪声加速度零偏游走ba1e-4~1e-3零偏不稳定性陀螺零偏游走bg1e-5~1e-4零偏不稳定性Eigen::Matrixdouble, 15, 15 Q Eigen::Matrixdouble, 15, 15::Zero(); double sigma_a 0.05; // 加速度计噪声密度 double sigma_g 0.005; // 陀螺仪噪声密度 double sigma_ba 1e-3; // 加速度零偏游走 double sigma_bg 1e-4; // 陀螺零偏游走 Q.block3,3(3, 3) Eigen::Matrix3d::Identity() * sigma_a * sigma_a * dt * dt; Q.block3,3(6, 6) Eigen::Matrix3d::Identity() * sigma_g * sigma_g * dt * dt; Q.block3,3(9, 9) Eigen::Matrix3d::Identity() * sigma_ba * sigma_ba * dt; Q.block3,3(12, 12) Eigen::Matrix3d::Identity() * sigma_bg * sigma_bg * dt;注意Q矩阵里速度噪声和姿态噪声乘的是dt²零偏游走乘的是dt。这是因为白噪声积分后方差与dt²成正比而随机游走的方差与dt成正比。这个细节写错的话滤波器在不同频率下表现会完全不一样。3. 预测与更新ESKF的完整代码链路3.1 预测步骤协方差传播与状态积分预测步骤做两件事名义状态按IMU数据积分误差状态协方差按F矩阵传播。void predict(const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt) { // 1. 积分名义状态 integrateNominal(nominal, acc, gyro, dt); // 2. 构建F矩阵见上一节 Eigen::Matrixdouble, 15, 15 F buildF(acc, gyro, dt); // 3. 协方差传播 P F * P * F.transpose() Q; // 4. 强制对称数值误差会导致P不对称 P 0.5 * (P P.transpose()); }这里有个实操中非常重要的细节协方差矩阵P必须强制对称。浮点运算累积误差会让P逐渐失去对称性虽然理论上不影响但实际跑久了可能出现负特征值导致滤波器发散。我一般每步都做一次对称化计算量可以忽略。另一个细节是名义状态积分用中值积分比欧拉积分精度高很多。具体做法是保存上一时刻的IMU数据用两次测量的平均值来积分Eigen::Vector3d acc_mid 0.5 * (acc_prev acc_curr); Eigen::Vector3d gyro_mid 0.5 * (gyro_prev gyro_curr);在200Hz的IMU频率下欧拉积分和中值积分的差异可能不明显但如果IMU频率降到100Hz以下或者机器人运动比较剧烈中值积分的优势就体现出来了。3.2 观测更新以GPS位置观测为例观测更新是ESKF里最容易写错的部分核心难点在于观测雅可比矩阵H的推导。这里以GPS提供的位置观测为例观测方程是z p n其中z是GPS测量位置p是名义状态中的位置n是观测噪声。因为观测直接对应误差状态中的位置部分所以H矩阵非常简单// 观测维度3x,y,z位置误差状态维度15 Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 0) Eigen::Matrix3d::Identity();然后走标准卡尔曼更新流程void updatePosition(const Eigen::Vector3d z, const Eigen::Matrix3d R) { Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 0) Eigen::Matrix3d::Identity(); Eigen::Vector3d r z - nominal.p; // 残差 Eigen::Matrix3d S H * P * H.transpose() R; Eigen::Matrixdouble, 15, 3 K P * H.transpose() * S.inverse(); dx K * r; P (Eigen::Matrixdouble, 15, 15::Identity() - K * H) * P; P 0.5 * (P P.transpose()); injectErrorState(); }3.3 误差状态注入最容易出bug的一步更新完误差状态dx后需要把它“注入”名义状态然后把dx清零。这一步看起来简单但姿态部分的注入方式很容易写错void injectErrorState() { nominal.p dx.segment3(0); nominal.v dx.segment3(3); // 姿态注入用误差旋转向量构造四元数右乘到名义四元数上 Eigen::Vector3d dtheta dx.segment3(6); Eigen::Quaterniond dq(1, 0.5*dtheta.x(), 0.5*dtheta.y(), 0.5*dtheta.z()); nominal.q (nominal.q * dq).normalized(); nominal.ba dx.segment3(9); nominal.bg dx.segment3(12); dx.setZero(); }关键点姿态误差注入是右乘而不是左乘。因为误差状态是在机体坐标系下定义的右乘对应的是机体坐标系下的微小旋转。如果写成左乘在小误差下可能看不出问题但误差稍大时姿态会往反方向修正滤波器直接发散。我第一次实现时就踩了这个坑当时用左乘跑了一下午姿态角一直在震荡以为是Q矩阵参数不对调了半天才发现是注入方向反了。这个教训告诉我ESKF的每个符号都有物理意义不能凭感觉写。4. 实测调参从能跑到好用的关键细节4.1 初始协方差P0的设置策略初始协方差P0反映的是对初始状态的置信度。很多人直接设成单位阵结果滤波器启动后前几秒姿态剧烈跳动。合理的做法是根据实际传感器的初始不确定性来设P.setZero(); P.block3,3(0, 0) Eigen::Matrix3d::Identity() * 1.0; // 位置不确定度1m² P.block3,3(3, 3) Eigen::Matrix3d::Identity() * 0.1; // 速度不确定度0.1(m/s)² P.block3,3(6, 6) Eigen::Matrix3d::Identity() * 0.01; // 姿态不确定度约5.7度 P.block3,3(9, 9) Eigen::Matrix3d::Identity() * 0.01; // 加速度零偏 P.block3,3(12, 12) Eigen::Matrix3d::Identity() * 0.001; // 陀螺零偏如果系统启动时是静止的可以先做几秒钟的零偏估计把ba和bg的初始值设准对应的P0也可以设小一些。我一般会让系统静止2秒取这2秒陀螺仪和加速度计的平均值作为初始零偏效果比直接设零好很多。4.2 观测噪声R的标定方法观测噪声R的设定直接影响滤波器对观测的信任程度。以GPS为例如果R设得太大滤波器几乎不修正位置纯靠IMU积分漂移R设得太小GPS的跳变会直接传到姿态上。我的经验是R的对角线元素取观测传感器标称精度的平方。比如GPS水平精度1.5米那R的水平分量就设2.25。但实际使用中GPS的噪声往往不是高斯的会有多路径效应导致的野值。这时候需要在更新前做卡方检验double chi2 r.transpose() * S.inverse() * r; if (chi2 7.815) { // 3自由度卡方分布95%置信度阈值 return; // 拒绝这次观测 }这个简单的门限判断能过滤掉大部分GPS野值比调R参数有效得多。4.3 零偏估计的收敛判断ESKF相比普通卡尔曼滤波的一个优势是能在线估计零偏。但零偏估计需要足够的激励才能收敛如果机器人一直静止或匀速运动零偏是不可观的。判断零偏是否收敛有个实用技巧观察P矩阵中零偏对应的对角元素。当这些元素降到初始值的1/10以下时说明零偏估计已经比较稳定了。如果跑了很久P还不降要么是运动激励不够要么是Q中零偏游走设得太大。// 监控零偏收敛情况 double ba_uncertainty std::sqrt(P(9,9) P(10,10) P(11,11)); double bg_uncertainty std::sqrt(P(12,12) P(13,13) P(14,14)); std::cout ba uncertainty: ba_uncertainty , bg uncertainty: bg_uncertainty std::endl;5. 那些让我熬夜的坑ESKF实现中的典型错误5.1 坐标系混乱导致的“幽灵漂移”ESKF涉及至少三个坐标系世界坐标系、机体坐标系、IMU坐标系。如果IMU安装时与机体有旋转偏差而代码里没有做外参补偿姿态估计会一直存在一个固定偏差。更隐蔽的问题是重力的方向。有些IMU的加速度计输出是“比力”静止时读数是9.81指向天有些是-9.81。如果符号搞反重力向量g的初始值就错了滤波器会试图用姿态去补偿这个错误导致姿态缓慢漂移。我的做法是在代码里显式定义清楚// 世界坐标系Z轴向上重力向量为(0, 0, -9.81) // 加速度计测量的是比力静止时输出(0, 0, 9.81) Eigen::Vector3d g_world(0, 0, -9.81);然后在初始化时用静止段的加速度计读数来对齐初始姿态// 用静止时的加速度计读数估计初始roll和pitch Eigen::Vector3d acc0 meanAccStatic; double roll std::atan2(acc0.y(), acc0.z()); double pitch std::atan2(-acc0.x(), std::sqrt(acc0.y()*acc0.y() acc0.z()*acc0.z()));5.2 四元数更新时的归一化陷阱四元数在每次乘法后都应该归一化但归一化的时机有讲究。如果在误差注入后立即归一化而误差旋转向量本身不是精确的单位四元数会引入微小误差。正确做法是名义四元数积分后归一化误差注入时构造的dq不需要归一化小角度下模长接近1注入后再归一化// 积分后 nominal.q.normalize(); // 注入时 Eigen::Quaterniond dq(1, 0.5*dtheta.x(), 0.5*dtheta.y(), 0.5*dtheta.z()); nominal.q nominal.q * dq; nominal.q.normalize(); // 注入后归一化5.3 数值精度问题float还是doubleEigen默认用double但在嵌入式平台上为了性能可能会用float。我的建议是协方差矩阵P和卡尔曼增益K的计算必须用double即使名义状态用float。因为P矩阵的条件数可能很大float的精度不够会导致K矩阵计算出现数值问题。如果平台性能实在有限至少要把P和K的计算放在double下算完后再转回float存储。这个开销在200Hz下完全可以接受。5.4 更新频率不匹配的处理IMU通常跑200Hz以上而GPS可能只有10Hz视觉里程计30Hz。不同频率的观测怎么处理我的做法是把预测和更新解耦// 主循环每次IMU数据到来时做预测 void imuCallback(const ImuMsg msg) { double dt msg.timestamp - last_timestamp; eskf.predict(msg.acc, msg.gyro, dt); last_timestamp msg.timestamp; } // GPS回调独立线程或事件触发 void gpsCallback(const GpsMsg msg) { eskf.updatePosition(msg.position, R_gps); }这样预测始终以IMU频率运行观测到来时随时更新不需要做复杂的时间对齐。唯一需要注意的是多线程访问ESKF对象时要加锁或者用消息队列把观测数据转到IMU线程处理。6. 从ESKF到多传感器融合的扩展思路6.1 加入视觉观测的注意事项视觉里程计或SLAM系统通常提供位置和姿态观测。位置观测的H矩阵和GPS一样但姿态观测的H矩阵需要特别注意// 姿态观测z q n观测的是四元数 // 但误差状态中的姿态误差是旋转向量需要转换 Eigen::Matrixdouble, 4, 15 H_q Eigen::Matrixdouble, 4, 15::Zero(); // 四元数对旋转向量的雅可比 Eigen::Matrixdouble, 4, 3 J_q_theta; J_q_theta -0.5*dx_q.x(), -0.5*dx_q.y(), -0.5*dx_q.z(), 0.5*dx_q.w(), 0, 0, 0, 0.5*dx_q.w(), 0, 0, 0, 0.5*dx_q.w(); H_q.block4,3(0, 6) J_q_theta;这个雅可比矩阵推导起来比较繁琐而且四元数观测的残差计算也要注意符号。如果视觉姿态观测的坐标系和IMU不一致还需要做外参旋转。6.2 零速修正ZUPT的实用技巧对于足式机器人或行人导航脚触地时可以认为速度为零。ZUPT是ESKF里性价比最高的观测更新实现简单且效果显著void updateZUPT() { Eigen::Matrixdouble, 3, 15 H Eigen::Matrixdouble, 3, 15::Zero(); H.block3,3(0, 3) Eigen::Matrix3d::Identity(); // 观测速度 Eigen::Vector3d r -nominal.v; // 残差测量速度0减去估计速度 Eigen::Matrix3d R Eigen::Matrix3d::Identity() * 0.01; // 零速噪声很小 // 标准更新... }ZUPT的关键是准确检测触地时刻。我一般用加速度计的模长和陀螺仪的模长做判断加速度接近9.81且陀螺接近0时认为触地。检测窗口取触地前后各50ms能有效抑制速度漂移。6.3 磁力计观测的融合策略磁力计可以提供航向角观测但磁干扰是常态。我的策略是只在磁力计读数与当前估计航向差异小于30度时才做更新否则直接跳过。这样既能利用磁力计抑制航向漂移又不会被室内磁干扰带偏。double yaw_meas std::atan2(mag.y(), mag.x()); double yaw_est getYawFromQuaternion(nominal.q); double yaw_diff normalizeAngle(yaw_meas - yaw_est); if (std::abs(yaw_diff) M_PI / 6) { updateYaw(yaw_meas, R_mag); }7. 代码组织与工程化建议7.1 类结构设计一个可维护的ESKF实现应该把状态、预测、更新分离清楚。我的类结构大致如下class EskfEstimator { public: void init(const Eigen::Vector3d acc0, const Eigen::Vector3d gyro0); void predict(const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt); void updatePosition(const Eigen::Vector3d pos, const Eigen::Matrix3d R); void updateZUPT(); void updateYaw(double yaw, double R); NominalState getState() const { return nominal_; } private: NominalState nominal_; Eigen::Matrixdouble, 15, 1 dx_; Eigen::Matrixdouble, 15, 15 P_; Eigen::Matrixdouble, 15, 15 Q_; void injectErrorState(); Eigen::Matrixdouble, 15, 15 buildF( const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt); };7.2 调试与可视化ESKF调试最有效的手段是把关键量记录下来画图。我一般会记录姿态角随时间变化、速度估计、零偏估计、P矩阵对角线、每次更新的残差和卡方值。用Python的matplotlib画出来一眼就能看出滤波器是否正常。如果姿态角出现高频抖动通常是Q中姿态噪声太大如果姿态角缓慢漂移可能是零偏估计没收敛或重力方向设错如果更新时姿态跳变检查H矩阵和残差的符号。7.3 实时性优化在嵌入式平台上15x15矩阵的乘法和求逆是主要计算量。几个优化点F矩阵大部分是零可以用稀疏矩阵或手动展开乘法S矩阵只有3x3或4x4求逆开销很小P矩阵的对称性可以利用只计算上三角如果更新频率低于预测频率可以把多次预测合并在树莓派上200Hz的预测加10Hz的位置更新CPU占用不到5%。即使是在STM32F4上优化后也能跑到100Hz以上。8. 一些个人体会ESKF这个算法我前前后后实现了三四遍每一遍都会发现之前没注意到的细节。第一遍照着论文抄跑起来姿态发散第二遍搞清楚了坐标系和符号能跑了但精度一般第三遍认真调了Q和R做了零偏在线估计效果才真正出来。最大的体会是ESKF的数学推导可以看论文但工程实现必须自己踩坑。论文里不会告诉你四元数注入要右乘不会告诉你P矩阵要强制对称不会告诉你GPS野值要用卡方检验过滤。这些细节才是决定滤波器能不能用的关键。另外不要迷信“最优估计”。ESKF在模型准确、噪声高斯的前提下确实是最优的但实际系统中IMU有刻度因数误差、轴间耦合、温度漂移这些都不在模型里。所以调参时不要追求理论最优而是根据实际数据反复试。我通常会用同一段数据跑不同参数对比姿态和位置的均方误差选一个在实际场景中表现最稳的参数组合。最后分享一个实用建议如果你的应用对姿态精度要求高但对位置要求不高可以把位置和速度从误差状态中去掉只保留姿态和零偏做成9维或6维的ESKF。状态维度降低后计算量更小姿态估计的收敛也更快。这个简化在云台稳定、AR/VR头显等场景中非常实用。