拓冰网站定制 
1. 从零吃透 ESKF为什么 IMU 状态估计非它不可搞过 IMU 姿态解算的人都有一个共同的痛原始陀螺仪积分漂得亲妈都不认识加速度计噪声大得跟菜市场一样磁力计在室内基本废掉。你拿这些数据直接做姿态融合要么响应慢得像蜗牛要么抖得跟帕金森一样。误差卡尔曼滤波器Error-State Kalman FilterESKF就是在这个背景下杀出来的一套工程上极其好用的方案。我第一次接触 ESKF 是在做一个小型无人机飞控的时候。当时用互补滤波凑合了一阵静态还行一旦机动稍微剧烈一点姿态估计就跟不上节奏了。后来换成 ESKF同样的 IMU 硬件姿态估计的滞后感和漂移量肉眼可见地下降了一个档次。这不是玄学是数学结构决定的。ESKF 的核心思想用一句话说清楚不直接估计状态本身而是估计状态的误差。听起来像是脱裤子放屁但恰恰是这个“多此一举”的设计让它在工程实现上比标准 EKF 优雅得多。为什么因为 IMU 的状态量里包含旋转姿态。旋转这个东西属于李群 SO(3)你不能像加减普通数字一样对它做线性运算。标准 EKF 直接对旋转做线性化会遇到万向锁、参数冗余、协方差矩阵奇异等一堆烂事。ESKF 的做法是用一个标称状态nominal state来积分 IMU 数据然后让卡尔曼滤波器去估计标称状态和真实状态之间的“小误差”。这个误差是定义在切空间上的是一个三维小量可以放心大胆地用线性卡尔曼滤波去处理。这个思路的好处是实打实的误差量永远是小量线性化精度极高不会出现大角度下线性化崩溃的问题。姿态误差可以用三维向量表示不需要四元数的四维冗余参数化协方差矩阵是 3x3 而不是 4x4计算量更小。标称状态积分和误差修正解耦代码结构清晰调试的时候可以分别定位是积分出了问题还是滤波更新出了问题。误差状态在每次更新后可以清零避免了长时间运行后数值漂移积累。这套方法最早由 Roumeliotis 等人在 1999 年前后系统提出后来在 Mourikis 和 Roumeliotis 的 MSCKFMulti-State Constraint Kalman Filter里被发扬光大VINS-Mono、OpenVINS 这些视觉惯性里程计方案里都能看到 ESKF 的影子。你如果去看 VINS-Mono 的代码会发现它的 IMU 预积分和状态估计部分核心就是 ESKF 的变体。所以不管你是做无人机、机器人、VR/AR 手柄、还是车载组合导航只要涉及到 IMU 和其他传感器轮速计、GPS、视觉、激光的融合ESKF 都是一个值得花时间啃下来的硬核技能。这篇文章我会从零开始把 ESKF 的数学推导、C 代码实现、调试技巧、踩坑经验全部倒出来让你看完就能自己动手写一个能跑的 ESKF。注意这篇文章假设你有基本的线性代数和概率论基础知道什么是协方差矩阵知道卡尔曼滤波的基本递推公式。如果你对这些还不熟建议先补一下卡尔曼滤波的基础知识再回来。2. ESKF 的数学骨架状态定义、运动模型与观测模型2.1 状态向量到底该放哪些量ESKF 的状态设计是整个系统的地基。放多了计算量大且容易引入不可观测的方向放少了该估计的东西估计不出来。对于典型的 9 轴 IMU三轴陀螺仪 三轴加速度计 三轴磁力计一个常用的 18 维误差状态向量长这样δx [δp, δv, δθ, δba, δbg, δg]其中δp3 维位置误差δv3 维速度误差δθ3 维姿态误差定义在切空间上的小旋转δba3 维加速度计零偏误差δbg3 维陀螺仪零偏误差δg3 维重力向量误差可选有些实现不估计重力总共 18 维。如果你不需要位置和速度比如只做姿态估计可以砍掉 δp 和 δv变成 12 维甚至 9 维。如果你还要估计重力就加上 δg。状态维度的选择完全取决于你的应用场景和传感器配置。我个人的经验是做室内机器人定位位置、速度、姿态、零偏全上重力可以不估因为室内重力方向基本不变提前标定好就行。做长时间户外导航重力最好加上因为温度变化和传感器老化会导致重力估计缓慢漂移。标称状态nominal state对应的量是p, v, q, ba, bg, g其中 q 是四元数表示的名义姿态。误差状态和标称状态的关系是p_true p δp v_true v δv q_true q ⊗ δq ba_true ba δba bg_true bg δbg g_true g δg这里 δq 是由 δθ 构造的小四元数δq ≈ [1, δθ/2]^T。注意姿态误差是“右乘”还是“左乘”取决于你的误差定义约定两种都有人用但一定要在代码里保持一致否则调试的时候你会怀疑人生。2.2 运动模型IMU 积分到底在积什么ESKF 的预测步骤本质上就是用 IMU 数据对名义状态做积分同时把误差状态的协方差往前推。名义状态的连续时间运动方程是p_dot v v_dot R(q) * (am - ba) g q_dot 0.5 * q ⊗ (ωm - bg) ba_dot 0 bg_dot 0 g_dot 0其中 am 是加速度计测量值ωm 是陀螺仪测量值R(q) 是四元数对应的旋转矩阵。零偏和重力建模为随机游走导数项为零但它们的协方差会随着时间增长。实际代码里不可能做连续积分必须离散化。最简单的是欧拉积分p p v * dt 0.5 * (R*(am-ba) g) * dt^2 v v (R*(am-ba) g) * dt q q ⊗ delta_q(ωm-bg, dt)其中 delta_q 是由角速度构造的增量四元数。欧拉积分在 dt 比较小比如 1ms 到 5ms的时候精度够用但如果你的 IMU 输出频率只有 100Hz 甚至更低建议用中值积分或者四阶龙格库塔精度会好很多。误差状态的离散时间转移矩阵 F 可以通过对连续时间误差微分方程做一阶近似得到。连续时间的误差动力学是δp_dot δv δv_dot -R * [am-ba]× * δθ - R * δba δg δθ_dot -[ωm-bg]× * δθ - δbg δba_dot 0 δbg_dot 0 δg_dot 0这里 [·]× 表示反对称矩阵。离散化后 F I A * dt其中 A 是上面这个连续时间误差动力学的系数矩阵。协方差预测就是标准的 P F * P * F^T QQ 是过程噪声协方差矩阵。过程噪声 Q 的设计是个技术活。陀螺仪和加速度计的噪声密度可以从数据手册里查到但实际使用中往往需要根据实测数据调整。我的做法是先按数据手册的值设一个初始值然后用静态数据跑一段看协方差收敛情况再微调。Q 设得太大滤波器会过度信任观测姿态抖动明显Q 设得太小滤波器响应迟钝机动时跟不上。2.3 观测模型不同传感器怎么接入ESKF 的观测更新步骤和标准卡尔曼滤波一样K P * H^T * (H * P * H^T R)^-1 δx K * (z - h(x)) P (I - K * H) * P然后关键的一步把误差状态注入到名义状态里再把误差状态清零。p p δp v v δv q q ⊗ δq(δθ) ba ba δba bg bg δbg g g δg δx 0不同的传感器对应不同的观测方程 h(x) 和观测矩阵 H。以加速度计为例它测量的是比力specific force在静止或低速运动时可以近似为重力方向的反方向。观测方程是z_acc R^T * (g) noise对应的 H 矩阵需要对误差状态求偏导。这部分推导比较繁琐但套路是固定的先写出观测方程然后对每个误差状态量求偏导填入 H 矩阵。磁力计观测类似测量的是地磁场方向。GPS 观测的是位置和速度。轮速计观测的是车体前进速度。视觉观测的是特征点重投影误差。每种传感器的 H 矩阵都不一样但框架是统一的。提示观测更新的顺序会影响滤波器的收敛速度。一般来说先更新精度高的传感器再更新精度低的。比如先更新磁力计修正航向再更新加速度计修正俯仰和横滚。3. C 代码实现从数据结构到完整滤波器3.1 工程结构设计与依赖选择写 ESKF 的 C 代码第一件事是选依赖库。我的建议是Eigen矩阵运算必备没有它写 C 矩阵操作就是自虐。Eigen 是 header-only 的直接 include 就能用不需要编译链接。Sophus可选李群李代数库如果你对 SO(3) 的 exp/log 映射不熟用 Sophus 可以省很多事。但我个人建议自己手写四元数操作一来代码量不大二来方便调试。Ceres或G2O可选如果你要做优化这两个库很有用。但纯 ESKF 不需要它们。工程目录结构我一般这样组织eskf_project/ ├── include/ │ ├── eskf.h │ ├── imu_data.h │ └── math_utils.h ├── src/ │ ├── eskf.cpp │ ├── math_utils.cpp │ └── main.cpp ├── test/ │ └── test_eskf.cpp └── CMakeLists.txt核心类 ESKF 的设计class ESKF { public: ESKF(); void init(const ImuData imu, const Vec3 init_ba, const Vec3 init_bg); void predict(const ImuData imu, double dt); void updateAcc(const Vec3 acc_meas); void updateMag(const Vec3 mag_meas); void updateGps(const Vec3 pos_meas, const Vec3 vel_meas); // 获取状态 Vec3 getPosition() const { return p_; } Vec3 getVelocity() const { return v_; } Quat getOrientation() const { return q_; } Vec3 getAccBias() const { return ba_; } Vec3 getGyroBias() const { return bg_; } private: // 名义状态 Vec3 p_, v_, ba_, bg_, g_; Quat q_; // 误差状态协方差 Matrixdouble, 18, 18 P_; // 过程噪声 Matrixdouble, 18, 18 Q_; // 观测噪声 Matrix3d R_acc_, R_mag_, R_gps_pos_, R_gps_vel_; // 内部方法 void injectErrorState(const Matrixdouble, 18, 1 dx); Matrixdouble, 18, 18 computeF(const ImuData imu, double dt) const; Matrixdouble, 18, 18 computeQ(double dt) const; };这个设计把名义状态和协方差分开管理predict 和 update 各司其职逻辑清晰。3.2 四元数运算与误差状态注入四元数运算是 ESKF 里最容易写错的地方。我见过太多人因为四元数乘法顺序搞反、共轭搞错、归一化忘记做导致滤波器发散。这里把关键操作列出来// 四元数乘法 Quat quatMultiply(const Quat q1, const Quat q2) { Quat result; result.w() q1.w()*q2.w() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z(); result.x() q1.w()*q2.x() q1.x()*q2.w() q1.y()*q2.z() - q1.z()*q2.y(); result.y() q1.w()*q2.y() - q1.x()*q2.z() q1.y()*q2.w() q1.z()*q2.x(); result.z() q1.w()*q2.z() q1.x()*q2.y() - q1.y()*q2.x() q1.z()*q2.w(); return result; } // 由旋转向量构造四元数小角度近似 Quat quatFromSmallAngle(const Vec3 theta) { double angle theta.norm(); if (angle 1e-8) { return Quat(1.0, theta.x()/2, theta.y()/2, theta.z()/2).normalized(); } Vec3 axis theta / angle; return Quat(Eigen::AngleAxisd(angle, axis)); } // 四元数转旋转矩阵 Matrix3d quatToRotation(const Quat q) { return q.toRotationMatrix(); }误差状态注入是 ESKF 区别于标准 EKF 的关键步骤void ESKF::injectErrorState(const Matrixdouble, 18, 1 dx) { // 位置、速度、零偏、重力直接加 p_ dx.segment3(0); v_ dx.segment3(3); ba_ dx.segment3(9); bg_ dx.segment3(12); g_ dx.segment3(15); // 姿态用四元数乘法 Vec3 dtheta dx.segment3(6); Quat dq quatFromSmallAngle(dtheta); q_ quatMultiply(q_, dq); q_.normalize(); }注意姿态误差是右乘还是左乘取决于你的误差定义。我上面用的是右乘即 q_true q ⊗ δq。如果你用的是左乘那注入的时候就要改成 q_ dq ⊗ q_同时 H 矩阵和 F 矩阵里所有涉及姿态误差的项都要相应调整。这个一致性极其重要搞错了滤波器直接发散。3.3 预测步骤的完整实现预测步骤做两件事积分名义状态传播协方差。void ESKF::predict(const ImuData imu, double dt) { // 去偏 Vec3 acc imu.acc - ba_; Vec3 gyro imu.gyro - bg_; // 旋转矩阵 Matrix3d R quatToRotation(q_); // 积分名义状态欧拉积分 Vec3 acc_world R * acc g_; p_ v_ * dt 0.5 * acc_world * dt * dt; v_ acc_world * dt; // 四元数积分 Vec3 dtheta gyro * dt; Quat dq quatFromSmallAngle(dtheta); q_ quatMultiply(q_, dq); q_.normalize(); // 计算状态转移矩阵 F Matrixdouble, 18, 18 F Matrixdouble, 18, 18::Identity(); // δp 对 δv 的偏导 F.block3,3(0, 3) Matrix3d::Identity() * dt; // δv 对 δθ 的偏导 Matrix3d acc_skew skewSymmetric(acc); F.block3,3(3, 6) -R * acc_skew * dt; // δv 对 δba 的偏导 F.block3,3(3, 9) -R * dt; // δv 对 δg 的偏导 F.block3,3(3, 15) Matrix3d::Identity() * dt; // δθ 对 δθ 的偏导 Matrix3d gyro_skew skewSymmetric(gyro); F.block3,3(6, 6) Matrix3d::Identity() - gyro_skew * dt; // δθ 对 δbg 的偏导 F.block3,3(6, 12) -Matrix3d::Identity() * dt; // 协方差传播 P_ F * P_ * F.transpose() computeQ(dt); }这里有几个细节值得展开说第一反对称矩阵的定义。skewSymmetric(v) 返回的是[ 0 -vz vy ] [ vz 0 -vx ] [-vy vx 0 ]这个矩阵满足 skewSymmetric(a) * b a × b。在推导 F 矩阵的时候符号特别容易搞错建议推导完用数值差分验证一下。第二过程噪声 Q 的构造。Q 矩阵通常是块对角的Matrixdouble, 18, 18 ESKF::computeQ(double dt) const { Matrixdouble, 18, 18 Q Matrixdouble, 18, 18::Zero(); // 速度噪声来自加速度计噪声 double acc_noise 0.02; // m/s^2 / sqrt(Hz) Q.block3,3(3, 3) Matrix3d::Identity() * acc_noise * acc_noise * dt * dt; // 姿态噪声来自陀螺仪噪声 double gyro_noise 0.001; // rad/s / sqrt(Hz) Q.block3,3(6, 6) Matrix3d::Identity() * gyro_noise * gyro_noise * dt * dt; // 加速度计零偏随机游走 double acc_bias_noise 0.0001; Q.block3,3(9, 9) Matrix3d::Identity() * acc_bias_noise * acc_bias_noise * dt; // 陀螺仪零偏随机游走 double gyro_bias_noise 0.00001; Q.block3,3(12, 12) Matrix3d::Identity() * gyro_bias_noise * gyro_bias_noise * dt; // 重力随机游走通常设得很小 double gravity_noise 1e-6; Q.block3,3(15, 15) Matrix3d::Identity() * gravity_noise * gravity_noise * dt; return Q; }这些噪声参数不是拍脑袋来的。加速度计噪声密度可以从数据手册查到比如 MPU6050 的加速度计噪声密度大约是 400 μg/√Hz换算成 m/s²/√Hz 大约是 0.004。但实际使用中由于振动、温度变化等因素有效噪声往往比手册值大好几倍。我的经验是先按手册值设然后根据静态数据的 Allan 方差分析结果调整。第三积分方法的选择。欧拉积分在 dt1ms 时误差很小但如果你的 IMU 是 100Hz 输出dt10ms欧拉积分的误差就不能忽略了。这时候可以用中值积分// 中值积分 Vec3 acc_mid 0.5 * (R_prev * (acc_prev - ba_) R_curr * (acc_curr - ba_)) g_; p_ v_ * dt 0.5 * acc_mid * dt * dt; v_ acc_mid * dt;中值积分需要保存上一时刻的 IMU 数据和旋转矩阵代码稍微复杂一点但精度提升明显。3.4 观测更新加速度计、磁力计与 GPS观测更新的代码结构是统一的计算残差、计算 H 矩阵、计算卡尔曼增益、更新误差状态、注入并清零。以加速度计更新为例。加速度计测量的是比力在静止时等于重力的反方向在机体系下的表示void ESKF::updateAcc(const Vec3 acc_meas) { // 预测的加速度计观测 Matrix3d R quatToRotation(q_); Vec3 acc_pred R.transpose() * g_; // 残差 Vec3 residual acc_meas - acc_pred; // 观测矩阵 H (3x18) Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); // 对姿态误差的偏导 H.block3,3(0, 6) R.transpose() * skewSymmetric(g_); // 对重力误差的偏导 H.block3,3(0, 15) -R.transpose(); // 卡尔曼增益 Matrix3d S H * P_ * H.transpose() R_acc_; Matrixdouble, 18, 3 K P_ * H.transpose() * S.inverse(); // 更新误差状态 Matrixdouble, 18, 1 dx K * residual; // 注入并清零 injectErrorState(dx); // 协方差更新Joseph 形式数值更稳定 Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K * H) * P_ * (I - K * H).transpose() K * R_acc_ * K.transpose(); }这里用了 Joseph 形式的协方差更新比简单的 (I-KH)P 数值稳定性更好。特别是在观测维度小于状态维度的时候Joseph 形式能保证协方差矩阵始终对称正定。磁力计更新的逻辑类似但观测方程不同。磁力计测量的是地磁场在机体系下的方向void ESKF::updateMag(const Vec3 mag_meas) { Matrix3d R quatToRotation(q_); Vec3 mag_pred R.transpose() * mag_world_; Vec3 residual mag_meas - mag_pred; Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); H.block3,3(0, 6) R.transpose() * skewSymmetric(mag_world_); Matrix3d S H * P_ * H.transpose() R_mag_; Matrixdouble, 18, 3 K P_ * H.transpose() * S.inverse(); Matrixdouble, 18, 1 dx K * residual; injectErrorState(dx); Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K * H) * P_ * (I - K * H).transpose() K * R_mag_ * K.transpose(); }GPS 更新的是位置和速度观测矩阵更简单void ESKF::updateGps(const Vec3 pos_meas, const Vec3 vel_meas) { // 位置更新 Vec3 pos_residual pos_meas - p_; Matrixdouble, 3, 18 H_pos Matrixdouble, 3, 18::Zero(); H_pos.block3,3(0, 0) Matrix3d::Identity(); Matrix3d S_pos H_pos * P_ * H_pos.transpose() R_gps_pos_; Matrixdouble, 18, 3 K_pos P_ * H_pos.transpose() * S_pos.inverse(); Matrixdouble, 18, 1 dx_pos K_pos * pos_residual; injectErrorState(dx_pos); Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K_pos * H_pos) * P_ * (I - K_pos * H_pos).transpose() K_pos * R_gps_pos_ * K_pos.transpose(); // 速度更新 Vec3 vel_residual vel_meas - v_; Matrixdouble, 3, 18 H_vel Matrixdouble, 3, 18::Zero(); H_vel.block3,3(0, 3) Matrix3d::Identity(); Matrix3d S_vel H_vel * P_ * H_vel.transpose() R_gps_vel_; Matrixdouble, 18, 3 K_vel P_ * H_vel.transpose() * S_vel.inverse(); Matrixdouble, 18, 1 dx_vel K_vel * vel_residual; injectErrorState(dx_vel); P_ (I - K_vel * H_vel) * P_ * (I - K_vel * H_vel).transpose() K_vel * R_gps_vel_ * K_vel.transpose(); }注意每次观测更新后都要重新计算 H 矩阵因为 H 矩阵依赖于当前的名义状态。如果你在多次更新之间不重新计算 H滤波器的收敛性会变差。4. 调试与实战那些文档里不会告诉你的坑4.1 初始化滤波器能不能收敛一半看初始化ESKF 的初始化极其关键。如果初始姿态、初始零偏、初始协方差设得离谱滤波器要么发散要么收敛得极慢。我的初始化流程是这样的第一步静止采集。让 IMU 静止放置至少 2 秒采集 200 到 500 个样本。计算加速度计和陀螺仪的均值Vec3 acc_mean Vec3::Zero(); Vec3 gyro_mean Vec3::Zero(); for (const auto sample : static_samples) { acc_mean sample.acc; gyro_mean sample.gyro; } acc_mean / static_samples.size(); gyro_mean / static_samples.size();第二步估计初始姿态。用加速度计均值估计俯仰和横滚用磁力计估计航向// 俯仰和横滚 double roll atan2(acc_mean.y(), acc_mean.z()); double pitch atan2(-acc_mean.x(), sqrt(acc_mean.y()*acc_mean.y() acc_mean.z()*acc_mean.z())); // 航向需要磁力计 Vec3 mag_mean ...; // 磁力计均值 double yaw atan2(-mag_mean.y(), mag_mean.x());第三步估计初始零偏。陀螺仪零偏直接用静止时的均值。加速度计零偏需要扣除重力分量Vec3 gravity_body quatToRotation(q_init).transpose() * Vec3(0, 0, -9.81); Vec3 ba_init acc_mean - gravity_body;第四步设置初始协方差。位置和速度的初始协方差设小一点比如 0.01姿态的初始协方差根据加速度计和磁力计的噪声水平设通常 0.1 到 1.0 弧度零偏的初始协方差设大一点比如 0.1因为零偏的不确定性最大。我踩过的一个坑是初始协方差设得太小导致滤波器过度自信后续观测修正不进去。比如姿态初始协方差设成 0.001结果实际初始姿态误差有 0.1 弧度滤波器要花很长时间才能修正过来。后来我把姿态初始协方差统一设成 0.5收敛速度明显改善。4.2 数值稳定性协方差矩阵为什么不对称了ESKF 跑一段时间后协方差矩阵 P 可能会失去对称性甚至出现负特征值。这是数值误差积累的结果。解决方法有几个方法一强制对称化。每次更新后做 P 0.5 * (P P^T)。这个操作简单粗暴但有效。方法二用 Joseph 形式更新。前面代码里已经用了能显著改善对称性。方法三定期做特征值分解把负特征值截断到一个小正数。这个操作计算量大一般只在调试阶段用。方法四用平方根滤波。这是最彻底的方法但实现复杂度高一般工程上没必要。我的经验是Joseph 形式 强制对称化能解决 95% 的数值稳定性问题。如果还不行检查一下 Q 矩阵是不是设得太小导致 P 矩阵条件数过大。4.3 观测噪声调参R 矩阵怎么设才合理观测噪声矩阵 R 的调参是 ESKF 最玄学的部分。设得太大滤波器不信任观测姿态漂移设得太小滤波器过度信任观测噪声放大。我的调参流程静态测试IMU 静止放置只开预测不开更新观察姿态漂移速度。如果 10 秒内漂移超过 1 度说明陀螺仪零偏没标定好。开加速度计更新观察俯仰和横滚是否稳定。如果抖动明显增大 R_acc如果响应迟钝减小 R_acc。开磁力计更新观察航向是否稳定。磁力计受环境干扰大R_mag 通常要比 R_acc 大一个数量级。动态测试手持 IMU 做各种机动观察姿态跟踪是否跟得上。如果滞后明显减小 R如果噪声放大增大 R。一个实用的技巧是用 Allan 方差分析结果来设 R 的初始值。Allan 方差能给出传感器的噪声密度和零偏不稳定性直接对应到 R 矩阵的对角线元素。4.4 常见问题速查表问题现象可能原因排查方法解决方案姿态缓慢漂移陀螺仪零偏未标定静态下观察零偏估计是否收敛重新标定零偏增大 Q 中零偏噪声姿态抖动明显R 矩阵设得太小观察残差序列是否白噪声增大 R_acc 和 R_mag机动时姿态滞后Q 矩阵设得太小观察协方差是否过小增大 Q 中姿态噪声滤波器发散F 矩阵或 H 矩阵符号错误用数值差分验证雅可比检查反对称矩阵符号协方差矩阵不对称数值误差积累检查特征值是否有负值Joseph 更新 强制对称化航向缓慢旋转磁力计受干扰对比磁力计和陀螺仪航向增大 R_mag或暂时关闭磁力计位置估计漂移加速度计零偏未估计观察 ba 估计是否收敛增大 Q 中零偏噪声检查可观测性4.5 实测数据与性能评估我在一个室内机器人平台上实测过这套 ESKF。IMU 是 MPU6050输出频率 200Hz磁力计是 HMC5883L输出频率 50Hz。测试场景是机器人原地旋转和直线行走。静态测试结果姿态估计的标准差在俯仰和横滚方向小于 0.1 度航向方向小于 0.5 度磁力计受室内钢筋干扰。动态测试结果机器人以 90 度/秒旋转时姿态跟踪误差小于 1 度滞后小于 20ms。这个性能对于室内机器人导航已经够用了。如果你要更高精度可以考虑用工业级 IMU比如 ADIS16470噪声密度低一个数量级姿态精度能到 0.01 度级别。5. 进阶扩展从 ESKF 到多传感器融合5.1 视觉惯性融合ESKF 在 VIO 里的角色ESKF 在视觉惯性里程计VIO里的应用非常广泛。VINS-Mono 的核心就是一个 ESKF 的变体它把视觉特征点的重投影误差作为观测更新 IMU 的误差状态。视觉观测的 H 矩阵计算比加速度计和磁力计复杂得多因为涉及到相机投影模型和特征点位置。但框架是一样的写出观测方程对误差状态求偏导填入 H 矩阵。如果你要做 VIO我建议先跑通纯 IMU 的 ESKF再加入视觉观测。这样调试的时候可以分层定位问题。5.2 激光雷达与 IMU 融合LIO 中的 ESKF激光雷达和 IMU 的融合LIO是另一个热门方向。LOAM、LIO-SAM、FAST-LIO 这些方案里ESKF 或者其变体都是核心。FAST-LIO 用的是迭代扩展卡尔曼滤波IEKF本质上是 ESKF 的迭代版本。它在每次观测更新时多次迭代重新线性化观测方程精度比标准 ESKF 更高。如果你要做 LIOESKF 的状态向量需要加上外参IMU 到激光雷达的旋转和平移。外参可以在线估计也可以提前标定。在线估计的好处是能适应安装误差坏处是增加了状态维度计算量变大。5.3 轮速计与 IMU 融合低成本组合导航如果你做的是地面机器人轮速计是一个便宜又好用的传感器。轮速计观测的是车体前进速度观测方程很简单// 轮速计观测车体前进速度 double v_wheel ...; // 轮速计测量值 Vec3 v_body Vec3(v_wheel, 0, 0); // 假设车体坐标系 x 轴向前 Vec3 v_world quatToRotation(q_) * v_body; // 观测矩阵 Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); H.block3,3(0, 3) Matrix3d::Identity();轮速计和 IMU 融合能显著抑制位置漂移特别是在 GPS 信号不好的室内环境。我做过一个测试纯 IMU 积分 10 秒位置漂移 2 米加上轮速计后漂移降到 0.3 米。5.4 代码优化让 ESKF 跑得更快ESKF 的计算瓶颈主要在矩阵乘法和求逆。18 维状态的协方差矩阵是 18x18求逆是 O(18^3)在嵌入式平台上可能吃不消。优化方法利用稀疏性F 矩阵和 H 矩阵都是稀疏的用稀疏矩阵运算能省不少时间。降低状态维度如果不需要估计重力砍掉 δg状态降到 15 维。用固定大小的 Eigen 矩阵Matrixdouble, 18, 18 比 MatrixXd 快很多因为编译期就能确定大小。避免动态内存分配在 predict 和 update 里不要用 new 或 malloc所有矩阵都在栈上分配。我在树莓派 4B 上实测18 维 ESKF 单次 predictupdate 耗时约 0.5ms200Hz 跑完全没问题。如果在 STM32 上跑可能需要降到 12 维状态并且用单精度浮点。6. 一些掏心窝子的实操心得写 ESKF 代码这几年踩过的坑比写过的代码还多。分享几条我觉得最有价值的经验第一条先仿真后实测。不要一上来就拿真实 IMU 数据跑。先用 MATLAB 或 Python 生成一段仿真 IMU 数据已知真实轨迹 加噪声在仿真数据上把算法调通再上真实数据。仿真数据的好处是你知道真值能定量评估误差。第二条可视化调试。把姿态、速度、零偏、协方差对角线都画出来。滤波器发散之前协方差通常会有异常变化。比如协方差突然变小说明滤波器过度自信了协方差突然变大说明观测和预测严重不一致。第三条单元测试。四元数乘法、旋转矩阵构造、反对称矩阵、误差状态注入这些基础函数一定要写单元测试。我见过太多人因为四元数乘法写错调了一周才发现问题。第四条记录数据。每次测试都把 IMU 原始数据、ESKF 输出、真值如果有保存下来。调参的时候可以离线回放不用反复跑实验。第五条别迷信理论值。数据手册上的噪声密度是理想条件下的实际使用中振动、温度、电磁干扰都会让噪声变大。R 和 Q 矩阵的最终值一定是调出来的不是算出来的。第六条注意坐标系约定。IMU 的机体系定义、世界系定义、重力方向定义这些一定要在代码注释里写清楚。我见过一个项目两个人分别写预测和更新结果一个用 NED 系一个用 ENU 系滤波器直接爆炸。第七条零偏估计要慢。零偏的随机游走噪声不要设得太大否则零偏估计会跟着观测噪声一起抖。零偏是一个缓慢变化的量让它慢慢收敛就好。第八条磁力计要慎用。室内环境下磁力计受钢筋、电器干扰严重航向估计可能比陀螺仪积分还差。我的做法是先判断磁力计数据的可靠性比如检查磁场强度是否在合理范围内可靠时才用来更新航向。第九条GPS 更新要检查有效性。GPS 在隧道、室内、城市峡谷里会给出离谱的位置直接用来更新会把滤波器带偏。更新前检查 HDOP、卫星数、位置跳变是否超过阈值。第十条代码要能回放。把 ESKF 设计成可以离线回放数据的形式输入是 IMU 数据流和观测数据流输出是状态估计序列。这样调参的时候不用连硬件效率高很多。这套 ESKF 代码我后来用在了好几个项目上从无人机到地面机器人从纯 IMU 到多传感器融合框架基本没大改只是调整了状态维度和观测模型。ESKF 的魅力就在于它的结构足够清晰扩展起来足够灵活。你把这篇里的代码敲一遍跑通再根据自己的传感器配置改一改基本就能应付大部分 IMU 状态估计的需求了。 从零吃透ESKF：IMU状态估计的工程实践与C++实现
1. 从零吃透 ESKF为什么 IMU 状态估计非它不可搞过 IMU 姿态解算的人都有一个共同的痛原始陀螺仪积分漂得亲妈都不认识加速度计噪声大得跟菜市场一样磁力计在室内基本废掉。你拿这些数据直接做姿态融合要么响应慢得像蜗牛要么抖得跟帕金森一样。误差卡尔曼滤波器Error-State Kalman FilterESKF就是在这个背景下杀出来的一套工程上极其好用的方案。我第一次接触 ESKF 是在做一个小型无人机飞控的时候。当时用互补滤波凑合了一阵静态还行一旦机动稍微剧烈一点姿态估计就跟不上节奏了。后来换成 ESKF同样的 IMU 硬件姿态估计的滞后感和漂移量肉眼可见地下降了一个档次。这不是玄学是数学结构决定的。ESKF 的核心思想用一句话说清楚不直接估计状态本身而是估计状态的误差。听起来像是脱裤子放屁但恰恰是这个“多此一举”的设计让它在工程实现上比标准 EKF 优雅得多。为什么因为 IMU 的状态量里包含旋转姿态。旋转这个东西属于李群 SO(3)你不能像加减普通数字一样对它做线性运算。标准 EKF 直接对旋转做线性化会遇到万向锁、参数冗余、协方差矩阵奇异等一堆烂事。ESKF 的做法是用一个标称状态nominal state来积分 IMU 数据然后让卡尔曼滤波器去估计标称状态和真实状态之间的“小误差”。这个误差是定义在切空间上的是一个三维小量可以放心大胆地用线性卡尔曼滤波去处理。这个思路的好处是实打实的误差量永远是小量线性化精度极高不会出现大角度下线性化崩溃的问题。姿态误差可以用三维向量表示不需要四元数的四维冗余参数化协方差矩阵是 3x3 而不是 4x4计算量更小。标称状态积分和误差修正解耦代码结构清晰调试的时候可以分别定位是积分出了问题还是滤波更新出了问题。误差状态在每次更新后可以清零避免了长时间运行后数值漂移积累。这套方法最早由 Roumeliotis 等人在 1999 年前后系统提出后来在 Mourikis 和 Roumeliotis 的 MSCKFMulti-State Constraint Kalman Filter里被发扬光大VINS-Mono、OpenVINS 这些视觉惯性里程计方案里都能看到 ESKF 的影子。你如果去看 VINS-Mono 的代码会发现它的 IMU 预积分和状态估计部分核心就是 ESKF 的变体。所以不管你是做无人机、机器人、VR/AR 手柄、还是车载组合导航只要涉及到 IMU 和其他传感器轮速计、GPS、视觉、激光的融合ESKF 都是一个值得花时间啃下来的硬核技能。这篇文章我会从零开始把 ESKF 的数学推导、C 代码实现、调试技巧、踩坑经验全部倒出来让你看完就能自己动手写一个能跑的 ESKF。注意这篇文章假设你有基本的线性代数和概率论基础知道什么是协方差矩阵知道卡尔曼滤波的基本递推公式。如果你对这些还不熟建议先补一下卡尔曼滤波的基础知识再回来。2. ESKF 的数学骨架状态定义、运动模型与观测模型2.1 状态向量到底该放哪些量ESKF 的状态设计是整个系统的地基。放多了计算量大且容易引入不可观测的方向放少了该估计的东西估计不出来。对于典型的 9 轴 IMU三轴陀螺仪 三轴加速度计 三轴磁力计一个常用的 18 维误差状态向量长这样δx [δp, δv, δθ, δba, δbg, δg]其中δp3 维位置误差δv3 维速度误差δθ3 维姿态误差定义在切空间上的小旋转δba3 维加速度计零偏误差δbg3 维陀螺仪零偏误差δg3 维重力向量误差可选有些实现不估计重力总共 18 维。如果你不需要位置和速度比如只做姿态估计可以砍掉 δp 和 δv变成 12 维甚至 9 维。如果你还要估计重力就加上 δg。状态维度的选择完全取决于你的应用场景和传感器配置。我个人的经验是做室内机器人定位位置、速度、姿态、零偏全上重力可以不估因为室内重力方向基本不变提前标定好就行。做长时间户外导航重力最好加上因为温度变化和传感器老化会导致重力估计缓慢漂移。标称状态nominal state对应的量是p, v, q, ba, bg, g其中 q 是四元数表示的名义姿态。误差状态和标称状态的关系是p_true p δp v_true v δv q_true q ⊗ δq ba_true ba δba bg_true bg δbg g_true g δg这里 δq 是由 δθ 构造的小四元数δq ≈ [1, δθ/2]^T。注意姿态误差是“右乘”还是“左乘”取决于你的误差定义约定两种都有人用但一定要在代码里保持一致否则调试的时候你会怀疑人生。2.2 运动模型IMU 积分到底在积什么ESKF 的预测步骤本质上就是用 IMU 数据对名义状态做积分同时把误差状态的协方差往前推。名义状态的连续时间运动方程是p_dot v v_dot R(q) * (am - ba) g q_dot 0.5 * q ⊗ (ωm - bg) ba_dot 0 bg_dot 0 g_dot 0其中 am 是加速度计测量值ωm 是陀螺仪测量值R(q) 是四元数对应的旋转矩阵。零偏和重力建模为随机游走导数项为零但它们的协方差会随着时间增长。实际代码里不可能做连续积分必须离散化。最简单的是欧拉积分p p v * dt 0.5 * (R*(am-ba) g) * dt^2 v v (R*(am-ba) g) * dt q q ⊗ delta_q(ωm-bg, dt)其中 delta_q 是由角速度构造的增量四元数。欧拉积分在 dt 比较小比如 1ms 到 5ms的时候精度够用但如果你的 IMU 输出频率只有 100Hz 甚至更低建议用中值积分或者四阶龙格库塔精度会好很多。误差状态的离散时间转移矩阵 F 可以通过对连续时间误差微分方程做一阶近似得到。连续时间的误差动力学是δp_dot δv δv_dot -R * [am-ba]× * δθ - R * δba δg δθ_dot -[ωm-bg]× * δθ - δbg δba_dot 0 δbg_dot 0 δg_dot 0这里 [·]× 表示反对称矩阵。离散化后 F I A * dt其中 A 是上面这个连续时间误差动力学的系数矩阵。协方差预测就是标准的 P F * P * F^T QQ 是过程噪声协方差矩阵。过程噪声 Q 的设计是个技术活。陀螺仪和加速度计的噪声密度可以从数据手册里查到但实际使用中往往需要根据实测数据调整。我的做法是先按数据手册的值设一个初始值然后用静态数据跑一段看协方差收敛情况再微调。Q 设得太大滤波器会过度信任观测姿态抖动明显Q 设得太小滤波器响应迟钝机动时跟不上。2.3 观测模型不同传感器怎么接入ESKF 的观测更新步骤和标准卡尔曼滤波一样K P * H^T * (H * P * H^T R)^-1 δx K * (z - h(x)) P (I - K * H) * P然后关键的一步把误差状态注入到名义状态里再把误差状态清零。p p δp v v δv q q ⊗ δq(δθ) ba ba δba bg bg δbg g g δg δx 0不同的传感器对应不同的观测方程 h(x) 和观测矩阵 H。以加速度计为例它测量的是比力specific force在静止或低速运动时可以近似为重力方向的反方向。观测方程是z_acc R^T * (g) noise对应的 H 矩阵需要对误差状态求偏导。这部分推导比较繁琐但套路是固定的先写出观测方程然后对每个误差状态量求偏导填入 H 矩阵。磁力计观测类似测量的是地磁场方向。GPS 观测的是位置和速度。轮速计观测的是车体前进速度。视觉观测的是特征点重投影误差。每种传感器的 H 矩阵都不一样但框架是统一的。提示观测更新的顺序会影响滤波器的收敛速度。一般来说先更新精度高的传感器再更新精度低的。比如先更新磁力计修正航向再更新加速度计修正俯仰和横滚。3. C 代码实现从数据结构到完整滤波器3.1 工程结构设计与依赖选择写 ESKF 的 C 代码第一件事是选依赖库。我的建议是Eigen矩阵运算必备没有它写 C 矩阵操作就是自虐。Eigen 是 header-only 的直接 include 就能用不需要编译链接。Sophus可选李群李代数库如果你对 SO(3) 的 exp/log 映射不熟用 Sophus 可以省很多事。但我个人建议自己手写四元数操作一来代码量不大二来方便调试。Ceres或G2O可选如果你要做优化这两个库很有用。但纯 ESKF 不需要它们。工程目录结构我一般这样组织eskf_project/ ├── include/ │ ├── eskf.h │ ├── imu_data.h │ └── math_utils.h ├── src/ │ ├── eskf.cpp │ ├── math_utils.cpp │ └── main.cpp ├── test/ │ └── test_eskf.cpp └── CMakeLists.txt核心类 ESKF 的设计class ESKF { public: ESKF(); void init(const ImuData imu, const Vec3 init_ba, const Vec3 init_bg); void predict(const ImuData imu, double dt); void updateAcc(const Vec3 acc_meas); void updateMag(const Vec3 mag_meas); void updateGps(const Vec3 pos_meas, const Vec3 vel_meas); // 获取状态 Vec3 getPosition() const { return p_; } Vec3 getVelocity() const { return v_; } Quat getOrientation() const { return q_; } Vec3 getAccBias() const { return ba_; } Vec3 getGyroBias() const { return bg_; } private: // 名义状态 Vec3 p_, v_, ba_, bg_, g_; Quat q_; // 误差状态协方差 Matrixdouble, 18, 18 P_; // 过程噪声 Matrixdouble, 18, 18 Q_; // 观测噪声 Matrix3d R_acc_, R_mag_, R_gps_pos_, R_gps_vel_; // 内部方法 void injectErrorState(const Matrixdouble, 18, 1 dx); Matrixdouble, 18, 18 computeF(const ImuData imu, double dt) const; Matrixdouble, 18, 18 computeQ(double dt) const; };这个设计把名义状态和协方差分开管理predict 和 update 各司其职逻辑清晰。3.2 四元数运算与误差状态注入四元数运算是 ESKF 里最容易写错的地方。我见过太多人因为四元数乘法顺序搞反、共轭搞错、归一化忘记做导致滤波器发散。这里把关键操作列出来// 四元数乘法 Quat quatMultiply(const Quat q1, const Quat q2) { Quat result; result.w() q1.w()*q2.w() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z(); result.x() q1.w()*q2.x() q1.x()*q2.w() q1.y()*q2.z() - q1.z()*q2.y(); result.y() q1.w()*q2.y() - q1.x()*q2.z() q1.y()*q2.w() q1.z()*q2.x(); result.z() q1.w()*q2.z() q1.x()*q2.y() - q1.y()*q2.x() q1.z()*q2.w(); return result; } // 由旋转向量构造四元数小角度近似 Quat quatFromSmallAngle(const Vec3 theta) { double angle theta.norm(); if (angle 1e-8) { return Quat(1.0, theta.x()/2, theta.y()/2, theta.z()/2).normalized(); } Vec3 axis theta / angle; return Quat(Eigen::AngleAxisd(angle, axis)); } // 四元数转旋转矩阵 Matrix3d quatToRotation(const Quat q) { return q.toRotationMatrix(); }误差状态注入是 ESKF 区别于标准 EKF 的关键步骤void ESKF::injectErrorState(const Matrixdouble, 18, 1 dx) { // 位置、速度、零偏、重力直接加 p_ dx.segment3(0); v_ dx.segment3(3); ba_ dx.segment3(9); bg_ dx.segment3(12); g_ dx.segment3(15); // 姿态用四元数乘法 Vec3 dtheta dx.segment3(6); Quat dq quatFromSmallAngle(dtheta); q_ quatMultiply(q_, dq); q_.normalize(); }注意姿态误差是右乘还是左乘取决于你的误差定义。我上面用的是右乘即 q_true q ⊗ δq。如果你用的是左乘那注入的时候就要改成 q_ dq ⊗ q_同时 H 矩阵和 F 矩阵里所有涉及姿态误差的项都要相应调整。这个一致性极其重要搞错了滤波器直接发散。3.3 预测步骤的完整实现预测步骤做两件事积分名义状态传播协方差。void ESKF::predict(const ImuData imu, double dt) { // 去偏 Vec3 acc imu.acc - ba_; Vec3 gyro imu.gyro - bg_; // 旋转矩阵 Matrix3d R quatToRotation(q_); // 积分名义状态欧拉积分 Vec3 acc_world R * acc g_; p_ v_ * dt 0.5 * acc_world * dt * dt; v_ acc_world * dt; // 四元数积分 Vec3 dtheta gyro * dt; Quat dq quatFromSmallAngle(dtheta); q_ quatMultiply(q_, dq); q_.normalize(); // 计算状态转移矩阵 F Matrixdouble, 18, 18 F Matrixdouble, 18, 18::Identity(); // δp 对 δv 的偏导 F.block3,3(0, 3) Matrix3d::Identity() * dt; // δv 对 δθ 的偏导 Matrix3d acc_skew skewSymmetric(acc); F.block3,3(3, 6) -R * acc_skew * dt; // δv 对 δba 的偏导 F.block3,3(3, 9) -R * dt; // δv 对 δg 的偏导 F.block3,3(3, 15) Matrix3d::Identity() * dt; // δθ 对 δθ 的偏导 Matrix3d gyro_skew skewSymmetric(gyro); F.block3,3(6, 6) Matrix3d::Identity() - gyro_skew * dt; // δθ 对 δbg 的偏导 F.block3,3(6, 12) -Matrix3d::Identity() * dt; // 协方差传播 P_ F * P_ * F.transpose() computeQ(dt); }这里有几个细节值得展开说第一反对称矩阵的定义。skewSymmetric(v) 返回的是[ 0 -vz vy ] [ vz 0 -vx ] [-vy vx 0 ]这个矩阵满足 skewSymmetric(a) * b a × b。在推导 F 矩阵的时候符号特别容易搞错建议推导完用数值差分验证一下。第二过程噪声 Q 的构造。Q 矩阵通常是块对角的Matrixdouble, 18, 18 ESKF::computeQ(double dt) const { Matrixdouble, 18, 18 Q Matrixdouble, 18, 18::Zero(); // 速度噪声来自加速度计噪声 double acc_noise 0.02; // m/s^2 / sqrt(Hz) Q.block3,3(3, 3) Matrix3d::Identity() * acc_noise * acc_noise * dt * dt; // 姿态噪声来自陀螺仪噪声 double gyro_noise 0.001; // rad/s / sqrt(Hz) Q.block3,3(6, 6) Matrix3d::Identity() * gyro_noise * gyro_noise * dt * dt; // 加速度计零偏随机游走 double acc_bias_noise 0.0001; Q.block3,3(9, 9) Matrix3d::Identity() * acc_bias_noise * acc_bias_noise * dt; // 陀螺仪零偏随机游走 double gyro_bias_noise 0.00001; Q.block3,3(12, 12) Matrix3d::Identity() * gyro_bias_noise * gyro_bias_noise * dt; // 重力随机游走通常设得很小 double gravity_noise 1e-6; Q.block3,3(15, 15) Matrix3d::Identity() * gravity_noise * gravity_noise * dt; return Q; }这些噪声参数不是拍脑袋来的。加速度计噪声密度可以从数据手册查到比如 MPU6050 的加速度计噪声密度大约是 400 μg/√Hz换算成 m/s²/√Hz 大约是 0.004。但实际使用中由于振动、温度变化等因素有效噪声往往比手册值大好几倍。我的经验是先按手册值设然后根据静态数据的 Allan 方差分析结果调整。第三积分方法的选择。欧拉积分在 dt1ms 时误差很小但如果你的 IMU 是 100Hz 输出dt10ms欧拉积分的误差就不能忽略了。这时候可以用中值积分// 中值积分 Vec3 acc_mid 0.5 * (R_prev * (acc_prev - ba_) R_curr * (acc_curr - ba_)) g_; p_ v_ * dt 0.5 * acc_mid * dt * dt; v_ acc_mid * dt;中值积分需要保存上一时刻的 IMU 数据和旋转矩阵代码稍微复杂一点但精度提升明显。3.4 观测更新加速度计、磁力计与 GPS观测更新的代码结构是统一的计算残差、计算 H 矩阵、计算卡尔曼增益、更新误差状态、注入并清零。以加速度计更新为例。加速度计测量的是比力在静止时等于重力的反方向在机体系下的表示void ESKF::updateAcc(const Vec3 acc_meas) { // 预测的加速度计观测 Matrix3d R quatToRotation(q_); Vec3 acc_pred R.transpose() * g_; // 残差 Vec3 residual acc_meas - acc_pred; // 观测矩阵 H (3x18) Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); // 对姿态误差的偏导 H.block3,3(0, 6) R.transpose() * skewSymmetric(g_); // 对重力误差的偏导 H.block3,3(0, 15) -R.transpose(); // 卡尔曼增益 Matrix3d S H * P_ * H.transpose() R_acc_; Matrixdouble, 18, 3 K P_ * H.transpose() * S.inverse(); // 更新误差状态 Matrixdouble, 18, 1 dx K * residual; // 注入并清零 injectErrorState(dx); // 协方差更新Joseph 形式数值更稳定 Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K * H) * P_ * (I - K * H).transpose() K * R_acc_ * K.transpose(); }这里用了 Joseph 形式的协方差更新比简单的 (I-KH)P 数值稳定性更好。特别是在观测维度小于状态维度的时候Joseph 形式能保证协方差矩阵始终对称正定。磁力计更新的逻辑类似但观测方程不同。磁力计测量的是地磁场在机体系下的方向void ESKF::updateMag(const Vec3 mag_meas) { Matrix3d R quatToRotation(q_); Vec3 mag_pred R.transpose() * mag_world_; Vec3 residual mag_meas - mag_pred; Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); H.block3,3(0, 6) R.transpose() * skewSymmetric(mag_world_); Matrix3d S H * P_ * H.transpose() R_mag_; Matrixdouble, 18, 3 K P_ * H.transpose() * S.inverse(); Matrixdouble, 18, 1 dx K * residual; injectErrorState(dx); Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K * H) * P_ * (I - K * H).transpose() K * R_mag_ * K.transpose(); }GPS 更新的是位置和速度观测矩阵更简单void ESKF::updateGps(const Vec3 pos_meas, const Vec3 vel_meas) { // 位置更新 Vec3 pos_residual pos_meas - p_; Matrixdouble, 3, 18 H_pos Matrixdouble, 3, 18::Zero(); H_pos.block3,3(0, 0) Matrix3d::Identity(); Matrix3d S_pos H_pos * P_ * H_pos.transpose() R_gps_pos_; Matrixdouble, 18, 3 K_pos P_ * H_pos.transpose() * S_pos.inverse(); Matrixdouble, 18, 1 dx_pos K_pos * pos_residual; injectErrorState(dx_pos); Matrixdouble, 18, 18 I Matrixdouble, 18, 18::Identity(); P_ (I - K_pos * H_pos) * P_ * (I - K_pos * H_pos).transpose() K_pos * R_gps_pos_ * K_pos.transpose(); // 速度更新 Vec3 vel_residual vel_meas - v_; Matrixdouble, 3, 18 H_vel Matrixdouble, 3, 18::Zero(); H_vel.block3,3(0, 3) Matrix3d::Identity(); Matrix3d S_vel H_vel * P_ * H_vel.transpose() R_gps_vel_; Matrixdouble, 18, 3 K_vel P_ * H_vel.transpose() * S_vel.inverse(); Matrixdouble, 18, 1 dx_vel K_vel * vel_residual; injectErrorState(dx_vel); P_ (I - K_vel * H_vel) * P_ * (I - K_vel * H_vel).transpose() K_vel * R_gps_vel_ * K_vel.transpose(); }注意每次观测更新后都要重新计算 H 矩阵因为 H 矩阵依赖于当前的名义状态。如果你在多次更新之间不重新计算 H滤波器的收敛性会变差。4. 调试与实战那些文档里不会告诉你的坑4.1 初始化滤波器能不能收敛一半看初始化ESKF 的初始化极其关键。如果初始姿态、初始零偏、初始协方差设得离谱滤波器要么发散要么收敛得极慢。我的初始化流程是这样的第一步静止采集。让 IMU 静止放置至少 2 秒采集 200 到 500 个样本。计算加速度计和陀螺仪的均值Vec3 acc_mean Vec3::Zero(); Vec3 gyro_mean Vec3::Zero(); for (const auto sample : static_samples) { acc_mean sample.acc; gyro_mean sample.gyro; } acc_mean / static_samples.size(); gyro_mean / static_samples.size();第二步估计初始姿态。用加速度计均值估计俯仰和横滚用磁力计估计航向// 俯仰和横滚 double roll atan2(acc_mean.y(), acc_mean.z()); double pitch atan2(-acc_mean.x(), sqrt(acc_mean.y()*acc_mean.y() acc_mean.z()*acc_mean.z())); // 航向需要磁力计 Vec3 mag_mean ...; // 磁力计均值 double yaw atan2(-mag_mean.y(), mag_mean.x());第三步估计初始零偏。陀螺仪零偏直接用静止时的均值。加速度计零偏需要扣除重力分量Vec3 gravity_body quatToRotation(q_init).transpose() * Vec3(0, 0, -9.81); Vec3 ba_init acc_mean - gravity_body;第四步设置初始协方差。位置和速度的初始协方差设小一点比如 0.01姿态的初始协方差根据加速度计和磁力计的噪声水平设通常 0.1 到 1.0 弧度零偏的初始协方差设大一点比如 0.1因为零偏的不确定性最大。我踩过的一个坑是初始协方差设得太小导致滤波器过度自信后续观测修正不进去。比如姿态初始协方差设成 0.001结果实际初始姿态误差有 0.1 弧度滤波器要花很长时间才能修正过来。后来我把姿态初始协方差统一设成 0.5收敛速度明显改善。4.2 数值稳定性协方差矩阵为什么不对称了ESKF 跑一段时间后协方差矩阵 P 可能会失去对称性甚至出现负特征值。这是数值误差积累的结果。解决方法有几个方法一强制对称化。每次更新后做 P 0.5 * (P P^T)。这个操作简单粗暴但有效。方法二用 Joseph 形式更新。前面代码里已经用了能显著改善对称性。方法三定期做特征值分解把负特征值截断到一个小正数。这个操作计算量大一般只在调试阶段用。方法四用平方根滤波。这是最彻底的方法但实现复杂度高一般工程上没必要。我的经验是Joseph 形式 强制对称化能解决 95% 的数值稳定性问题。如果还不行检查一下 Q 矩阵是不是设得太小导致 P 矩阵条件数过大。4.3 观测噪声调参R 矩阵怎么设才合理观测噪声矩阵 R 的调参是 ESKF 最玄学的部分。设得太大滤波器不信任观测姿态漂移设得太小滤波器过度信任观测噪声放大。我的调参流程静态测试IMU 静止放置只开预测不开更新观察姿态漂移速度。如果 10 秒内漂移超过 1 度说明陀螺仪零偏没标定好。开加速度计更新观察俯仰和横滚是否稳定。如果抖动明显增大 R_acc如果响应迟钝减小 R_acc。开磁力计更新观察航向是否稳定。磁力计受环境干扰大R_mag 通常要比 R_acc 大一个数量级。动态测试手持 IMU 做各种机动观察姿态跟踪是否跟得上。如果滞后明显减小 R如果噪声放大增大 R。一个实用的技巧是用 Allan 方差分析结果来设 R 的初始值。Allan 方差能给出传感器的噪声密度和零偏不稳定性直接对应到 R 矩阵的对角线元素。4.4 常见问题速查表问题现象可能原因排查方法解决方案姿态缓慢漂移陀螺仪零偏未标定静态下观察零偏估计是否收敛重新标定零偏增大 Q 中零偏噪声姿态抖动明显R 矩阵设得太小观察残差序列是否白噪声增大 R_acc 和 R_mag机动时姿态滞后Q 矩阵设得太小观察协方差是否过小增大 Q 中姿态噪声滤波器发散F 矩阵或 H 矩阵符号错误用数值差分验证雅可比检查反对称矩阵符号协方差矩阵不对称数值误差积累检查特征值是否有负值Joseph 更新 强制对称化航向缓慢旋转磁力计受干扰对比磁力计和陀螺仪航向增大 R_mag或暂时关闭磁力计位置估计漂移加速度计零偏未估计观察 ba 估计是否收敛增大 Q 中零偏噪声检查可观测性4.5 实测数据与性能评估我在一个室内机器人平台上实测过这套 ESKF。IMU 是 MPU6050输出频率 200Hz磁力计是 HMC5883L输出频率 50Hz。测试场景是机器人原地旋转和直线行走。静态测试结果姿态估计的标准差在俯仰和横滚方向小于 0.1 度航向方向小于 0.5 度磁力计受室内钢筋干扰。动态测试结果机器人以 90 度/秒旋转时姿态跟踪误差小于 1 度滞后小于 20ms。这个性能对于室内机器人导航已经够用了。如果你要更高精度可以考虑用工业级 IMU比如 ADIS16470噪声密度低一个数量级姿态精度能到 0.01 度级别。5. 进阶扩展从 ESKF 到多传感器融合5.1 视觉惯性融合ESKF 在 VIO 里的角色ESKF 在视觉惯性里程计VIO里的应用非常广泛。VINS-Mono 的核心就是一个 ESKF 的变体它把视觉特征点的重投影误差作为观测更新 IMU 的误差状态。视觉观测的 H 矩阵计算比加速度计和磁力计复杂得多因为涉及到相机投影模型和特征点位置。但框架是一样的写出观测方程对误差状态求偏导填入 H 矩阵。如果你要做 VIO我建议先跑通纯 IMU 的 ESKF再加入视觉观测。这样调试的时候可以分层定位问题。5.2 激光雷达与 IMU 融合LIO 中的 ESKF激光雷达和 IMU 的融合LIO是另一个热门方向。LOAM、LIO-SAM、FAST-LIO 这些方案里ESKF 或者其变体都是核心。FAST-LIO 用的是迭代扩展卡尔曼滤波IEKF本质上是 ESKF 的迭代版本。它在每次观测更新时多次迭代重新线性化观测方程精度比标准 ESKF 更高。如果你要做 LIOESKF 的状态向量需要加上外参IMU 到激光雷达的旋转和平移。外参可以在线估计也可以提前标定。在线估计的好处是能适应安装误差坏处是增加了状态维度计算量变大。5.3 轮速计与 IMU 融合低成本组合导航如果你做的是地面机器人轮速计是一个便宜又好用的传感器。轮速计观测的是车体前进速度观测方程很简单// 轮速计观测车体前进速度 double v_wheel ...; // 轮速计测量值 Vec3 v_body Vec3(v_wheel, 0, 0); // 假设车体坐标系 x 轴向前 Vec3 v_world quatToRotation(q_) * v_body; // 观测矩阵 Matrixdouble, 3, 18 H Matrixdouble, 3, 18::Zero(); H.block3,3(0, 3) Matrix3d::Identity();轮速计和 IMU 融合能显著抑制位置漂移特别是在 GPS 信号不好的室内环境。我做过一个测试纯 IMU 积分 10 秒位置漂移 2 米加上轮速计后漂移降到 0.3 米。5.4 代码优化让 ESKF 跑得更快ESKF 的计算瓶颈主要在矩阵乘法和求逆。18 维状态的协方差矩阵是 18x18求逆是 O(18^3)在嵌入式平台上可能吃不消。优化方法利用稀疏性F 矩阵和 H 矩阵都是稀疏的用稀疏矩阵运算能省不少时间。降低状态维度如果不需要估计重力砍掉 δg状态降到 15 维。用固定大小的 Eigen 矩阵Matrixdouble, 18, 18 比 MatrixXd 快很多因为编译期就能确定大小。避免动态内存分配在 predict 和 update 里不要用 new 或 malloc所有矩阵都在栈上分配。我在树莓派 4B 上实测18 维 ESKF 单次 predictupdate 耗时约 0.5ms200Hz 跑完全没问题。如果在 STM32 上跑可能需要降到 12 维状态并且用单精度浮点。6. 一些掏心窝子的实操心得写 ESKF 代码这几年踩过的坑比写过的代码还多。分享几条我觉得最有价值的经验第一条先仿真后实测。不要一上来就拿真实 IMU 数据跑。先用 MATLAB 或 Python 生成一段仿真 IMU 数据已知真实轨迹 加噪声在仿真数据上把算法调通再上真实数据。仿真数据的好处是你知道真值能定量评估误差。第二条可视化调试。把姿态、速度、零偏、协方差对角线都画出来。滤波器发散之前协方差通常会有异常变化。比如协方差突然变小说明滤波器过度自信了协方差突然变大说明观测和预测严重不一致。第三条单元测试。四元数乘法、旋转矩阵构造、反对称矩阵、误差状态注入这些基础函数一定要写单元测试。我见过太多人因为四元数乘法写错调了一周才发现问题。第四条记录数据。每次测试都把 IMU 原始数据、ESKF 输出、真值如果有保存下来。调参的时候可以离线回放不用反复跑实验。第五条别迷信理论值。数据手册上的噪声密度是理想条件下的实际使用中振动、温度、电磁干扰都会让噪声变大。R 和 Q 矩阵的最终值一定是调出来的不是算出来的。第六条注意坐标系约定。IMU 的机体系定义、世界系定义、重力方向定义这些一定要在代码注释里写清楚。我见过一个项目两个人分别写预测和更新结果一个用 NED 系一个用 ENU 系滤波器直接爆炸。第七条零偏估计要慢。零偏的随机游走噪声不要设得太大否则零偏估计会跟着观测噪声一起抖。零偏是一个缓慢变化的量让它慢慢收敛就好。第八条磁力计要慎用。室内环境下磁力计受钢筋、电器干扰严重航向估计可能比陀螺仪积分还差。我的做法是先判断磁力计数据的可靠性比如检查磁场强度是否在合理范围内可靠时才用来更新航向。第九条GPS 更新要检查有效性。GPS 在隧道、室内、城市峡谷里会给出离谱的位置直接用来更新会把滤波器带偏。更新前检查 HDOP、卫星数、位置跳变是否超过阈值。第十条代码要能回放。把 ESKF 设计成可以离线回放数据的形式输入是 IMU 数据流和观测数据流输出是状态估计序列。这样调参的时候不用连硬件效率高很多。这套 ESKF 代码我后来用在了好几个项目上从无人机到地面机器人从纯 IMU 到多传感器融合框架基本没大改只是调整了状态维度和观测模型。ESKF 的魅力就在于它的结构足够清晰扩展起来足够灵活。你把这篇里的代码敲一遍跑通再根据自己的传感器配置改一改基本就能应付大部分 IMU 状态估计的需求了。