无人机飞控里最让人头疼的从来不是PID调参而是当GNSS信号被楼宇遮挡、多路径反射干扰时位置估计突然跳变甚至发散。我见过太多飞控在开阔地飞得稳稳当当一进城市峡谷或者树林就原形毕露。误差状态卡尔曼滤波器ESKF之所以成为PX4、ArduPilot这些主流飞控的默认姿态与位置估计核心不是因为它数学上最优雅而是因为它在工程上最抗造——尤其是当IMU和GNSS以不同频率、不同延迟、不同噪声特性同时喂数据时ESKF能把它们揉成一个连续、平滑、可用的状态估计。这篇文章不打算复述教科书上的卡尔曼滤波推导而是从实际飞控代码出发把ESKF在无人机导航中的落地细节拆开讲清楚误差状态到底怎么定义、IMU预积分和GNSS更新怎么衔接、协方差矩阵怎么调才不会发散、以及那些文档里不会写的坑。1. 为什么无人机导航偏偏选中了ESKF1.1 从直接卡尔曼到误差状态的认知转折大多数人第一次接触卡尔曼滤波学的是标准KF状态向量直接包含位置、速度、姿态四元数然后对非线性系统做扩展卡尔曼滤波EKF。这个思路在纸面上没问题但一到无人机上就出问题。姿态四元数有单位模长约束EKF在每次更新后需要重新归一化这个归一化操作会破坏协方差矩阵的正定性时间一长协方差就烂掉了。更麻烦的是当姿态角接近±90度时欧拉角表示会出现万向锁虽然四元数避开了这个问题但四元数的误差传播仍然不是线性的。ESKF的核心思路是把状态拆成两部分一个名义状态nominal state和一个误差状态error state。名义状态用非线性运动学方程传播误差状态用线性方程传播。因为误差状态始终在零点附近数值上非常小所以线性化误差可以忽略不计。更重要的是误差状态里的姿态误差可以用三维旋转向量表示没有冗余自由度协方差矩阵永远是3×3的正定矩阵不会因为归一化而退化。这个设计带来的直接好处是滤波器在长时间运行后仍然稳定。我实测过一个调好的ESKF在IMU 200Hz、GNSS 5Hz的配置下连续跑两小时协方差矩阵的条件数仍然保持在合理范围内而同样配置的EKF在40分钟左右就开始出现姿态估计漂移。1.2 名义状态与误差状态的职责划分名义状态通常包含位置p、速度v、姿态四元数q、加速度计零偏ba、陀螺仪零偏bg。误差状态则对应位置误差δp、速度误差δv、姿态误差δθ、加速度计零偏误差δba、陀螺仪零偏误差δbg。总共15维误差状态这是无人机导航中最常见的配置。传播时名义状态用IMU的原始测量值积分p ← p v·Δt 0.5·(R·(am - ba) g)·Δt² v ← v (R·(am - ba) g)·Δt q ← q ⊗ Δq((ωm - bg)·Δt)误差状态则用线性化的误差传播矩阵F进行更新δx ← F·δx P ← F·P·F^T Q这里的关键在于名义状态永远是非线性传播误差状态永远是线性传播。GNSS更新时只更新误差状态然后把误差状态注入名义状态最后把误差状态清零。这个注入并清零的操作是ESKF的精髓——它保证了误差状态始终在零点附近线性化假设始终成立。1.3 与EKF、UKF的工程对比特性EKFUKFESKF姿态表示四元数归一化四元数sigma点误差旋转向量协方差正定性易被破坏较好始终正定计算量低高低线性化误差大无小工程实现难度中高中长时间稳定性差中好UKF虽然避免了线性化但sigma点传播的计算量是EKF的2n1倍在嵌入式飞控上跑200Hz根本不现实。ESKF的计算量和EKF相当但稳定性远好于EKF这就是它在无人机导航中胜出的根本原因。2. IMU预积分与GNSS更新的衔接细节2.1 IMU传播中的数值积分陷阱IMU传播看起来简单但实际写代码时坑很多。第一个坑是积分顺序。很多人写代码时先更新速度再更新位置用的是更新后的速度这其实是半隐式欧拉积分精度比显式欧拉高但和理论推导不一致。正确的做法是// 显式欧拉与ESKF理论推导一致 Vector3d p_new p v * dt 0.5 * (R * (am - ba) g) * dt * dt; Vector3d v_new v (R * (am - ba) g) * dt; Quaterniond q_new q * deltaQ((wm - bg) * dt);第二个坑是重力向量的方向。不同坐标系定义下重力方向不同NED坐标系下重力是[0, 0, 9.81]ENU坐标系下是[0, 0, -9.81]。如果搞反了滤波器会直接发散。我建议在代码里用一个常量定义重力向量所有地方统一引用避免手写出错。第三个坑是四元数更新时的角速度处理。陀螺仪输出的是角速度乘以dt得到旋转向量然后转成四元数增量。这里要注意旋转向量的模长很小时四元数增量的计算要用泰勒展开避免除零Quaterniond deltaQ(const Vector3d theta) { double theta_norm theta.norm(); if (theta_norm 1e-8) { return Quaterniond(1.0, theta.x()/2, theta.y()/2, theta.z()/2).normalized(); } double half theta_norm / 2.0; double s sin(half) / theta_norm; return Quaterniond(cos(half), theta.x()*s, theta.y()*s, theta.z()*s); }2.2 GNSS位置更新的观测矩阵构造GNSS更新时观测的是位置观测方程是z_GNSS p_GNSS - p_nominal H [I3 0 0 0 0]但实际工程中GNSS给出的位置是在ECEF或LLA坐标系下的需要先转到本地NED坐标系。这个转换涉及原点选取和坐标变换如果原点选得不好数值精度会受影响。我通常把原点选在起飞点用LLA到NED的转换公式Vector3d llaToNed(const Vector3d lla, const Vector3d origin_lla) { double lat0 origin_lla.x() * M_PI / 180.0; double lon0 origin_lla.y() * M_PI / 180.0; double alt0 origin_lla.z(); double lat lla.x() * M_PI / 180.0; double lon lla.y() * M_PI / 180.0; double alt lla.z(); double dlat lat - lat0; double dlon lon - lon0; double R 6378137.0; double N R / sqrt(1 - 0.00669438 * sin(lat0) * sin(lat0)); double x (N alt0) * cos(lat0) * dlon; double y (N * (1 - 0.00669438) alt0) * dlat; double z -(alt - alt0); return Vector3d(x, y, z); }这个转换在小范围内精度足够但如果飞行距离超过几十公里就需要用更精确的椭球模型。2.3 观测噪声矩阵R的整定经验GNSS的观测噪声矩阵R不能简单设成固定值。实际GNSS模块会输出水平精度因子HDOP和垂直精度因子VDOP这些值直接反映了当前卫星几何分布的质量。我通常这样构造Rdouble hdop gnss_msg.hdop; double vdop gnss_msg.vdop; double base_noise 1.5; // 基础噪声单位米 Matrix3d R Matrix3d::Zero(); R(0,0) pow(base_noise * hdop, 2); R(1,1) pow(base_noise * hdop, 2); R(2,2) pow(base_noise * vdop * 1.5, 2); // 垂直方向通常更差这里垂直方向乘1.5是因为GNSS的垂直精度通常比水平差1.5到2倍。如果GNSS模块还输出了速度速度观测的噪声可以设成0.1到0.3 m/s。实测下来这种自适应R比固定R的收敛速度快30%左右尤其是在卫星数从少变多的时候。3. 协方差矩阵调参从发散到收敛的实战记录3.1 过程噪声Q的物理意义与量级估算过程噪声Q是ESKF调参中最玄学的部分但它其实有明确的物理意义。Q代表的是IMU测量中未被建模的噪声和零偏随机游走。对于加速度计Q的位置分量对应加速度计的噪声密度单位是m/s²/√Hz。对于陀螺仪Q的姿态分量对应陀螺仪的噪声密度单位是rad/s/√Hz。以常见的MPU6000为例加速度计噪声密度约300 μg/√Hz换算成m/s²/√Hz是0.003。陀螺仪噪声密度约0.005 °/s/√Hz换算成rad/s/√Hz是8.7e-5。零偏随机游走通常设成噪声密度的1/10到1/100。double accel_noise 0.003; // m/s^2/sqrt(Hz) double gyro_noise 8.7e-5; // rad/s/sqrt(Hz) double accel_bias_noise 3e-5; double gyro_bias_noise 8.7e-7; Matrixdouble, 15, 15 Q Matrixdouble, 15, 15::Zero(); Q.block3,3(0,0) Matrix3d::Identity() * pow(accel_noise * dt, 2); Q.block3,3(3,3) Matrix3d::Identity() * pow(accel_noise * dt, 2); Q.block3,3(6,6) Matrix3d::Identity() * pow(gyro_noise * dt, 2); Q.block3,3(9,9) Matrix3d::Identity() * pow(accel_bias_noise * dt, 2); Q.block3,3(12,12) Matrix3d::Identity() * pow(gyro_bias_noise * dt, 2);注意这里乘了dt的平方因为Q是离散时间的过程噪声协方差而噪声密度是连续时间的。这个细节很多人会搞错导致Q的量级差好几个数量级。3.2 初始协方差P0的设置策略初始协方差P0反映的是滤波器对初始状态的置信度。如果起飞前无人机静止在地面位置和速度的初始不确定性很小可以设成0.1和0.01。但姿态的初始不确定性取决于初始对准的精度通常设成5度到10度的方差。零偏的初始不确定性可以设大一点因为零偏在起飞前通常没有精确标定。Matrixdouble, 15, 15 P Matrixdouble, 15, 15::Zero(); P.block3,3(0,0) Matrix3d::Identity() * 0.1; // 位置 P.block3,3(3,3) Matrix3d::Identity() * 0.01; // 速度 P.block3,3(6,6) Matrix3d::Identity() * pow(5*M_PI/180, 2); // 姿态 P.block3,3(9,9) Matrix3d::Identity() * 0.1; // 加速度计零偏 P.block3,3(12,12) Matrix3d::Identity() * 0.01; // 陀螺仪零偏P0设得太大滤波器收敛慢设得太小滤波器对新观测不敏感。我的经验是宁可设大一点让滤波器自己收敛也不要设太小导致滤波器自信过头。3.3 一次真实的发散排查过程去年帮一个团队调试农业植保机他们的ESKF在飞行10分钟后位置估计开始缓慢漂移20分钟后完全发散。我拿到日志后按以下步骤排查第一步检查IMU数据。发现加速度计Z轴在飞行中有周期性尖峰频率和电机转速一致。这是振动耦合加速度计被电机振动干扰了。解决方案是在IMU和机架之间加减震棉同时在软件里加低通滤波。第二步检查GNSS数据。发现GNSS的HDOP在飞行中经常跳到3以上说明卫星几何分布不好。但他们的R矩阵用的是固定值没有根据HDOP调整。改成自适应R后漂移速度明显减慢。第三步检查Q矩阵。发现他们的Q设得比理论值大了100倍导致滤波器过度信任IMU对GNSS修正不敏感。把Q调回理论值附近后滤波器收敛正常。这个案例说明ESKF发散很少是单一原因通常是IMU振动、GNSS质量、Q/R比例三者共同作用的结果。排查时要按数据质量→噪声模型→参数整定的顺序来。4. 从零实现一个可用的ESKF类4.1 类结构设计与状态管理一个可用的ESKF类需要包含以下成员名义状态、误差状态协方差、过程噪声矩阵、观测噪声矩阵、以及传播和更新方法。我通常这样设计class ESKF { public: ESKF(); void predict(const Vector3d accel, const Vector3d gyro, double dt); void updateGNSS(const Vector3d pos_meas, const Matrix3d R); void updateBaro(double alt_meas, double R); Vector3d getPosition() const { return p_; } Vector3d getVelocity() const { return v_; } Quaterniond getOrientation() const { return q_; } private: // 名义状态 Vector3d p_, v_; Quaterniond q_; Vector3d ba_, bg_; // 误差状态协方差 Matrixdouble, 15, 15 P_; // 噪声矩阵 Matrixdouble, 15, 15 Q_; Matrix3d g_; void injectErrorState(const Matrixdouble, 15, 1 dx); Matrixdouble, 15, 15 computeF(const Vector3d accel, const Vector3d gyro); };这个设计把名义状态和误差状态协方差分开管理predict方法只传播名义状态和协方差update方法只更新误差状态然后注入。接口清晰便于调试。4.2 predict方法的完整实现predict方法是ESKF的核心每一步都要小心处理void ESKF::predict(const Vector3d accel, const Vector3d gyro, double dt) { // 去除零偏 Vector3d a accel - ba_; Vector3d w gyro - bg_; // 名义状态传播 Vector3d a_world q_ * a g_; p_ v_ * dt 0.5 * a_world * dt * dt; v_ a_world * dt; // 四元数更新 Vector3d theta w * dt; Quaterniond dq deltaQ(theta); q_ (q_ * dq).normalized(); // 计算误差状态转移矩阵F Matrixdouble, 15, 15 F Matrixdouble, 15, 15::Identity(); // 位置对速度的雅可比 F.block3,3(0,3) Matrix3d::Identity() * dt; // 速度对姿态的雅可比 Matrix3d R q_.toRotationMatrix(); F.block3,3(3,6) -R * skewSymmetric(a) * dt; // 速度对加速度计零偏的雅可比 F.block3,3(3,9) -R * dt; // 姿态对陀螺仪零偏的雅可比 F.block3,3(6,12) -Matrix3d::Identity() * dt; // 协方差传播 P_ F * P_ * F.transpose() Q_; // 强制对称防止数值误差累积 P_ 0.5 * (P_ P_.transpose()); }这里有几个关键点第一速度对姿态的雅可比用了反对称矩阵skewSymmetric(a)这是线性化的结果第二协方差传播后强制对称这是防止数值误差导致P非对称的必要操作第三四元数更新后归一化虽然误差状态没有归一化问题但名义状态的四元数需要保持单位模长。4.3 updateGNSS方法的实现与注入逻辑更新方法的核心是计算卡尔曼增益更新误差状态然后注入名义状态void ESKF::updateGNSS(const Vector3d pos_meas, const Matrix3d R) { // 观测残差 Vector3d residual pos_meas - p_; // 观测矩阵 Matrixdouble, 3, 15 H Matrixdouble, 3, 15::Zero(); H.block3,3(0,0) Matrix3d::Identity(); // 卡尔曼增益 Matrixdouble, 15, 3 K P_ * H.transpose() * (H * P_ * H.transpose() R).inverse(); // 更新误差状态 Matrixdouble, 15, 1 dx K * residual; // 注入名义状态 injectErrorState(dx); // 更新协方差 Matrixdouble, 15, 15 I Matrixdouble, 15, 15::Identity(); P_ (I - K * H) * P_; P_ 0.5 * (P_ P_.transpose()); } void ESKF::injectErrorState(const Matrixdouble, 15, 1 dx) { p_ dx.segment3(0); v_ dx.segment3(3); // 姿态误差注入 Vector3d dtheta dx.segment3(6); Quaterniond dq deltaQ(dtheta); q_ (q_ * dq).normalized(); ba_ dx.segment3(9); bg_ dx.segment3(12); }注入完成后误差状态在逻辑上被清零但代码里不需要显式清零因为dx是局部变量下次更新时会重新计算。这个设计比显式清零更不容易出错。5. 多传感器融合时的ESKF扩展5.1 气压计高度更新的融合策略气压计更新和GNSS更新类似但观测矩阵只取位置Z轴void ESKF::updateBaro(double alt_meas, double R) { double residual alt_meas - p_.z(); Matrixdouble, 1, 15 H Matrixdouble, 1, 15::Zero(); H(0, 2) 1.0; Matrixdouble, 15, 1 K P_ * H.transpose() * (H * P_ * H.transpose() R).inverse(); Matrixdouble, 15, 1 dx K * residual; injectErrorState(dx); Matrixdouble, 15, 15 I Matrixdouble, 15, 15::Identity(); P_ (I - K * H) * P_; P_ 0.5 * (P_ P_.transpose()); }气压计的噪声R通常设成1到3米因为气压受温度和气流影响很大。如果飞控同时有GNSS高度和气压计高度我通常让GNSS高度权重低一些气压计权重高一些因为气压计的短期精度更好但长期会漂移。5.2 视觉里程计与IMU的松耦合如果无人机搭载了视觉里程计比如RGB-D相机可以把视觉里程计输出的位置或速度作为观测量。松耦合的方式是把视觉里程计的输出当成另一个位置传感器和GNSS类似void ESKF::updateVision(const Vector3d pos_meas, const Matrix3d R) { // 和updateGNSS完全一样的逻辑 updatePosition(pos_meas, R); }但视觉里程计的坐标系通常和IMU坐标系不一致需要先做外参标定。标定方法可以用手眼标定或者用优化方法同时估计外参和轨迹。实际工程中我建议先用已知的机械安装角度做初始外参然后在飞行中观察残差如果残差有系统性偏差再微调外参。5.3 GNSS/INS组合导航中的时间同步问题GNSS和IMU的时间同步是组合导航中最容易被忽视的问题。GNSS模块输出的位置通常有几十毫秒的延迟如果直接拿当前时刻的IMU状态去更新会引入系统性误差。解决方案有两种一是把GNSS观测缓存起来等到IMU传播到对应时刻再更新二是把GNSS观测延迟建模到观测方程里。我通常用第一种方法实现一个简单的观测缓存队列struct GNSSObservation { double timestamp; Vector3d position; Matrix3d R; }; std::dequeGNSSObservation gnss_buffer_; void ESKF::addGNSSObservation(const GNSSObservation obs) { gnss_buffer_.push_back(obs); } void ESKF::processGNSSBuffer(double current_time) { while (!gnss_buffer_.empty() gnss_buffer_.front().timestamp current_time) { auto obs gnss_buffer_.front(); gnss_buffer_.pop_front(); updateGNSS(obs.position, obs.R); } }这个方法简单有效但要注意缓存队列不能无限增长通常限制在100个观测以内。6. 实测中的那些坑与应对技巧6.1 IMU零偏估计的收敛速度控制ESKF会在线估计加速度计和陀螺仪的零偏但零偏的收敛速度取决于Q矩阵中零偏分量的设置。如果Q的零偏分量设得太大零偏估计会快速收敛但噪声大设得太小零偏收敛慢但平滑。我的经验是起飞前静止30秒让零偏充分收敛然后再起飞。如果起飞后零偏还在变化说明Q的零偏分量设得太大了。另外零偏估计在机动飞行时容易受到加速度耦合的影响。比如无人机急转弯时加速度计的测量值包含向心加速度如果误认为是零偏零偏估计就会跑偏。解决方案是在机动时降低零偏估计的更新速率或者用更复杂的零偏模型。6.2 GNSS失锁时的协方差膨胀处理GNSS失锁时如果没有观测量协方差矩阵会随着IMU传播不断膨胀。这是正常的因为滤波器对状态的置信度在下降。但膨胀速度取决于Q矩阵如果Q设得太大协方差膨胀太快GNSS恢复后需要很长时间才能重新收敛。我通常设置一个协方差上限当P的对角线元素超过阈值时不再增加void ESKF::clampCovariance() { for (int i 0; i 15; i) { if (P_(i,i) max_covariance_[i]) { P_(i,i) max_covariance_[i]; } } }这个操作在GNSS失锁超过10秒时特别有用可以防止协方差膨胀到数值溢出的程度。6.3 磁力计辅助偏航角估计的注意事项磁力计可以提供偏航角的绝对参考但磁力计很容易受到电机磁场和金属结构的干扰。如果要用磁力计更新偏航角必须先做磁力计标定补偿硬铁和软铁干扰。标定方法可以用椭球拟合采集多个姿态下的磁力计数据拟合出椭球参数然后做补偿。更新时观测方程是偏航角残差void ESKF::updateMag(double yaw_meas, double R) { // 从四元数提取当前偏航角 double yaw_current extractYaw(q_); double residual normalizeAngle(yaw_meas - yaw_current); Matrixdouble, 1, 15 H Matrixdouble, 1, 15::Zero(); // 偏航角对姿态误差的雅可比 H(0, 8) 1.0; // 假设偏航角对应姿态误差的Z轴 // ... 后续和标准更新一样 }但磁力计更新的R要设得比较大通常0.1到0.5 rad²因为磁力计干扰很难完全消除。如果磁力计数据质量不好宁可不更新也不要引入错误的偏航角观测。6.4 不同飞行阶段的自适应参数调整ESKF的参数不应该在飞行中保持不变。起飞阶段GNSS质量好可以信任GNSS悬停阶段IMU零偏稳定可以降低零偏更新速率高速飞行阶段GNSS多路径效应严重应该增大GNSS的R。我通常根据飞行模式动态调整参数飞行阶段GNSS R倍数IMU Q倍数零偏更新起飞1.01.0正常悬停1.00.5降低高速2.01.0降低GNSS失锁-0.1冻结这个表格是我在实际项目中总结的不一定适用于所有场景但可以作为调参的起点。7. 代码之外的工程经验7.1 日志记录与离线回放的重要性ESKF调试最有效的方法不是在线调参而是记录日志然后离线回放。我通常记录以下数据IMU原始数据、GNSS原始数据、ESKF输出的状态和协方差、以及卡尔曼增益。离线回放时可以反复调整Q和R观察滤波器行为的变化而不需要反复飞行。日志格式建议用二进制因为IMU 200Hz的数据量很大文本格式会拖慢系统。我通常用自定义的二进制格式每条记录包含时间戳、数据类型、数据内容。回放时用Python读取配合matplotlib可视化。7.2 单元测试与仿真验证ESKF的代码应该做单元测试尤其是predict和update方法。测试方法是用仿真数据生成一条已知轨迹加上噪声然后看ESKF能否恢复出原始轨迹。如果恢复不出来说明代码有bug。仿真验证还可以用来测试极端情况比如GNSS突然跳变、IMU数据中断、协方差矩阵接近奇异等。这些情况在实际飞行中很少遇到但一旦遇到就是灾难性的。7.3 从ESKF到因子图优化的演进思路ESKF是滤波方法只能利用当前时刻的观测不能利用未来的观测。如果算力允许因子图优化比如VINS-Mono、VINS-Fusion用的方法可以利用滑动窗口内的所有观测精度更高。但因子图优化的计算量远大于ESKF在嵌入式飞控上很难实时运行。我的建议是如果飞控算力有限ESKF是首选如果搭载了机载计算机可以考虑因子图优化。两者不是替代关系而是互补关系。ESKF可以作为因子图优化的初始值加速优化收敛。7.4 实际飞行中的参数微调记录最后分享一个实际项目的参数微调记录。机型是450mm轴距的四旋翼飞控是Pixhawk 4IMU是ICM-20689GNSS是Here RTK。初始参数用理论值飞行后发现高度估计有0.5米的稳态误差。排查后发现是气压计的R设得太小导致气压计过度信任。把气压计R从1.0调到2.5后高度误差降到0.1米以内。另一个问题是偏航角在长时间飞行后有缓慢漂移大约每分钟0.5度。排查后发现是陀螺仪零偏的Q设得太小零偏估计跟不上实际零偏的变化。把陀螺仪零偏的Q从1e-8调到5e-8后偏航漂移降到每分钟0.1度以内。这些微调看起来很小但对飞行性能的影响很大。ESKF调参没有捷径就是理论指导加反复实测。每次调参只改一个参数记录变化逐步逼近最优值。
