专栏名称: 新机器视觉
最前沿的机器视觉与计算机视觉技术
目录
相关文章推荐
51好读  ›  专栏  ›  新机器视觉

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

新机器视觉  · 公众号  ·  · 2024-09-16 22:35

正文



前言


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 


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



缺少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文件。








请到「今天看啥」查看全文