PX4 EKF2简介

PX4是一个流行的开源飞控系统，广泛用于无人机和其他自动驾驶飞行器。EKF2（Extended Kalman Filter 2）是PX4中用于状态估计的一个关键组件，它负责融合来自不同传感器的数据，如GPS、IMU（惯性测量单元）、磁力计、气压计等，以提供精确的飞行器位置、速度、姿态和海拔信息。

ECL （Estimation and Control Library，估计和控制库），其中的状态估计使用扩展卡尔曼滤波算法（EKF）处理传感器的测量信息,是一个很成熟很优秀的状态估计模块。

EKF 提供如下状态量的估计值：

• 四元数定义从北东地（NED）局部地球坐标系到 X，Y，Z 机体坐标系的旋转
• IMU 的速度 — 北，东，地 (NED) (m/s)
• IMU 的位置 — 北，东，地 (NED) (m)
• IMU 增量角度偏差估计 — X，Y ，Z (rad)
• IMU 增量速度偏差估计 - X, Y, Z(m/s)
• 地球磁场分量 — 北，东，地 (NED) (gauss)
• 飞行器机体坐标系磁场偏差 — X，Y ，Z (gauss)
• 风速 — 北, 东 (NE) (m/s)

位置及速度状态变量在输出至控制回路之前会根据IMU与机体坐标系之间的偏差量进行修正。 IMU 相对于机体坐标系的位置由 EKF2_IMU_POS_X,Y,Z 参数设置。

EKF 的启动

EKF2 默认是启动的，相关参数为 EKF2_EN，默认为 Enable。

此外PX4还有其他的状态估计器：

• EKF2 姿态，位置和风状态估计器 (推荐的) - 扩展卡尔曼滤波估计姿态， 3D 位置/速度和风的状态。
• LPE 位置估计器 (已弃用) - 用于三维位置与速度状态的扩展卡尔曼滤波器。[LPE 已弃用。 （在开发 PX4 v1.14时）它是工作的，但是不再支持或维护。]
• Q 姿态估计器 - 一个非常简单、基于四元数的互补滤波器。 它不需要 GPS、磁力计或气压计。

如果是希望启动其它状态估计器，那么则需要启用其参数并关闭其它估计器的参数：

• EKF2_EN - EKF2 (默认/推荐)
• ATT_EN - Q 估计器 (基于四元数的姿态估计器)
• LPE_EN - LPE (不支持固定翼)

ecl EKF 的优缺点

缺点

1、ecl EKF 是一种复杂的算法，需要很好地理解扩展卡尔曼滤波器理论及其应用于导航中的问题才能成功调参。 因此，不知道怎么修改，用户就很难得到好结果。

2、ecl EKF 使用更多 RAM 和闪存空间。

3、ecl EKF 使用更多的日志空间。

优点

1、ecl EKF 能够以数学上一致的方式融合来自具有不同时间延迟和数据速率的传感器的数据，一旦正确设置时间延迟参数，就可以提高动态操作期间的准确性。

2、ecl EKF 能够融合各种不同的传感器类型

3、当 ecl EKF 检测并报告传感器数据中统计上显着的不一致性，将帮助诊断传感器错误。

4、对应固定翼而言，ecl EKF 可以在有空速计和没有空速计的情况下，估计风速。如果GPS在飞行中丢失，可以使用估计的风速结合空速测量和侧滑假设来延长可用的航位推算时间。

5、ecl EKF估计3轴加速度计偏差，这提高了尾座式无人机和其它机体在飞行阶段之间经历大的姿态变化时的精度。

运行单个EKF实例

默认的就是运行EKF的单个实例

在这种情况下，EKF收到数据之前执行传感器选择和故障切换。这为防止有限数量的传感器故障，如数据丢失等，提供了保护。但不能防止传感器提供的不准确数据超过EKF和控制循环的补偿能力（传感器的数据不准还是没有办法）。

运行单个EKF实例的参数设置为：

• EKF2_MULTI_IMU = 0 | Multi-EKF使用的最大imu数。设置0为禁用（即运行单个EKF）。
• EKF2_MULTI_MAG = 0 | Multi-EKF使用的最大磁力计数。设置0为禁用（即运行单个EKF）。
• SENS_IMU_MODE = 1 | 传感器hub IMU 的模式。0为发布所以imu数据 （即有几个imu发布几个imu数据）；1为发布主IMU选择（即仅发布1个imu数据）
• SENS_MAG_MODE = 1 | 传感器hub 磁力计 的模式。0为发布所以磁力计 数据 （即有几个磁力计 发布几个imu磁力计 ）；1为发布主磁力计 选择（即仅发布1个磁力计 数据）

运行多个EKF实例

为了冗余，飞行器上一般不会仅有一个IMU，如果传感器出现故障，那么则无法恢复。一般会有两个IMU，一个磁力计，甚至两个磁力计。

例如 CUAV 的 X7+ 飞控 ， 有三组IMU ，和一个磁力计 ，两个气压计

其中ICM-42688-P 的 性能要比后两个ICM-20689的性能要好，与传统的消费类IMU相比，噪声系数降低了40%，且温度稳定性提高了2倍。

在之前的PX4版本中，是一个工作，另一个作为备份，这个实际上是一种浪费。

通过运行多个EKF实例的方法，可以根据 IMU 和磁力计的数量以及自动驾驶仪的 CPU 能力，运行多个 EKF 实例，将 IMU 和磁力计的数据交叉输入以计算估计值。通过每个 EKF 实例使用不同的传感器组合，提供了一系列更广泛的传感器错误的保护。

过比较每个 EKF 实例的内部一致性，EKF 选择器能够确定具有最佳数据一致性的 EKF 和传感器组合。

这样可以检测和隔离IMU偏差、饱和或数据卡住等故障。

EKF实例总数是由 EKF2_MULTI_IMU 和 EKF2_MULTI_MAG 所选择的IMU数量和磁强计数量的乘积，由以下公式给出：
N_instances = MAX(EKF2_MULTI_IMU , 1) x MAX(EKF2_MULTI_MAG , 1)

例如，一个带有 2 个IMU和 2 个磁强计的自动化驾驶仪可以在 EKF2_MULTI_IMU = 2 和 EKF2_MULTI_MAG = 2 的情况下运行，总共 4 个EKF实例，其中每个实例使用以下传感器组合：

• EKF instance 1 : IMU 1, magnetometer 1
• EKF instance 2 : IMU 1, magnetometer 2
• EKF instance 3 : IMU 2, magnetometer 1
• EKF instance 4 : IMU 2, magnetometer 2

可处理的IMU或磁强计传感器的最大数量为每种传感器有4个，因此理论上最大有 4 x 4 = 16 个EKF实例。
实际上，这种做法受到现有计算资源的限制。 实测，使用基于STM32F7的硬件的CPU进行测试，结果显示 4 个EKF实例具有可接受的处理负载和内存利用率裕度。

那么根据上面举的cuav x7+ 的例子，使用多EKF实例，需要进行如下配置：

• EKF2_MULTI_IMU =3
• EKF2_MULTI_MAG = 1 （SENS_MAG_MODE 需要被设置为0，才会有这个参数，如果SENS_MAG_MODE为1 ，则搜不到EKF2_MULTI_MAG，其值也就是ECL （Estimation and Control Library，估计和控制库），其中的状态估计使用扩展卡尔曼滤波算法（EKF）处理传感器的测量信息,是一个很成熟很优秀的状态估计模块。

EKF 提供如下状态量的估计值：

• 四元数定义从北东地（NED）局部地球坐标系到 X，Y，Z 机体坐标系的旋转
• IMU 的速度 — 北，东，地 (NED) (m/s)
• IMU 的位置 — 北，东，地 (NED) (m)
• IMU 增量角度偏差估计 — X，Y ，Z (rad)
• IMU 增量速度偏差估计 - X, Y, Z(m/s)
• 地球磁场分量 — 北，东，地 (NED) (gauss)
• 飞行器机体坐标系磁场偏差 — X，Y ，Z (gauss)
• 风速 — 北, 东 (NE) (m/s)

位置及速度状态变量在输出至控制回路之前会根据IMU与机体坐标系之间的偏差量进行修正。 IMU 相对于机体坐标系的位置由 EKF2_IMU_POS_X,Y,Z 参数设置。1）

• SENS_IMU_MODE = 0
• SENS_MAG_MODE = 1

如果使用的是上面这个型号飞控，则建议进行如下设置

判断和选择当前工作的 EKF 实例的方法：
为每一个实例维护两个信息测试率

• 组合测试率(combined_test_ratio) ， 是速度和位置平均测试率和高度测试率的最大值，通过这个可以知道各个EKF实例的质量情况
• 相对测试率 (relative_test_ratio) ，是在多实例 EKF 中与当前实例的组合测试率有最大差值的测试率，通过这个可以知道最佳候选实例

最佳候选实例超过 10 s 稳定好于当前工作实例，则将其切换为当前工作实例。
这样，外部用户就可以得到连续且稳定的最佳估计值。