PX4-Autopilot代码解析（7）-扩展卡尔曼滤波器（EKF2）

扩展卡尔曼滤波器 (EKF2) 模块是 PX4 自动驾驶系统的核心状态估计组件。它负责融合来自多个传感器的数据，以提供对飞行器姿态、位置、速度以及飞行控制所需的其他状态的可靠估计。本文档介绍了 EKF2 系统的架构、组件、操作和配置。

EKF2 采用模块化、可配置、抗传感器故障的设计，适用于各种车辆类型和操作条件。

架构概述

EKF2 模块使用处理估计过程不同方面的类的分层结构来实现。

类层次结构

该架构包括：

1. EKF2 类：PX4 模块接口，用于处理与其他 PX4 模块的通信
2. EstimatorInterface：定义状态估计器接口的基类
3. Ekf 类：扩展卡尔曼滤波器算法的核心实现
4. OutputPredictor：通过在过滤器更新之间传播状态来提供高速率输出
5. 环形缓冲区：存储带有时间戳的传感器数据，用于延迟融合

状态向量

EKF状态向量包含以下组成部分：

状态向量在结构中定义StateSample，包括：

1. 姿态：表示车辆方向的四元数
2. 速度：东北向下（NED）框架中的 3D 速度矢量
3. 位置：NED 框架中的 3D 位置矢量
4. 陀螺仪偏置：陀螺仪的 3D 偏置矢量
5. 加速度计偏差：加速度计的 3D 偏差矢量
6. 可选状态：根据配置，可能包括磁场、风速和地形高度

状态向量的大小和组成是在编译时根据配置选项确定的。

数据流

下图说明了数据如何流经 EKF2 系统：

数据流过程包括：

1. 传感器数据收集：通过 uORB 主题接收传感器数据
2. 时间对齐：数据存储在带有时间戳的环形缓冲区中，以便延迟融合
3. 状态预测：IMU 数据用于及时传播状态
4. 测量融合：融合传感器测量值以校正预测状态
5. 输出发布：估计状态发布到 uORB 主题，供其他模块使用

EKF2 模块在其自己的工作队列上运行，并由新的 IMU 数据触发。

传感器融合

EKF2 模块可以融合来自多个传感器的数据，以提供稳健的状态估计。融合过程由一组标志控制，这些标志决定使用哪些传感器。

融合控制标志

融合控制系统：

1. 根据可用性和质量确定使用哪些传感器
2. 管理不同传感器组合之间的转换
3. 通过回退到替代源来处理传感器故障
4. 当传感器数据有限时，提供劣化估计

Ekf 类中的函数controlFusionModes()负责根据传感器数据质量和可用性管理融合控制标志。

高度融合

EKF2 可以使用各种来源进行高度估计，由以下EKF2_HGT_REF参数控制：

ID	高度传感器	描述	优点	缺点
0	陀螺	气压计（气压高度）	始终可用，稳定	受天气、地面效应影响
1	全球导航卫星系统	GNSS 高度（参考椭球或 MSL）	全球参考，无漂移	需要良好的卫星可见性
2	RangeFinder