PX4 EKF2 源码解析三EKF2::Run()主循环与完整数据链摘要EKF2::Run()是 PX4 接口层的运行入口。它将工作队列、uORB、传感器样本、主 EKF 与输出发布连接起来。本文沿一帧 IMU 数据的生命周期分析主循环明确高频路径、滤波更新路径和低频诊断路径。关键词PX4、EKF2、uORB、ScheduledWorkItem、实时数据流1. 调度模型EKF2 采用事件驱动而不是固定周期轮询。多实例模式通常注册vehicle_imu回调单实例模式使用sensor_combined。新 IMU 数据到达后工作队列调度Run()。IMU 驱动发布 │ ▼ uORB subscription callback │ ▼ ScheduledWorkItem::ScheduleNow() │ ▼ EKF2::Run()回调注册失败时源码使用延迟调度进行重试防止模块永久失去执行机会。2. 主循环结构根据 v1.14.3 的EKF2.cpp可以把Run()分为五段退出与回调检查参数和命令处理读取并规范化 IMU更新辅助传感器样本调用 Ekf::update发布导航、状态和诊断结果2.1 退出与回调管理模块收到退出请求后必须注销订阅回调避免工作队列再次调度已准备销毁的对象。初始化阶段则尝试注册 IMU 回调并设置失败重试。2.2 参数与命令parameter_update到达时接口层同步EKF2_*参数并执行边界检查。飞行中参数变化可能影响观测噪声、融合门限和输出时间常数因此参数更新属于实时状态的一部分。vehicle_command可携带设置全局原点等命令。全局原点变化不仅改变经纬度表示还会影响本地位置到全局位置的映射。3. IMU 样本构造两种运行模式的数据来源不同模式输入话题特点多实例vehicle_imu每个实例绑定具体 IMU直接提供积分增量单实例sensor_combined接收选定传感器组合接口层构造增量滤波器消费的是一个采样区间内的角增量和速度增量Δθ∫t0t1ω(t) dt,Δv∫t0t1f(t) dt \Delta\theta\int_{t_0}^{t_1}\omega(t)\,dt, \qquad \Delta v\int_{t_0}^{t_1}f(t)\,dtΔθ∫t0​t1​​ω(t)dt,Δv∫t0​t1​​f(t)dt因此时间戳、delta_ang_dt和delta_vel_dt与测量值本身同等重要。4. 高频路径先更新输出预测器调用_ekf.setIMUData(imu_sample_new)后EstimatorInterface会令OutputPredictor积分到最新 IMU 时间将 IMU 数据送入降采样器达到滤波周期后把累积样本压入主 IMU 缓冲区。姿态可以在完整主 EKF 更新之前发布因为它来自已经积分到当前时刻的输出预测器。该顺序降低了姿态控制链路延迟。5. 辅助传感器更新接口层在 IMU 驱动的执行周期内检查其他订阅UpdateAirspeedSample() UpdateAuxVelSample() UpdateBaroSample() UpdateExtVisionSample() UpdateFlowSample() UpdateGpsSample() UpdateMagSample() UpdateRangeSample() UpdateSystemFlagsSample()“检查到新数据”不等于“立即融合”。更新函数完成单位、坐标和时间戳处理后将样本放入对应缓冲区主 EKF 到达与该测量匹配的延迟时刻后才会消费它。6. 主滤波更新_ekf.update()只有在降采样 IMU 缓冲区产生新样本时才执行有效更新。算法层主干如下constimuSample imu_sample_delayed_imu_buffer.get_oldest();predictCovariance(imu_sample_delayed);predictState(imu_sample_delayed);controlFusionModes(imu_sample_delayed);runTerrainEstimator(imu_sample_delayed);_output_predictor.correctOutputStates(...);这里存在两个不同频率路径典型触发目的输出预测每帧新 IMU获得低延迟姿态、速度、位置主 EKF产生降采样 IMU 时控制计算量并融合延迟观测7. 输出发布分层主滤波成功更新后接口层发布多类结果类别典型话题消费者控制状态vehicle_attitude、vehicle_local_position姿态/位置控制器导航状态vehicle_global_position导航任务与地理围栏环境状态wind固定翼控制与性能估计估计器健康estimator_status_flagsCommander、日志分析观测诊断estimator_aid_src_*调参与故障定位偏差状态estimator_sensor_bias校准与健康监测多实例模式下每个实例先发布estimator_*话题由EKF2Selector选择主实例并转发到全局vehicle_*话题。8. 传感器切换处理接口层检测 IMUdevice_id变化。新的传感器具有不同的零偏统计特性继续沿用旧偏差会造成明显瞬态因此相关偏差状态需要重置或重新初始化。这个处理说明传感器标识也是估计状态有效性的组成部分。9. 实时性分析主循环中的主要计算压力来自协方差预测和观测融合。PX4 通过以下方式控制实时成本IMU 降采样限制主 EKF 更新频率单个观测分量通常顺序标量融合雅可比由符号工具化简后生成条件编译移除未使用的融合模块诊断话题按较低频率发布。10.Run()中的参数更新细节参数更新不是对所有成员做无条件覆盖。典型过程包括parameter_update.updated() │ ├─ updateParams()把参数系统值同步到绑定成员 ├─ VerifyParams()检查枚举、范围和组合合法性 ├─ 更新 EstimatorInterface 参数副本 ├─ 更新传感器最大到达间隔 └─ 更新 OutputPredictor 位置/速度时间常数其中缓冲长度通常在接口初始化阶段确定。飞行中改变延迟参数不一定能无代价地重构所有历史容量因此实际调参需要确认参数是否支持运行时完整生效。输出时间常数则可以直接改变后续误差反馈动态。11. IMU 更新的两条分支多实例vehicle_imu已经包含delta_angle、delta_velocity及各自积分时间单实例sensor_combined主要提供角速度和加速度接口层需要按采样间隔构造增量。两条路径最终统一为imuSample imu_sample_new{};imu_sample_new.time_us...;imu_sample_new.delta_ang...;imu_sample_new.delta_ang_dt...;imu_sample_new.delta_vel...;imu_sample_new.delta_vel_dt...;统一结构使算法层不关心上游选择模式但也要求接口层正确处理时间戳是否对应积分区间末端陀螺与加速度计积分时间是否分别有效clipping 标志是否传递device ID 变化是否触发偏差重置数据间隔异常是否导致积分量不可信。12. 为什么姿态发布位于主 EKF 更新之前setIMUData()内部已经调用 Output Predictor 传播最新 IMU因此PublishAttitude(now)可以在_ekf.update()之前读取最新输出。时序如下t0: 收到新 IMU t1: OutputPredictor 积分到 t0 对应测量时刻 t2: 发布当前姿态 t3: 辅助观测写入各自缓冲 t4: 若降采样完成主 EKF 在 delayed horizon 更新 t5: 主 EKF 修正反馈给 OutputPredictor影响随后输出这种安排使当次辅助观测修正不会立即以阶跃形式进入姿态而是由输出校正器按设计动态吸收。控制器获得较低延迟和较好连续性。13. 发布频率并不完全相同接口层对不同话题采用不同发布条件发布类别触发依据原因姿态每个有效 IMU 更新姿态控制要求最低延迟位置/速度主 EKF 成功更新与滤波状态和有效性同步aid source新融合状态或限频降低日志/uORB 带宽状态向量/协方差调试限频数据量较大偏差校准置信度和时间条件避免频繁写参数因此两个话题时间戳相近并不表示它们在同一代码位置生成。分析实时延迟时需要区分采样时间、发布时间和记录时间。14. 一帧数据的失败路径主循环并非每次都进入融合。常见提前退出或无有效更新的情况包括未读取到新 IMU ───────────────► 不推进输出和主 EKF IMU dt 无效 ──────────────────► 初始化继续等待 降采样尚未达到目标周期 ───────► 只更新 OutputPredictor _imu_updated 为 false ─────────► Ekf::update() 返回 false 主滤波未初始化 ────────────────► 不发布完整导航更新 观测缓冲无匹配样本 ────────────► 只做惯性预测 观测创新被拒绝 ────────────────► aid source 更新但状态不修正这组分支说明“模块正在运行”“EKF 完成一次预测”“某观测完成一次融合”是三个不同事件。日志中应通过 perf counter、time_last_fuse和 aid sourcefused分别确认。15. 计算负载与调度抖动主 EKF 的平均周期由filter_update_interval_us约束predictState()还对实际积分周期做滤波并限制到标称周期的一定范围用于后续噪声和偏差换算。短期工作队列抖动由 IMU 积分时间吸收但长期 CPU 过载会造成IMU 或观测缓冲覆盖辅助数据到达间隔超限输出发布时间抖动多实例健康评分下降时间延迟假设失效。因此性能诊断不能只观察 EKF 单次执行时间还应检查输入丢帧、缓冲状态、工作队列负载和发布时间间隔。16. 辅助传感器更新函数的接口职责UpdateGpsSample()、UpdateBaroSample()等函数不是机械复制 uORB 字段。它们通常承担处理示例时间选择使用timestamp_sample应用接口约定单位转换μs→s、角度→弧度、精度→方差所需尺度坐标转换传感器轴、FRD/NED、视觉 frame安装外参GPS/EV/flow/range 相对 IMU 位置数据有效性NaN、quality、variance、device ID限频/去重只把新样本送入算法层状态附加EV reset counter、GNSS fix 信息完成转换后才调用_ekf.set*Data()。因此算法层创新异常时必须同时检查接口函数中的单位和 frame 处理。直接打印 uORB 原始值与内部 aid source 比较若忽略这些转换可能得出错误结论。系统标志也通过UpdateSystemFlagsSample()构造延迟样本包括at_rest、in_air、is_fixed_wing和gnd_effect。这些状态影响 ZUPT、侧滑、阻力、高度和磁模式。它们与传感器观测一样需要时间对齐而不是直接在 delayed EKF 中读取当前飞行状态。主循环还承担在线校准发布的节流。偏差每次滤波更新都可能变化但参数系统不应每帧写入接口层根据偏差变化、方差、设备 ID 和时间间隔决定是否更新校准。由此可见Run()同时协调快速数据平面和慢速管理平面。验证主循环修改时应记录每秒 IMU 回调数、主 EKF 成功更新数和各话题发布数。三者比例应分别接近原始 IMU 频率、目标滤波频率和各自限频策略比例异常可快速发现回调丢失、降采样未触发或发布条件错误。同时应检查这些计数在参数更新、传感器切换和多实例主从变化前后是否连续防止管理事件意外注销回调或重复注册订阅。任何计数突变都应与事件日志逐时刻对应。源码解读逐段跟踪EKF2::Run()文件src/modules/ekf2/EKF2.cpp。1. 退出分支先注销回调if(should_exit()){_sensor_combined_sub.unregisterCallback();_vehicle_imu_sub.unregisterCallback();return;}即使当前只使用一种 IMU 订阅也同时尝试注销两种可能的回调。return保证退出周期不再读取参数或传感器。2. 参数更新会同步到算法对象if(_parameter_update_sub.updated()||!_callback_registered){parameter_update_s pupdate;_parameter_update_sub.copy(pupdate);updateParams();VerifyParams();_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get()*1_s);_ekf.output_predictor().set_pos_correction_tc(_param_ekf2_tau_pos.get());_ekf.output_predictor().set_vel_correction_tc(_param_ekf2_tau_vel.get());}这里展示了两类参数生效方式大部分参数通过绑定直接写入_params结构需要派生单位或调用对象 setter 的参数在Run()显式同步。例如 GPS 健康时间从秒转换为微秒输出校正时间常数写入OutputPredictor。3. 回调注册决定调度源if(!_callback_registered){#ifdefined(CONFIG_EKF2_MULTI_INSTANCE)if(_multi_mode){_callback_registered_vehicle_imu_sub.registerCallback();}else#endif{_callback_registered_sensor_combined_sub.registerCallback();}if(!_callback_registered){ScheduleDelayed(10_ms);return;}}多实例从vehicle_imu读取绑定 IMU 的积分量单实例从sensor_combined读取系统选定传感器。注册失败时 10 ms 后重试而不是继续执行一个没有可靠触发源的主循环。4. 多实例 IMU 消息转换imu_sample_new.time_usimu.timestamp_sample;imu_sample_new.delta_ang_dtimu.delta_angle_dt*1.e-6f;imu_sample_new.delta_angVector3f{imu.delta_angle};imu_sample_new.delta_vel_dtimu.delta_velocity_dt*1.e-6f;imu_sample_new.delta_velVector3f{imu.delta_velocity};理论变量的源码对应为理论量uORB 字段内部字段测量时刻timestamp_sampleimuSample.time_us(\Delta\theta)delta_angledelta_ang(\Delta t_\theta)delta_angle_dtμsdelta_ang_dts(\Delta v)delta_velocitydelta_vel(\Delta t_v)delta_velocity_dtμsdelta_vel_dts1.e-6f是微秒到秒的单位转换。如果漏掉该转换偏差补偿、重力积分和过程噪声都会相差六个数量级。5. clipping 按轴展开if(imu.delta_velocity_clipping0){imu_sample_new.delta_vel_clipping[0]imu.delta_velocity_clippingvehicle_imu_s::CLIPPING_X;imu_sample_new.delta_vel_clipping[1]imu.delta_velocity_clippingvehicle_imu_s::CLIPPING_Y;imu_sample_new.delta_vel_clipping[2]imu.delta_velocity_clippingvehicle_imu_s::CLIPPING_Z;}uORB 用位掩码表示 clipping算法 sample 使用逐轴布尔数组。后续偏差学习和垂直加速度健康判断可以只抑制受影响轴。6. 消息代数检查发现丢帧constunsignedlast_generation_vehicle_imu_sub.get_last_generation();imu_updated_vehicle_imu_sub.update(imu);if(imu_updated(_vehicle_imu_sub.get_last_generation()!last_generation1)){perf_count(_msg_missed_imu_perf);}generation 不连续说明两次Run()之间至少有一条 IMU 消息未被消费。滤波仍使用最新积分消息继续运行但性能计数器为调度过载或队列延迟提供证据。7. 主处理块的阅读顺序在imu_updated分支继续向下可找到_ekf.setIMUData(imu_sample_new) PublishAttitude(now) UpdateAirspeed/Baro/ExtVision/Flow/Gps/Mag/Range... _ekf.update() PublishLocalPosition/GlobalPosition/Status/Innovations...初学者应在编辑器中对_ekf.setIMUData和_ekf.update分别“跳转到定义”。前者进入EstimatorInterface后者进入Ekf正好跨越接口层与算法层。8. 用断点或日志验证调用频率建议设置三个计数点// 伪代码仅用于说明调试位置count_run;// EKF2::Run() 入口count_set_imu;// imu_updated 分支if(_ekf.update()){count_main_ekf;}若 IMU 为 400 Hz、主滤波目标约 100 Hz则前两者接近 400 Hz第三个接近 100 Hz。该验证能直观说明 Output Predictor 高频、主 EKF 降采样运行的架构。17. 小结EKF2::Run()的关键不是某一条滤波公式而是建立确定的数据时序最新 IMU 先推进低延迟输出辅助观测进入延迟缓冲区主 EKF 在降采样时刻执行预测和融合最后将延迟修正反馈给输出预测器。后续分析任何传感器融合问题时都应先确定它在这条链路中的位置。