PX4 EKF2 源码解析（二）：接口层、算法层与符号生成层-育师

PX4 EKF2 源码解析（二）：接口层、算法层与符号生成层

摘要

EKF2 的复杂性不仅来自滤波方程，还来自实时数据接口、传感器时间对齐、功能裁剪和自动生成代码。本文按照职责将源码划分为 PX4 接口层、EKF 算法层和符号生成层，并给出对象关系、数据流与推荐阅读顺序。

关键词：PX4、EKF2、软件架构、EstimatorInterface、SymForce

1. 总体分层

┌────────────────────────────────────────────────────────────┐ │ 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 缓冲区和多个观测缓冲区：

该层使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 的修正平滑传递到当前时刻。

主 EKF	OutputPredictor
在延迟时域计算	积分到最新 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()`、`EstimatorInterface`
aid source 更新但`fused=false`	`*_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.h

classEstimatorInterface{public:virtualboolcollect_gps(constgpsMessage&gps)=0;voidsetIMUData(constimuSample&imu_sample);voidsetMagData(constmagSample&mag_sample);voidsetGpsData(constgpsMessage&gps);voidsetBaroData(constbaroSample&baro_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)#include"range_finder_consistency_check.hpp"#include"sensor_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 是实时接口、延迟数据管理、非线性滤波和代码生成共同构成的系统。接口层回答“数据从何处来、结果向何处去”，算法层回答“状态如何预测和修正”，符号生成层回答“高维雅可比如何可靠实现”。三者边界是后续源码分析的基本坐标。

PX4 EKF2 源码解析（二）：接口层、算法层与符号生成层