PX4 EKF2 源码解析二接口层、算法层与符号生成层摘要EKF2 的复杂性不仅来自滤波方程还来自实时数据接口、传感器时间对齐、功能裁剪和自动生成代码。本文按照职责将源码划分为 PX4 接口层、EKF 算法层和符号生成层并给出对象关系、数据流与推荐阅读顺序。关键词PX4、EKF2、软件架构、EstimatorInterface、SymForce1. 总体分层┌────────────────────────────────────────────────────────────┐ │ PX4 接口层EKF2 / EKF2Selector │ │ uORB 订阅发布、参数绑定、工作队列、多实例管理、校准回写 │ ├────────────────────────────────────────────────────────────┤ │ EKF 算法层EstimatorInterface / Ekf / OutputPredictor │ │ 延迟缓冲、惯性预测、观测融合、故障检测、地形与输出预测 │ ├────────────────────────────────────────────────────────────┤ │ 符号生成层EKF/python/ekf_derivation │ │ 定义过程与观测模型推导雅可比生成优化的 C 头文件 │ └────────────────────────────────────────────────────────────┘三个层次的运行属性不同接口层和算法层运行于飞控目标板符号生成层只在开发阶段执行其输出作为 C 源码参与编译。2. PX4 接口层EKF2继承ModuleParams和px4::ScheduledWorkItem。前者负责参数系统集成后者负责在工作队列中按 IMU 数据到达事件调度。主要职责包括职责典型对象或函数读取 IMU_vehicle_imu_sub或_sensor_combined_sub读取辅助传感器UpdateGpsSample()、UpdateBaroSample()等设置 EKF 输入_ekf.setIMUData()、setGpsData()等驱动滤波器_ekf.update()发布结果PublishAttitude()、PublishLocalPosition()等参数同步updateParams()、VerifyParams()在线校准UpdateAccelCalibration()等接口层完成数据类型和坐标约定转换但不应承担具体观测雅可比或卡尔曼增益计算。3. EstimatorInterface算法层的数据边界EstimatorInterface将传感器输入转换成滤波器可按时间消费的样本流。其核心结构是一个 IMU 缓冲区和多个观测缓冲区IMU sampleImuDownSamplerIMU RingBufferGNSSGNSS BufferMagMag BufferBaroBaro BufferEV/Flow/Range对应观测 BufferEkf该层使Ekf不需要理解 uORB也不依赖具体 PX4 驱动消息。单元测试中的传感器模拟器可直接调用这些set*Data()接口。4. Ekf主滤波器Ekf继承EstimatorInterface因此同时拥有延迟数据访问能力和滤波状态。Ekf::update()的主干非常短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(...);简短的主干并不意味着算法简单。复杂性被按职责分散到covariance.cpp协方差预测与数值修复control.cpp融合源总调度*_control.cpp单个传感器的状态机*_fusion.cpp观测模型与滤波更新ekf_helper.cpp重置、状态约束和状态查询。5. OutputPredictor控制输出与主 EKF 解耦主 EKF 工作在延迟融合时域而控制器需要接近当前 IMU 时刻的状态。OutputPredictor保存一条高频惯性积分链并将延迟 EKF 的修正平滑传递到当前时刻。主 EKFOutputPredictor在延迟时域计算积分到最新 IMU 时刻包含完整协方差和观测融合不执行完整 EKF 更新更新频率受滤波周期限制每个新 IMU 样本均可更新提供统计一致的基准状态提供低延迟控制状态6. 符号生成层24 维状态的过程雅可比和非线性观测雅可比具有大量重复项。PX4 使用符号工具生成表达式典型关系如下derivation.py │ 定义状态、过程模型、观测模型 ▼ SymForce 符号求导与化简 │ ▼ generated/*.h │ include ▼ covariance.cpp / mag_fusion.cpp / optflow_fusion.cpp ...生成层只负责数学表达式不负责数据质量、启停条件、超时或故障恢复。这些工程逻辑仍由手写的控制文件负责。7. 编译期开关源码中可见大量CONFIG_EKF2_*条件#ifdefined(CONFIG_EKF2_EXTERNAL_VISION)controlExternalVisionFusion();#endif它们用于目标板功能裁剪和运行时EKF2_*参数作用不同机制生效阶段作用CONFIG_EKF2_*编译期是否包含功能代码及相关缓冲区EKF2_*参数运行期已编译功能是否启用及其噪声、门限若固件未编译某项功能仅修改参数不能使该功能出现。8. 推荐源码阅读顺序EKF2::Run() → EstimatorInterface::setIMUData() → Ekf::update() → predictState() / predictCovariance() → controlFusionModes() → 选择一个传感器*_control.cpp → 对应 *_fusion.cpp → measurementUpdate() / fuse() → OutputPredictor::correctOutputStates()该顺序先建立纵向调用链再横向扩展传感器模块可避免一开始陷入大量参数和状态标志。9. 类继承与数据所有权Ekf继承EstimatorInterface意味着主滤波器可以直接访问已经标准化的延迟样本但 uORB 仍被隔离在EKF2顶层。该关系可以概括为EKF2 └─ has-a Ekf └─ is-a EstimatorInterface ├─ owns IMU RingBuffer ├─ owns optional sensor buffers └─ owns time_latest/time_delayed Ekf ├─ owns stateSample _state ├─ owns SquareMatrix24f P ├─ owns OutputPredictor ├─ owns control/fault/reset status └─ owns per-sensor aid source status这种所有权设计对测试十分重要。传感器模拟器不需要启动完整 uORB 系统直接调用setIMUData()、setGpsData()等接口即可驱动同一套Ekf算法。10. 输入接口的统一契约EstimatorInterface中每类传感器均具有独立 sample 结构。尽管字段不同它们共享四项基本契约契约具体要求时间time_us表示经延迟修正后的测量时刻坐标进入算法层前完成轴向和参考系规范化单位使用 SI 单位或源码明确规定的单位方差/质量用于构造观测噪声和控制准入以 GNSS 为例接口层接收经纬度、海拔、NED 速度及精度字段算法层缓冲的gpsSample则提供可直接用于局部投影和融合控制的数据。外部视觉还需要保留 reset counter因为坐标系重定位属于观测语义的一部分。11. 控制文件与融合文件为何分离以磁力计为例mag_control.cpp ├─ 数据是否新鲜 ├─ 磁场是否受干扰 ├─ 使用 heading 还是 3D 模式 ├─ 是否需要 yaw reset └─ 何时 start/stop │ ▼ mag_fusion.cpp ├─ h(x)预测机体系磁场或偏航 ├─ H、S、K ├─ innovation/test ratio └─ measurementUpdate()分离后同一数学融合函数可以在不同控制条件下复用故障恢复也不必侵入符号生成表达式。阅读代码时若只看mag_fusion.cpp无法解释“公式正确但为何从未实际融合”只看mag_control.cpp则无法解释创新和增益如何计算。12. aid source算法状态到诊断接口的桥梁每类观测维护一维、二维或三维estimator_aid_source状态。它既服务内部控制也被接口层发布到 uORB观测值 观测方差 │ 预测状态与 P ─► innovation / innovation_variance │ └──────► test_ratio / rejected / fused / time_last_fuse │ ▼ estimator_aid_src_* 日志因此日志中的 aid source 不是接口层重新计算的摘要而是融合算法本身使用的统计量。调试时可以由日志直接追溯到相应update*AidSrcStatus()和fuse*()。13. 功能裁剪如何贯穿对象布局条件编译不仅包围函数调用也会移除缓冲区、订阅器、状态成员和发布器。例如未定义CONFIG_EKF2_OPTICAL_FLOW时光流控制函数、观测缓冲和 aid source 发布均可能不进入固件。这带来两点工程要求新增融合源时必须同时修改 CMake/Kconfig、接口成员、算法缓冲、控制调度和日志发布对目标板调试前应查看实际构建配置而不能仅依据源码树中存在某文件判断功能可用。编译期开关还会影响 RAM 布局和实例上限。多实例系统中每增加一个可选传感器缓冲其内存成本会按 EKF 实例数放大。14. 从架构定位异常异常首先定位的层次uORB 没有收到传感器消息PX4 接口层/驱动层收到消息但 sample 时间错误Update*Sample()、EstimatorInterfaceaid source 更新但fusedfalse*_control.cpp与创新门限创新计算明显不符物理模型*_fusion.cpp/生成表达式主 EKF 正常而控制输出滞后OutputPredictor与发布链单实例正常、多实例频繁切换EKF2Selector与实例健康评分这种分层定位可以显著减少在错误文件中调整参数或插入日志的成本。15. 对象生命周期与实时内存EKF2 在模块启动阶段创建接口对象、绑定参数并注册回调EstimatorInterface::initialise_interface()才依据参数分配固定容量传感器缓冲。运行阶段避免频繁创建对象或扩容模块构造 ├─ 参数引用和uORB成员构造 ├─ Ekf对象构造 └─ 尚未形成完整传感器历史 │ init(timestamp) ├─ 根据delay/update interval计算buffer length ├─ 为已编译传感器分配RingBuffer ├─ 初始化OutputPredictor history └─ 建立时间基准 │ Run阶段 └─ 只做push/pop/update不动态改变容器规模固定内存使最坏情况资源可在上电时判断。多实例模式中每个Ekf都拥有独立P、状态、IMU 历史、观测缓冲和 Output Predictor内存近似随实例数线性增长共享 uORB 数据并不意味着共享滤波历史。析构或 Stop 流程需要先注销回调再停止调度最后释放对象。若回调仍能触发已销毁实例会产生典型异步生命周期错误。EKF2Selector也必须在实例退出时识别状态话题不再更新而不能继续转发旧主实例数据。接口层和算法层还具有不同的错误报告方式缓冲分配失败属于初始化/资源错误观测创新拒绝属于运行时统计事件协方差非健康属于滤波数值故障。把这些错误统一成一个布尔“EKF failed”会丢失恢复所需信息。从二次开发角度新增成员时应回答其所有权和生命周期是否每实例独立、是否按功能编译、何时分配、是否进入 reset、如何发布诊断、测试如何注入。只有明确这些问题新功能才能维持 EKF2 原有的确定性架构。线程关系上EKF2 主要依赖工作队列串行执行单个实例的Run()从而减少主状态和协方差的并发访问。uORB 回调用于触发调度并不应在回调上下文直接执行完整滤波更新。多实例之间各有工作项Selector 又是独立工作项因此实例输出与 Selector 读取之间仍通过 uORB 消息边界解耦。这种消息边界带来确定的数据快照Selector 不直接读取另一个对象的_state接口发布也不把内部矩阵指针暴露给控制器。代价是需要维护话题实例、时间戳和发布频率。新增跨模块功能时应优先扩展明确的消息契约避免绕过 uORB 共享可变内部状态。消息契约还承担版本兼容责任。算法内部可以重构成员名称但已经被 Commander、控制器和日志工具消费的字段需要同步迁移。新增 fault 或 reset 状态时应明确默认值、多实例转发方式和旧日志解析行为避免算法正确而系统集成语义不完整。相同原则适用于参数接口层负责把稳定的参数标识映射到算法参数结构算法文件不应直接依赖参数服务器。这样单元测试能够构造参数结构回放也能明确记录一次运行所使用的配置快照。源码解读三层架构如何落实到类、目录与调用关系1. 顶层对象通过组合持有算法对象EKF2与Ekf是 has-a 关系Ekf与EstimatorInterface是继承关系。阅读EKF2.hpp和ekf.h可以得到EKF2 └─ member: Ekf _ekf └─ class Ekf : public EstimatorInterface ├─ protected sensor buffers ├─ protected timing state └─ OutputPredictor这解释了为什么顶层可以调用_ekf.setGpsData()该函数定义在基类EstimatorInterface而_ekf.update()定义在派生类Ekf。对初学者而言遇到“在ekf.h找不到setIMUData()声明”时应继续查看父类而不是认为函数由宏生成。2. 输入层的虚函数边界文件EKF/estimator_interface.hclassEstimatorInterface{public:virtualboolcollect_gps(constgpsMessagegps)0;voidsetIMUData(constimuSampleimu_sample);voidsetMagData(constmagSamplemag_sample);voidsetGpsData(constgpsMessagegps);voidsetBaroData(constbaroSamplebaro_sample);};setGpsData()属于通用数据接口但 GNSS 质量统计collect_gps()被声明为纯虚函数由Ekf实现。这一设计使接口层负责缓冲和时间处理同时允许具体滤波器决定哪些 GNSS 统计需要收集。调用关系为EKF2::UpdateGpsSample() └─ _ekf.setGpsData(gps) ├─ EstimatorInterface时间/缓冲处理 └─ virtual collect_gps(gps) └─ Ekf::collect_gps() / gps_checks.cpp理论中的“传感器准入检查”因此跨越接口基类与具体 EKF 实现。3. 条件编译不仅控制调用头文件中可见#ifdefined(CONFIG_EKF2_RANGE_FINDER)#includerange_finder_consistency_check.hpp#includesensor_range_finder.hpp#endif同一宏还包围setRangeData()接口range sensor 成员controlRangeHeightFusion()调用terrain estimator 代码uORBdistance_sensor订阅对应 aid source 发布。所以源码树里存在range_height_control.cpp不代表当前固件一定包含它。分析目标板行为时需要查看Kconfig、板级配置和编译命令。4.Ekf::update()展示算法层内部模块化predictCovariance(imu_sample_delayed);predictState(imu_sample_delayed);controlFusionModes(imu_sample_delayed);#ifdefined(CONFIG_EKF2_RANGE_FINDER)runTerrainEstimator(imu_sample_delayed);#endif_output_predictor.correctOutputStates(/* ... */);这里有三个值得初学者注意的边界地形估计是单独小滤波器不在主 24 维P中Output Predictor 在全部观测融合后接受主状态修正功能编译宏直接决定对象是否参与一次主周期。5. 手写融合与生成代码的连接方式以covariance.cpp为例手写文件会包含生成头文件并在函数内调用生成表达式。整体关系为derivation.py定义 f(x,u) 与符号状态 │ ▼ generated/predict_covariance.h展开数值表达式 │ #include / 函数调用 ▼ covariance.cpp参数限幅、噪声构造、模式抑制、数值修复同理mag_fusion.cpp、optflow_fusion.cpp、airspeed_fusion.cpp会使用各自的 generated 头文件。生成代码解决雅可比和公共子表达式手写文件解决传感器数据是否适合进入这些公式。6. aid source 成员如何连接日志Ekf为各观测维护_aid_src_gnss_pos、_aid_src_gnss_vel、_aid_src_mag等成员。融合文件写入这些结构顶层EKF2::PublishAidSourceStatus()再发布为estimator_aid_src_*。gps_control/vel_pos_fusion └─ 更新 _aid_src_gnss_pos ├─ observation ├─ innovation ├─ innovation_variance ├─ test_ratio └─ fused/time_last_fuse │ ▼ EKF2::PublishAidSourceStatus() │ ▼ estimator_aid_src_gnss_pos uORB/ULog因此调试某个日志字段时应先查 uORB 发布函数确认字段复制再查对应_aid_src_*的更新位置。7. 参数的跨层映射参数在ekf2_params.c定义在EKF2.hpp通过参数包装成员绑定构造时映射到parameters结构算法文件读取_params。可以按以下链路追踪PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110) ▼ EKF2.hpp 参数成员 ▼ EKF2 构造函数_param_...(_params-gps_delay_ms) ▼ EstimatorInterface::setGpsData() ▼ sample.time_us - gps_delay_ms * 1000对于初学者“参数在源码哪里生效”应沿这个链逐级查找而不是只搜索参数字符串算法层通常只出现结构字段名不再出现EKF2_GPS_DELAY。8. 架构阅读检查表对任何新融合源逐项确认层需要找到的源码证据接口uORB subscription、Update*Sample()数据sample 结构、set*Data()、RingBuffer调度controlFusionModes()中的入口控制control*Fusion()的 start/stop/reset数学update*()、fuse*()、generated header状态control flag、fault flag、aid source输出PublishAidSourceStatus()或状态发布测试test_EKF_*.cpp与 sensor simulator这张表把“三层架构”变成可执行的源码定位方法。进一步确认对象边界时可在构造函数中搜索_params-、_ekf和各Subscription的初始化列表参数映射属于接口层构造滤波状态由Ekf自身构造uORB 实例号由EKF2管理。三类成员的初始化位置与其所有权完全对应。还应查看EKF/CMakeLists.txt它列出参与算法库编译的ekf.cpp、control.cpp、covariance.cpp和各融合文件。目录中存在但未加入 source list 的文件不会进入链接结果这一层是从源码结构到最终固件的最后证据。16. 小结EKF2 是实时接口、延迟数据管理、非线性滤波和代码生成共同构成的系统。接口层回答“数据从何处来、结果向何处去”算法层回答“状态如何预测和修正”符号生成层回答“高维雅可比如何可靠实现”。三者边界是后续源码分析的基本坐标。