dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Tools/ecl_ekf: fix vibe_metrics usage (moved to vehicle_imu_status instances)

This commit is contained in:
Daniel Agar 2022-04-12 13:44:08 -04:00
parent b4158c1b48
commit 9c381a60b5
6 changed files with 84 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/ecl_ekf/analysis/checks.py
View File

@ -55,7 +55,7 @@ def perform_imu_checks(
# perform the vibration check
imu_status['imu_vibration_check'] = 'Pass'
for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']:
for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']:
mean_metric = '{:s}_mean'.format(imu_vibr_metric)
peak_metric = '{:s}_peak'.format(imu_vibr_metric)
if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \

30
Tools/ecl_ekf/analysis/metrics.py
View File

@ -144,17 +144,27 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:
imu_metrics[result] = calculate_stat_from_signal(
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)
# calculates peak and mean for IMU vibration checks
for signal, result in [('vibe[0]', 'imu_coning'),
('vibe[1]', 'imu_hfdang'),
('vibe[2]', 'imu_hfdvel')]:
peak = calculate_stat_from_signal(
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax)
if peak > 0.0:
imu_metrics['{:s}_peak'.format(result)] = peak
imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(
estimator_status_data, 'estimator_status', signal,
in_air_no_ground_effects, np.mean)
for imu_status_instance in range(4):
try:
vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data
if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]:
for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
('gyro_vibration_metric', 'imu_hfgyro'),
('accel_vibration_metric', 'imu_hfaccel')]:
peak = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.amax)
if peak > 0.0:
imu_metrics['{:s}_peak'.format(result)] = peak
imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.mean)
except:
pass
# IMU bias checks
estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data

56
Tools/ecl_ekf/batch_process_metadata_ekf.py
View File

@ -48,7 +48,7 @@ for filename in os.listdir(metadata_directory):
# # print out the check levels
# print('\n'+'The following metadata loaded from '+filename+' were used'+'\n')
# val = population_data.get(filename, {}).get('imu_hfdang_mean')
# val = population_data.get(filename, {}).get('imu_hfgyro_mean')
# print(val)
# Open pdf file for plotting
@ -90,10 +90,10 @@ population_results = {
'ofy_fail_pct_avg':[float('NaN'),'The mean percentage of innovation test fails for the Y axis optical flow sensor'],
'imu_coning_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU delta angle coning vibration level (mrad)'],
'imu_coning_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta angle coning vibration level (mrad)'],
'imu_hfdang_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta angle vibration level (mrad)'],
'imu_hfdang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta angle vibration level (mrad)'],
'imu_hfdvel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta velocity vibration level (m/s)'],
'imu_hfdvel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta velocity vibration level (m/s)'],
'imu_hfgyro_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency gyro vibration level (rad/s)'],
'imu_hfgyro_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency gyro vibration level (rad/s)'],
'imu_hfaccel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency accel vibration level (m/s/s)'],
'imu_hfaccel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency accel vibration level (m/s/s)'],
'obs_ang_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer angular tracking error magnitude (mrad)'],
'obs_vel_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer velocity tracking error magnitude (m/s)'],
'obs_pos_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer position tracking error magnitude (m)'],
@ -360,54 +360,54 @@ if (len(result1) > 0 and len(result2) > 0):
plt.close(8)
# IMU high frequency delta angle vibration levels
temp = np.asarray([population_data[k].get('imu_hfdang_peak') for k in found_keys])
temp = np.asarray([population_data[k].get('imu_hfgyro_peak') for k in found_keys])
result1 = 1000.0 * temp[np.isfinite(temp)]
temp = np.asarray([population_data[k].get('imu_hfdang_mean') for k in found_keys])
temp = np.asarray([population_data[k].get('imu_hfgyro_mean') for k in found_keys])
result2 = 1000.0 * temp[np.isfinite(temp)]
if (len(result1) > 0 and len(result2) > 0):
population_results['imu_hfdang_max_avg'][0] = np.mean(result1)
population_results['imu_hfdang_mean_avg'][0] = np.mean(result2)
population_results['imu_hfgyro_max_avg'][0] = np.mean(result1)
population_results['imu_hfgyro_mean_avg'][0] = np.mean(result2)
plt.figure(9,figsize=(20,13))
plt.subplot(2,1,1)
plt.hist(result1)
plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Peak")
plt.xlabel("imu_hfdang_max (mrad)")
plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Peak")
plt.xlabel("imu_hfgyro_max (rad/s)")
plt.ylabel("Frequency")
plt.subplot(2,1,2)
plt.hist(result2)
plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Mean")
plt.xlabel("imu_hfdang_mean (mrad)")
plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Mean")
plt.xlabel("imu_hfgyro_mean (rad/s)")
plt.ylabel("Frequency")
pp.savefig()
plt.close(9)
# IMU high frequency delta velocity vibration levels
temp = np.asarray([population_data[k].get('imu_hfdvel_peak') for k in found_keys])
# IMU high frequency accel vibration levels
temp = np.asarray([population_data[k].get('imu_hfaccel_peak') for k in found_keys])
result1 = temp[np.isfinite(temp)]
temp = np.asarray([population_data[k].get('imu_hfdvel_mean') for k in found_keys])
temp = np.asarray([population_data[k].get('imu_hfaccel_mean') for k in found_keys])
result2 = temp[np.isfinite(temp)]
if (len(result1) > 0 and len(result2) > 0):
population_results['imu_hfdvel_max_avg'][0] = np.mean(result1)
population_results['imu_hfdvel_mean_avg'][0] = np.mean(result2)
population_results['imu_hfaccel_max_avg'][0] = np.mean(result1)
population_results['imu_hfaccel_mean_avg'][0] = np.mean(result2)
plt.figure(10,figsize=(20,13))
plt.subplot(2,1,1)
plt.hist(result1)
plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Peak")
plt.xlabel("imu_hfdvel_max (m/s)")
plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Peak")
plt.xlabel("imu_hfaccel_max (m/s/s)")
plt.ylabel("Frequency")
plt.subplot(2,1,2)
plt.hist(result2)
plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Mean")
plt.xlabel("imu_hfdvel_mean (m/s)")
plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Mean")
plt.xlabel("imu_hfaccel_mean (m/s/s)")
plt.ylabel("Frequency")
pp.savefig()
@ -535,12 +535,12 @@ single_log_results = {
'hgt_sensor_status':['Pass','Height sensor check summary. This sensor data can be sourced from either Baro, GPS, range fidner or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
'hgt_test_max':[float('NaN'),'The maximum in-flight value of the height sensor innovation consistency test ratio.'],
'hgt_test_mean':[float('NaN'),'The mean in-flight value of the height sensor innovation consistency test ratio.'],
'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad)'],
'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad)'],
'imu_hfdang_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
'imu_hfdang_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
'imu_hfdvel_mean':[float('NaN'),'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
'imu_hfdvel_peak':[float('NaN'),'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
'imu_hfgyro_mean':[float('NaN'),'Mean in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
'imu_hfgyro_peak':[float('NaN'),'Peak in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
'imu_hfaccel_mean':[float('NaN'),'Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
'imu_hfaccel_peak':[float('NaN'),'Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
'imu_sensor_status':['Pass','IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
'in_air_transition_time':[float('NaN'),'The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected.'],
'mag_percentage_amber':[float('NaN'),'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5.'],

8
Tools/ecl_ekf/check_level_dict.csv
View File

@ -21,10 +21,10 @@ hagl_amber_warn_pct,5.0
tas_amber_warn_pct,5.0
imu_coning_peak_warn,1.8E-5
imu_coning_mean_warn,3.6E-6
imu_hfdang_peak_warn,3.0E-3
imu_hfdang_mean_warn,6.0E-4
imu_hfdvel_peak_warn,9.0E-2
imu_hfdvel_mean_warn,1.8E-2
imu_hfgyro_peak_warn,12
imu_hfgyro_mean_warn,2.4
imu_hfaccel_peak_warn,360
imu_hfaccel_mean_warn,72
obs_ang_err_median_warn,8.0E-3
obs_vel_err_median_warn,0.05
obs_pos_err_median_warn,0.15

1 check_id threshold
21 tas_amber_warn_pct 5.0
22 imu_coning_peak_warn 1.8E-5
23 imu_coning_mean_warn 3.6E-6
24 imu_hfdang_peak_warn imu_hfgyro_peak_warn 3.0E-3 12
25 imu_hfdang_mean_warn imu_hfgyro_mean_warn 6.0E-4 2.4
26 imu_hfdvel_peak_warn imu_hfaccel_peak_warn 9.0E-2 360
27 imu_hfdvel_mean_warn imu_hfaccel_mean_warn 1.8E-2 72
28 obs_ang_err_median_warn 8.0E-3
29 obs_vel_err_median_warn 0.05
30 obs_pos_err_median_warn 0.15

12
Tools/ecl_ekf/check_table.csv
View File

@ -49,12 +49,12 @@ hagl_test_mean, The mean in-flight value of the height above ground sensor innov
ofx_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test.
ofy_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test.
filter_faults_max, Largest recorded value of the filter internal fault bitmask. Should always be zero.
imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad)
imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad)
imu_hfdang_peak, Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)
imu_hfdang_mean, Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)
imu_hfdvel_peak, Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
imu_hfdvel_mean, Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)
imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)
imu_hfgyro_peak, Peak in-flight value of the IMU accel high frequency vibration metric (rad/s)
imu_hfgyro_mean, Mean in-flight value of the IMU accel high frequency vibration metric (rad/s)
imu_hfaccel_peak, Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)
imu_hfaccel_mean, Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)
output_obs_ang_err_median, Median in-flight value of the output observer angular error (rad)
output_obs_vel_err_median, Median in-flight value of the output observer velocity error (m/s)
output_obs_pos_err_median, Median in-flight value of the output observer position error (m)

1 check_id check_description
49 ofx_fail_percentage The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test.
50 ofy_fail_percentage The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test.
51 filter_faults_max Largest recorded value of the filter internal fault bitmask. Should always be zero.
52 imu_coning_peak Peak in-flight value of the IMU delta angle coning vibration metric (rad) Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)
53 imu_coning_mean Mean in-flight value of the IMU delta angle coning vibration metric (rad) Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)
54 imu_hfdang_peak imu_hfgyro_peak Peak in-flight value of the IMU delta angle high frequency vibration metric (rad) Peak in-flight value of the IMU accel high frequency vibration metric (rad/s)
55 imu_hfdang_mean imu_hfgyro_mean Mean in-flight value of the IMU delta angle high frequency vibration metric (rad) Mean in-flight value of the IMU accel high frequency vibration metric (rad/s)
56 imu_hfdvel_peak imu_hfaccel_peak Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s) Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)
57 imu_hfdvel_mean imu_hfaccel_mean Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s) Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)
58 output_obs_ang_err_median Median in-flight value of the output observer angular error (rad)
59 output_obs_vel_err_median Median in-flight value of the output observer velocity error (m/s)
60 output_obs_pos_err_median Median in-flight value of the output observer position error (m)

36
Tools/ecl_ekf/plotting/pdf_report.py
View File

@ -250,18 +250,32 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
data_plot.save()
data_plot.close()
# Plot the EKF IMU vibration metrics
scaled_estimator_status = {'vibe[0]': 1000. * estimator_status['vibe[0]'],
'vibe[1]': 1000. * estimator_status['vibe[1]'],
'vibe[2]': estimator_status['vibe[2]']
}
data_plot = CheckFlagsPlot(
status_time, scaled_estimator_status, [['vibe[0]'], ['vibe[1]'], ['vibe[2]']],
x_label='time (sec)', y_labels=['Del Ang Coning (mrad)', 'HF Del Ang (mrad)',
'HF Del Vel (m/s)'], plot_title='IMU Vibration Metrics',
pdf_handle=pdf_pages, annotate=True)
data_plot.save()
data_plot.close()
for imu_status_instance in range(4):
try:
vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data
imu_status_time = 1e-6 * vehicle_imu_status_data['timestamp']
if vehicle_imu_status_data['accel_device_id'][0] == estimator_status['accel_device_id'][0]:
scaled_estimator_status = {'delta_angle_coning_metric': 1000. * vehicle_imu_status_data['delta_angle_coning_metric'],
'gyro_vibration_metric': vehicle_imu_status_data['gyro_vibration_metric'],
'accel_vibration_metric': vehicle_imu_status_data['accel_vibration_metric']
}
data_plot = CheckFlagsPlot(
imu_status_time, scaled_estimator_status, [['delta_angle_coning_metric'], ['gyro_vibration_metric'], ['accel_vibration_metric']],
x_label='time (sec)',
y_labels=['Del Ang Coning (mrad^2)', 'HF Gyro (rad/s)', 'HF accel (m/s/s)'],
plot_title='IMU Vibration Metrics',
pdf_handle=pdf_pages, annotate=True)
data_plot.save()
data_plot.close()
except:
pass
# Plot the EKF output observer tracking errors
scaled_innovations = {