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 输出,状态和状态数据发布到许多 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
运行报错:
Traceback (most recent call last): File "process_logdata_ekf.py", line 11, in from pyulog import ULog ModuleNotFoundError: No module named 'pyulog'
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文件。