PX4 EKF2 目录与层级架构

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 930 阅读时长 ≈ 3 分钟
全面梳理 PX4 EKF2 模块从输入层到输出层的完整目录结构，说明延迟时间融合架构的核心设计思想，并列出各层级文件的功能定位。

一、延迟时间融合架构

EKF2 采用延迟时间融合架构，核心思想是：

  • IMU 数据驱动高频预测（1 kHz → 降采样至 250 Hz）
  • 多传感器数据在各自的延迟时刻点进行融合（GPS 延迟 ~200 ms，视觉延迟 ~50 ms）
  • OutputPredictor 将延迟时刻的状态外推到当前时刻，供控制器使用

这样设计的原因：各传感器的延迟不同，直接用当前时刻的 GPS 数据校正当前 IMU 状态会引入系统性时间错误。通过 RingBuffer 保存 IMU 历史数据，在 GPS 数据到达时回溯找到对应时刻的 IMU 状态进行融合，保证时间域的一致性。

相关基础：uORB 通信机制（上），模块入口与运行模式

二、整体架构流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
flowchart TB
    subgraph input["输入层 - uORB订阅"]
        IMU["sensor_combined / vehicle_imu<br/>IMU加速度计+陀螺仪"]
        GPS["vehicle_gps_position<br/>GNSS位置/速度"]
        MAG["vehicle_magnetometer<br/>磁力计"]
        BARO["vehicle_air_data<br/>气压计"]
        FLOW["vehicle_optical_flow<br/>光流"]
        RNG["distance_sensor<br/>测距仪"]
        EV["vehicle_visual_odometry<br/>外部视觉VIO/MoCap"]
    end

    subgraph wrapper["EKF2 PX4包装层"]
        EKF2CPP["EKF2.cpp<br/>Run()主循环<br/>传感器数据采集+格式转换+结果发布"]
        SELECTOR["EKF2Selector.cpp<br/>多实例选择器<br/>选择最优EKF输出"]
    end

    subgraph interface["数据接口层"]
        ESTI["estimator_interface.cpp<br/>setIMUData / setGpsData等<br/>RingBuffer延迟缓冲"]
        DOWNSAMP["imu_down_sampler.cpp<br/>IMU降采样 1kHz→250Hz"]
    end

    subgraph core["EKF核心算法层"]
        EKF_UPDATE["ekf.cpp<br/>update()主循环<br/>initialiseFilter()初始化<br/>predictState()状态预测"]
        COV["covariance.cpp<br/>predictCovariance()<br/>P=F*P*F'+Q"]
        CONTROL["control.cpp<br/>controlFusionModes()<br/>传感器融合调度中心"]
        HELPER["ekf_helper.cpp<br/>measurementUpdate()<br/>卡尔曼增益计算"]
    end

    subgraph publish["输出层 - uORB发布"]
        ATT["vehicle_attitude<br/>姿态四元数"]
        LPOS["vehicle_local_position<br/>本地NED位置/速度"]
        GPOS["vehicle_global_position<br/>全局经纬度/海拔"]
        STATUS["estimator_status等<br/>估计器健康状态"]
    end

    IMU --> EKF2CPP
    GPS --> EKF2CPP
    MAG --> EKF2CPP
    BARO --> EKF2CPP
    EKF2CPP --> ESTI
    ESTI --> DOWNSAMP
    DOWNSAMP --> EKF_UPDATE
    EKF_UPDATE --> COV
    EKF_UPDATE --> CONTROL
    CONTROL --> HELPER
    EKF_UPDATE --> wrapper
    wrapper --> SELECTOR
    SELECTOR --> ATT
    SELECTOR --> LPOS
    SELECTOR --> GPOS
    EKF2CPP --> STATUS

三、目录结构与功能

3.1 顶层目录 /src/modules/ekf2/

文件 功能
EKF2.cpp PX4 包装层主入口，Run() 主循环：采集 uORB 传感器数据，调用 EKF 库，发布估计结果
EKF2.hpp EKF2 类定义，所有 uORB 订阅/发布声明
EKF2Selector.cpp 多实例选择器，管理多个 EKF 实例（每个 IMU 一个），基于误差评分选择最优输出
Kconfig 模块编译配置，控制各传感器融合功能的启用/禁用

3.2 EKF 核心算法 /EKF/

核心文件

文件 功能
common.h 公共数据结构：imuSample、gnssSample 等样本结构体，参数结构体，枚举
ekf.h / ekf.cpp EKF 主循环：init()、update()、predictState()、initialiseFilter()
estimator_interface.h/.cpp 数据接口基类，传感器数据接收、RingBuffer 延迟缓冲、IMU 降采样
control.cpp 融合调度中心：controlFusionModes() 协调各传感器融合时机
covariance.cpp 协方差矩阵管理：初始化、预测（P=FPF’+Q）、约束、重置
RingBuffer.h 环形缓冲区模板类，所有传感器数据的延迟缓冲
ekf_helper.cpp 辅助函数：measurementUpdate() 卡尔曼更新、状态重置、精度报告

状态向量 /python/ekf_derivation/generated/state.h

24 维状态向量（EKF 内部操作 24 维，含地形则 25 维）：

分量 维数 说明
四元数（旋转向量误差） 3 姿态
速度（NED） 3 本地坐标系速度
位置（NED） 3 本地坐标系位置
陀螺仪零偏 3 角速度传感器偏差估计
加速度计零偏 3 加速度传感器偏差估计
地球磁场（NED） 3 磁场矢量
磁偏差 3 机体磁场偏差
风速（NE） 2 水平风速估计
地形高度（可选） 1 HAGL

3.3 辅助源融合 /EKF/aid_sources/

子目录 传感器 主控文件
gnss/ GNSS/GPS gps_control.cpp、gps_checks.cpp、gnss_height_control.cpp、gnss_yaw_control.cpp
magnetometer/ 磁力计 mag_control.cpp、mag_fusion.cpp
barometer/ 气压计 baro_height_control.cpp
optical_flow/ 光流 optical_flow_control.cpp、optical_flow_fusion.cpp
range_finder/ 测距仪 range_height_control.cpp、range_height_fusion.cpp
external_vision/ 外部视觉 ev_control.cpp（+ pos/vel/height/yaw 子文件）
airspeed/ 空速 airspeed_fusion.cpp
drag/ 阻力 drag_fusion.cpp（多旋翼风速估计）

3.4 通用融合函数

文件 功能
height_control.cpp 高度融合总调度：多高度源选择（气压/GPS/测距/EV）、参考切换
velocity_fusion.cpp 通用速度融合函数
position_fusion.cpp 通用位置融合函数
yaw_fusion.cpp 通用航向融合函数
terrain_control.cpp 地形估计：初始化、伪融合、有效性判断
wind.cpp 风速估计：外部观测重置、协方差管理

3.5 支撑模块

文件 功能
EKFGSF_yaw.cpp/h EKF-GSF 航向估计器：多个并行 3 状态 EKF + AHRS，高斯和滤波加权，不依赖磁力计
output_predictor.cpp/h 输出预测器：IMU 实时积分 + 互补滤波校正（延迟 EKF 状态），提供低延迟输出
bias_estimator.cpp/hpp 通用偏差估计器（用于气压/EV 高度偏差）
generated/*.h Symforce 自动生成的雅可比矩阵 H、新息方差计算、协方差预测公式（25 个头文件）

四、核心处理时序

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
sequenceDiagram
    participant Sensor as 传感器驱动
    participant EKF2 as EKF2.cpp
    participant Buffer as RingBuffer
    participant Core as ekf.cpp
    participant Control as control.cpp
    participant AidSrc as aid_sources/*
    participant Output as output_predictor

    Sensor->>EKF2: uORB回调("IMU数据到达")
    EKF2->>EKF2: Run()主循环
    EKF2->>Buffer: push("imu_downsampled")
    EKF2->>Buffer: push("sensor_sample, GPS/Mag等")
    Output->>Output: calculateOutputStates() 实时积分（无延迟）
    EKF2->>Core: update()
    Core->>Buffer: get_oldest() 获取延迟时刻IMU
    Core->>Core: predictState() 状态预测
    Core->>Core: predictCovariance() P=FPF'+Q
    Core->>Control: controlFusionModes()
    Control->>AidSrc: controlGpsFusion()等
    AidSrc->>Buffer: pop_first_older_than() 获取延迟传感器数据
    AidSrc->>Core: measurementUpdate() 卡尔曼更新
    Core->>Output: correctOutputStates() 用延迟修正量校正实时积分
    EKF2->>EKF2: PublishAttitude / LocalPosition / ...

下一篇：common.h 数据结构
系列总目录：PX4 v1.16 源码解读总目录