From 9c381a60b585c7f596e0bbe937832bd5a0329049 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 12 Apr 2022 13:44:08 -0400 Subject: [PATCH] Tools/ecl_ekf: fix vibe_metrics usage (moved to vehicle_imu_status instances) --- Tools/ecl_ekf/analysis/checks.py | 2 +- Tools/ecl_ekf/analysis/metrics.py | 30 +++++++---- Tools/ecl_ekf/batch_process_metadata_ekf.py | 56 ++++++++++----------- Tools/ecl_ekf/check_level_dict.csv | 8 +-- Tools/ecl_ekf/check_table.csv | 12 ++--- Tools/ecl_ekf/plotting/pdf_report.py | 36 +++++++++---- 6 files changed, 84 insertions(+), 60 deletions(-) diff --git a/Tools/ecl_ekf/analysis/checks.py b/Tools/ecl_ekf/analysis/checks.py index 9a1b20b96e..bdddb65421 100644 --- a/Tools/ecl_ekf/analysis/checks.py +++ b/Tools/ecl_ekf/analysis/checks.py @@ -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)] \ diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 22b6a61931..9c20a065cd 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -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 diff --git a/Tools/ecl_ekf/batch_process_metadata_ekf.py b/Tools/ecl_ekf/batch_process_metadata_ekf.py index f6a5464dc0..2797c53401 100755 --- a/Tools/ecl_ekf/batch_process_metadata_ekf.py +++ b/Tools/ecl_ekf/batch_process_metadata_ekf.py @@ -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.'], diff --git a/Tools/ecl_ekf/check_level_dict.csv b/Tools/ecl_ekf/check_level_dict.csv index e1ac34451d..97976145e8 100644 --- a/Tools/ecl_ekf/check_level_dict.csv +++ b/Tools/ecl_ekf/check_level_dict.csv @@ -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 diff --git a/Tools/ecl_ekf/check_table.csv b/Tools/ecl_ekf/check_table.csv index b111e233f0..1c02836e69 100644 --- a/Tools/ecl_ekf/check_table.csv +++ b/Tools/ecl_ekf/check_table.csv @@ -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) diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index b4c10ac03f..0c5e037e42 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -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 = {