无人机 PX4 飞控 | 如何检测状态估计EKF性能
- 前言
- 检查EKF性能
- 缺少pyulog问题解决
- 脚本崩溃没有输出文件
- 生成对应文件
- 结语
前言
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文件。
estimator instance: 0
here
flow sensor check failure.
Using test description loaded from /home/jk-jone/PX4-Autopilot/Tools/ecl_ekf/check_table.csv
Test results written to log_13_2024-8-8-18-27-22.ulg-0.mdat.csv
found innovation data (merged estimator_innovations + estimator_innovation_variances) instance 0
posx and posy should be finite values
Plots saved to log_13_2024-8-8-18-27-22.ulg-0.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可能会标记相应的传感器为故障，并可能切换到备用传感器或模式
