无人机 PX4 飞控 | 如何检测状态估计EKF性能

sunflower2018

前言

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估计状态的性能好坏和很多因素有关，例如振动、传感器噪声、信号丢失等等，下面介绍如何通过PX4输出的日志数据，来进行EKF性能分析。

检查EKF性能

EKF 输出，状态和状态数据发布到许多 uORB 主题，这些主题在飞行期间记录到 SD 卡上。以.ulog文件格式保存成飞行日志文件。

该文件可以通过 PX4 pyulog libary 解析。

大部分的EKF数据在 EstimatorInnovations 和 EstimateStatus 的uORB消息里，存入.ulog文件。

在PX4的文件目录下面有一个Python的脚本，名为process_logdata_ekf.py，具体路径为 PX4-Autopilot/Tools/ecl_ekf/process_logdata_ekf.py

该脚本可以自动生成分析的图形曲线以及metadata文件。

使用的话就是cd 到文件的目录下（提前将日志文件拷入），然后输入：

python process_logdata_ekf.py <log_file.ulg>

可能会遇到下面两个问题，提供了解决办法。

缺少pyulog问题解决

运行报错：

Traceback (most recent call last): File "process_logdata_ekf.py", line 11, in from pyulog import ULog ModuleNotFoundError: No module named 'pyulog'



需要通过pip 进行该模块安装

python process_logdata_ekf.py log_13_2024-8-8-18-27-22.ulg

再次执行，可以正常运行

python process_logdata_ekf.py log_13_2024-8-8-18-27-22.ulg

终端输出

found estimator_selector_status (multi-ekf) data 3 ekf instances Using test criteria loaded from /home/jk-jone/PX4-Autopilot/Tools/ecl_ekf/check_level_dict.csv estimator instance: 0 'reject_mag_x'



脚本崩溃没有输出文件

没有正常的运行完脚本生成分析文件， 只输出了一个关键信息 reject_mag_x 并没有输出自动分析的图形曲线以及metadata文件。

看github上有一样的问题的，有人提出了解决方法，但是没有被官方更新到脚本中。

需要更改Tools/ecl_ekf 路径下的几个脚本文件：



mean_metric = '{:s}_mean_warn'.format(imu_vibr_metric)        peak_metric = '{:s}_peak_warn'.format(imu_vibr_metric)        mean_key = '{:s}_mean'.format(imu_vibr_metric)        peak_key = '{:s}_peak'.format(imu_vibr_metric)        if mean_key in imu_metrics and peak_key in imu_metrics:            if imu_metrics[mean_key] > check_levels[mean_metric] \                   or imu_metrics[peak_key] > check_levels[peak_metric]:                imu_status['imu_vibration_check'] = 'Warning'



                                      ('magx', 'fs_bad_mag_x', 'magx_fail_percentage'),                                      ('magy', 'fs_bad_mag_y', 'magy_fail_percentage'),                                      ('magz', 'fs_bad_mag_z', 'magz_fail_percentage'),



       ['fs_bad_mag_x', 'fs_bad_mag_y', 'fs_bad_mag_z',

再次运行则成功生成了对应文件。

生成对应文件

成功运行脚本后，会在脚本路径下面生成几个文件，分别是EKF实例数的对应的csv文件和plots pdf文件。



运行的日志，是用光流在室内飞行的，在pdf中可以看到 Optical Flow Innovations。



在生成的pdf中，有很多的innovations。

innovations 在 EKF 中什么意思？Innovations在EKF（Extended Kalman Filter，扩展卡尔曼滤波器）中是一个关键概念，它代表了预测步骤后的残差或创新量。在滤波过程中，EKF首先通过数学模型对系统状态进行预测，然后将当前观测值与这个预测进行比较。创新（Innovation）就是观测值减去预测值的结果，它可以用来评估预测的准确度，并用于估计状态更新中的协方差矩阵变化。

当创新较小或者接近零时，说明预测与实际观测吻合得较好；如果创新较大，则可能意味着模型需要调整或者噪声影响较大，这时会利用创新来校正系统的状态估计。简而言之，Innovations是衡量传感器数据对系统动态估计贡献的重要工具。

结语

生成的PDF中，还有很多其它数据曲线，可以用来分析EKF的状态和性能。

例如 test ratio Test Ratio：这是一个统计测试的比值，用于确定创新是否在预期的统计分布内。通常，这个比值是通过将创新值除以其标准差来计算的。这个比率用于以下目的：

1、检测异常：如果测试比率超过了某个阈值（通常是2.0或3.0），这可能表明速度测量存在异常，可能是由于传感器故障、外部干扰或其他问题。

2、数据质量评估：一个较低的测试比率表明EKF的速度估计与实际测量值非常接近，这意味着数据质量良好。相反，一个较高的测试比率可能表明EKF对速度的估计不准确。

3、故障检测：在PX4中，EKF会使用这个比率来检测可能的传感器故障。如果测试比率连续超过阈值，EKF可能会标记相应的传感器为故障，并可能切换到备用传感器或模式。



来源：古月居