diff --git a/Tools/ecl_ekf/batch_process_metadata_ekf.py b/Tools/ecl_ekf/batch_process_metadata_ekf.py index 981ca90962..25fb87330e 100644 --- a/Tools/ecl_ekf/batch_process_metadata_ekf.py +++ b/Tools/ecl_ekf/batch_process_metadata_ekf.py @@ -94,12 +94,9 @@ population_results = { 'imu_hfdang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta hihg 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)'], -'obs_ang_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the output observer angular tracking error magnitude (mrad)'], -'obs_ang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the output observer angular tracking error magnitude (mrad)'], -'obs_vel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the output observer velocity tracking error magnitude (m/s)'], -'obs_vel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the output observer velocity tracking error magnitude (m/s)'], -'obs_pos_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the output observer position tracking error magnitude (m)'], -'obs_pos_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the output observer position tracking error magnitude (m)'], +'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)'], } # get population summary statistics @@ -171,6 +168,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() +plt.close(1) # Velocity Sensor (GPS) temp = np.asarray([population_data[k].get('vel_test_max') for k in found_keys]) @@ -197,6 +195,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(2) # Position Sensor (GPS or external vision) temp = np.asarray([population_data[k].get('pos_test_max') for k in found_keys]) @@ -223,6 +222,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(3) # Height Sensor temp = np.asarray([population_data[k].get('hgt_test_max') for k in found_keys]) @@ -249,6 +249,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(4) # Airspeed Sensor temp = np.asarray([population_data[k].get('tas_test_max') for k in found_keys]) @@ -275,6 +276,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(5) # Height Above Ground Sensor temp = np.asarray([population_data[k].get('hagl_test_max') for k in found_keys]) @@ -301,6 +303,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(6) # Optical Flow Sensor temp = np.asarray([population_data[k].get('ofx_fail_percentage') for k in found_keys]) @@ -327,7 +330,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() - + plt.close(7) # IMU coning vibration levels temp = np.asarray([population_data[k].get('imu_coning_peak') for k in found_keys]) @@ -354,6 +357,7 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + 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]) @@ -380,6 +384,7 @@ if (len(result1) > 0 and len(result2) > 0): 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]) @@ -406,84 +411,58 @@ if (len(result1) > 0 and len(result2) > 0): plt.ylabel("Frequency") pp.savefig() + plt.close(10) # Output Observer Angular Tracking -temp = np.asarray([population_data[k].get('output_obs_ang_err_peak') for k in found_keys]) -result1 = 1000.0 * temp[np.isfinite(temp)] -temp = np.asarray([population_data[k].get('output_obs_ang_err_mean') for k in found_keys]) -result2 = 1000.0 * temp[np.isfinite(temp)] +temp = np.asarray([population_data[k].get('output_obs_ang_err_median') for k in found_keys]) +result = 1000.0 * temp[np.isfinite(temp)] -if (len(result1) > 0 and len(result2) > 0): - population_results['obs_ang_max_avg'][0] = np.mean(result1) - population_results['obs_ang_mean_avg'][0] = np.mean(result2) +if (len(result) > 0): + population_results['obs_ang_median_avg'][0] = np.mean(result) plt.figure(11,figsize=(20,13)) - plt.subplot(2,1,1) - plt.hist(result1) - plt.title("Gaussian Histogram - Output Observer Angular Tracking Error Peak") - plt.xlabel("output_obs_ang_err_peak (mrad)") - plt.ylabel("Frequency") - - plt.subplot(2,1,2) - plt.hist(result2) - plt.title("Gaussian Histogram - Output Observer Angular Tracking Error Mean") - plt.xlabel("output_obs_ang_err_mean (mrad)") + plt.hist(result) + plt.title("Gaussian Histogram - Output Observer Angular Tracking Error Median") + plt.xlabel("output_obs_ang_err_median (mrad)") plt.ylabel("Frequency") pp.savefig() + plt.close(11) # Output Observer Velocity Tracking -temp = np.asarray([population_data[k].get('output_obs_vel_err_peak') for k in found_keys]) -result1 = temp[np.isfinite(temp)] -temp = np.asarray([population_data[k].get('output_obs_vel_err_mean') for k in found_keys]) -result2 = temp[np.isfinite(temp)] +temp = np.asarray([population_data[k].get('output_obs_vel_err_median') for k in found_keys]) +result = temp[np.isfinite(temp)] -if (len(result1) > 0 and len(result2) > 0): - population_results['obs_vel_max_avg'][0] = np.mean(result1) - population_results['obs_vel_mean_avg'][0] = np.mean(result2) +if (len(result) > 0): + population_results['obs_vel_median_avg'][0] = np.mean(result) plt.figure(12,figsize=(20,13)) - plt.subplot(2,1,1) - plt.hist(result1) - plt.title("Gaussian Histogram - Output Observer Velocity Tracking Error Peak") - plt.xlabel("output_obs_ang_err_peak (m/s)") - plt.ylabel("Frequency") - - plt.subplot(2,1,2) - plt.hist(result2) - plt.title("Gaussian Histogram - Output Observer Velocity Tracking Error Mean") - plt.xlabel("output_obs_ang_err_mean (m/s)") + plt.hist(result) + plt.title("Gaussian Histogram - Output Observer Velocity Tracking Error Median") + plt.xlabel("output_obs_ang_err_median (m/s)") plt.ylabel("Frequency") pp.savefig() + plt.close(12) # Output Observer Position Tracking -temp = np.asarray([population_data[k].get('output_obs_pos_err_peak') for k in found_keys]) -result1 = temp[np.isfinite(temp)] -temp = np.asarray([population_data[k].get('output_obs_pos_err_mean') for k in found_keys]) -result2 = temp[np.isfinite(temp)] +temp = np.asarray([population_data[k].get('output_obs_pos_err_median') for k in found_keys]) +result = temp[np.isfinite(temp)] -if (len(result1) > 0 and len(result2) > 0): - population_results['obs_pos_max_avg'][0] = np.mean(result1) - population_results['obs_pos_mean_avg'][0] = np.mean(result2) +if (len(result) > 0): + population_results['obs_pos_median_avg'][0] = np.mean(result) plt.figure(13,figsize=(20,13)) - plt.subplot(2,1,1) - plt.hist(result1) - plt.title("Gaussian Histogram - Output Observer Position Tracking Error Peak") - plt.xlabel("output_obs_ang_err_peak (m)") - plt.ylabel("Frequency") - - plt.subplot(2,1,2) - plt.hist(result2) - plt.title("Gaussian Histogram - Output Observer Position Tracking Error Mean") - plt.xlabel("output_obs_ang_err_mean (m)") + plt.hist(result) + plt.title("Gaussian Histogram - Output Observer Position Tracking Error Median") + plt.xlabel("output_obs_ang_err_median (m)") plt.ylabel("Frequency") pp.savefig() + plt.close(13) # close the pdf file pp.close() diff --git a/Tools/ecl_ekf/check_level_dict.csv b/Tools/ecl_ekf/check_level_dict.csv index 75fda66aff..3fdd5e07dd 100644 --- a/Tools/ecl_ekf/check_level_dict.csv +++ b/Tools/ecl_ekf/check_level_dict.csv @@ -18,15 +18,14 @@ pos_amber_warn_pct,5.0 hgt_amber_warn_pct,5.0 hagl_amber_warn_pct,5.0 tas_amber_warn_pct,5.0 -imu_coning_peak_warn,1.0E-5 +imu_coning_peak_warn,1.8E-5 imu_coning_mean_warn,3.6E-6 -imu_hfdang_peak_warn,2.4E-3 +imu_hfdang_peak_warn,3.0E-3 imu_hfdang_mean_warn,6.0E-4 -imu_hfdvel_peak_warn,2.5E-2 +imu_hfdvel_peak_warn,1.8E-2 imu_hfdvel_mean_warn,3.6E-3 -obs_ang_err_peak_warn,9.0E-2 -obs_ang_err_mean_warn,8.0E-3 -obs_vel_err_peak_warn,0.3 -obs_vel_err_mean_warn,0.05 -obs_pos_err_peak_warn,1.0 -obs_pos_err_mean_warn,0.15 +obs_ang_err_median_warn,8.0E-3 +obs_vel_err_median_warn,0.05 +obs_pos_err_median_warn,0.15 +imu_dang_bias_median_warn,4.4E-4 +imu_dvel_bias_median_warn,4.8E-3 diff --git a/Tools/ecl_ekf/process_logdata_ekf.py b/Tools/ecl_ekf/process_logdata_ekf.py index 0c2c4d4f5c..aca956de32 100644 --- a/Tools/ecl_ekf/process_logdata_ekf.py +++ b/Tools/ecl_ekf/process_logdata_ekf.py @@ -68,6 +68,7 @@ if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and ('gyro_inconsist plt.ylabel('angular rate (rad/s)') plt.xlabel('data index') pp.savefig() + plt.close(0) # vertical velocity and position innovations plt.figure(1,figsize=(20,13)) @@ -128,7 +129,7 @@ plt.text(innov_5_min_time, innov_5_min, 'min='+s_innov_5_min, fontsize=12, horiz #plt.legend(['std='+s_innov_5_std],loc='upper left',frameon=False) pp.savefig() - +plt.close(1) # horizontal velocity innovations plt.figure(2,figsize=(20,13)) @@ -183,6 +184,7 @@ plt.text(innov_1_min_time, innov_1_min, 'min='+s_innov_1_min, fontsize=12, horiz #plt.legend(['std='+s_innov_1_std],loc='upper left',frameon=False) pp.savefig() +plt.close(2) # horizontal position innovations plt.figure(3,figsize=(20,13)) @@ -239,6 +241,7 @@ plt.text(innov_4_min_time, innov_4_min, 'min='+s_innov_4_min, fontsize=12, horiz #plt.legend(['std='+s_innov_4_std],loc='upper left',frameon=False) pp.savefig() +plt.close(3) # manetometer innovations plt.figure(4,figsize=(20,13)) @@ -318,6 +321,7 @@ plt.text(innov_2_min_time, innov_2_min, 'min='+s_innov_2_min, fontsize=12, horiz #plt.legend(['std='+s_innov_2_std],loc='upper left',frameon=False) pp.savefig() +plt.close(4) # magnetic heading innovations plt.figure(5,figsize=(20,13)) @@ -348,6 +352,7 @@ plt.text(innov_0_min_time, innov_0_min, 'min='+s_innov_0_min, fontsize=12, horiz #plt.legend(['std='+s_innov_0_std],loc='upper left',frameon=False) pp.savefig() +plt.close(5) # air data innovations plt.figure(6,figsize=(20,13)) @@ -400,6 +405,7 @@ plt.text(beta_innov_max_time, beta_innov_max, 'max='+s_beta_innov_max, fontsize= plt.text(beta_innov_min_time, beta_innov_min, 'min='+s_beta_innov_min, fontsize=12, horizontalalignment='left', verticalalignment='top') pp.savefig() +plt.close(6) # optical flow innovations plt.figure(7,figsize=(20,13)) @@ -455,6 +461,7 @@ plt.text(flow_innov_y_min_time, flow_innov_y_min, 'min='+s_flow_innov_y_min, fon #plt.legend(['std='+s_flow_innov_y_std],loc='upper left',frameon=False) pp.savefig() +plt.close(7) # generate metadata for the normalised innovation consistency test levels # a value > 1.0 means the measurement data for that test has been rejected by the EKF @@ -541,6 +548,7 @@ if n_plots == 4: plt.text(tas_test_max_time, tas_test_max, 'max='+str(round(tas_test_max,2))+' , mean='+str(round(tas_test_mean,2)), fontsize=12, horizontalalignment='left', verticalalignment='bottom',color='b') pp.savefig() +plt.close(8) # extract control mode metadata from estimator_status.control_mode_flags # 0 - true if the filter tilt alignment is complete @@ -733,6 +741,7 @@ elif np.amax(using_magdecl) > 0: plt.text(using_magdecl_time, 0.75, 'magnetic declination aiding at '+str(round(using_magdecl_time,1))+' sec', fontsize=12, horizontalalignment='left', verticalalignment='center',color='g') pp.savefig() +plt.close(9) # control mode summary plot B plt.figure(10,figsize=(20,13)) @@ -767,6 +776,7 @@ plt.xlabel('time (sec)') plt.grid() pp.savefig() +plt.close(10) # innovation_check_flags summary plt.figure(11,figsize=(20,13)) @@ -837,6 +847,7 @@ plt.legend(loc='upper left') plt.grid() pp.savefig() +plt.close(11) # gps_check_fail_flags summary plt.figure(12,figsize=(20,13)) @@ -884,6 +895,7 @@ plt.legend(loc='upper right') plt.grid() pp.savefig() +plt.close(12) # filter reported accuracy plt.figure(13,figsize=(20,13)) @@ -896,6 +908,7 @@ plt.legend(loc='upper right') plt.grid() pp.savefig() +plt.close(13) # Plot the EKF IMU vibration metrics plt.figure(14,figsize=(20,13)) @@ -933,6 +946,7 @@ plt.grid() plt.text(vibe_hf_dvel_max_time, vibe_hf_dvel_max, 'max='+str(round(vibe_hf_dvel_max,4)), fontsize=12, horizontalalignment='left', verticalalignment='top') pp.savefig() +plt.close(14) # Plot the EKF output observer tracking errors plt.figure(15,figsize=(20,13)) @@ -970,10 +984,115 @@ plt.grid() plt.text(pos_track_err_max_time, pos_track_err_max, 'max='+str(round(pos_track_err_max,2)), fontsize=12, horizontalalignment='left', verticalalignment='top') pp.savefig() +plt.close(15) -# Plot the EKF wind estimates +# Plot the delta angle bias estimates plt.figure(16,figsize=(20,13)) +plt.subplot(3,1,1) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[10]'],'b') +plt.title('Delta Angle Bias Estimates') +plt.ylabel('X (rad)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,2) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[11]'],'b') +plt.ylabel('Y (rad)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,3) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[12]'],'b') +plt.ylabel('Z (rad)') +plt.xlabel('time (sec)') +plt.grid() + +pp.savefig() +plt.close(16) + +# Plot the delta velocity bias estimates +plt.figure(17,figsize=(20,13)) + +plt.subplot(3,1,1) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[13]'],'b') +plt.title('Delta Velocity Bias Estimates') +plt.ylabel('X (m/s)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,2) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[14]'],'b') +plt.ylabel('Y (m/s)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,3) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[15]'],'b') +plt.ylabel('Z (m/s)') +plt.xlabel('time (sec)') +plt.grid() + +pp.savefig() +plt.close(17) + +# Plot the earth frame magnetic field estimates + +plt.figure(18,figsize=(20,13)) +plt.subplot(3,1,3) +strength = (estimator_status['states[16]']**2 + estimator_status['states[17]']**2 + estimator_status['states[18]']**2)**0.5 +plt.plot(1e-6*estimator_status['timestamp'] , strength,'b') +plt.ylabel('strength (Gauss)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,1) +rad2deg = 57.2958 +declination = rad2deg * np.arctan2(estimator_status['states[17]'],estimator_status['states[16]']) +plt.plot(1e-6*estimator_status['timestamp'] , declination,'b') +plt.title('Earth Magnetic Field Estimates') +plt.ylabel('declination (deg)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,2) +inclination = rad2deg * np.arcsin(estimator_status['states[18]'] / strength) +plt.plot(1e-6*estimator_status['timestamp'] , inclination,'b') +plt.ylabel('inclination (deg)') +plt.xlabel('time (sec)') +plt.grid() + +pp.savefig() +plt.close(18) + +# Plot the body frame magnetic field estimates +plt.figure(19,figsize=(20,13)) + +plt.subplot(3,1,1) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[19]'],'b') +plt.title('Magnetomer Bias Estimates') +plt.ylabel('X (Gauss)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,2) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[20]'],'b') +plt.ylabel('Y (Gauss)') +plt.xlabel('time (sec)') +plt.grid() + +plt.subplot(3,1,3) +plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[21]'],'b') +plt.ylabel('Z (Gauss)') +plt.xlabel('time (sec)') +plt.grid() + +pp.savefig() +plt.close(19) + +# Plot the EKF wind estimates +plt.figure(20,figsize=(20,13)) + plt.subplot(2,1,1) plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[22]'],'b') plt.title('Wind Velocity Estimates') @@ -988,6 +1107,7 @@ plt.xlabel('time (sec)') plt.grid() pp.savefig() +plt.close(20) # close the pdf file pp.close() @@ -1073,12 +1193,11 @@ test_results = { 'imu_hfdang_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'], 'imu_hfdvel_peak':[float('NaN'),'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], 'imu_hfdvel_mean':[float('NaN'),'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], -'output_obs_ang_err_peak':[float('NaN'),'Peak in-flight value of the output observer angular error (rad)'], -'output_obs_ang_err_mean':[float('NaN'),'Mean in-flight value of the output observer angular error (rad)'], -'output_obs_vel_err_peak':[float('NaN'),'Peak in-flight value of the output observer velocity error (m/s)'], -'output_obs_vel_err_mean':[float('NaN'),'Mean in-flight value of the output observer velocity error (m/s)'], -'output_obs_pos_err_peak':[float('NaN'),'Peak in-flight value of the output observer position error (m)'], -'output_obs_pos_err_mean':[float('NaN'),'Mean in-flight value of the output observer position error (m)'], +'output_obs_ang_err_median':[float('NaN'),'Median in-flight value of the output observer angular error (rad)'], +'output_obs_vel_err_median':[float('NaN'),'Median in-flight value of the output observer velocity error (m/s)'], +'output_obs_pos_err_median':[float('NaN'),'Median in-flight value of the output observer position error (m)'], +'imu_dang_bias_median':[float('NaN'),'Median in-flight value of the delta angle bias vector length (rad)'], +'imu_dvel_bias_median':[float('NaN'),'Median in-flight value of the delta velocity bias vector length (m/s)'], 'tilt_align_time':[float('NaN'),'The time in seconds measured from startup that the EKF completed the tilt alignment. A nan value indicates that the alignment had completed before logging started or alignment did not complete.'], 'yaw_align_time':[float('NaN'),'The time in seconds measured from startup that the EKF completed the yaw alignment.'], 'in_air_transition_time':[round(in_air_transition_time,1),'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.'], @@ -1090,18 +1209,9 @@ test_results = { # reduction of innovation message data if (innov_early_end_index > (innov_late_start_index + 100)): # Output Observer Tracking Errors - temp = np.amax(ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index]) - if (temp > 0.0): - test_results['output_obs_ang_err_peak'][0] = temp - test_results['output_obs_ang_err_mean'][0] = np.mean(ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index]) - temp = np.amax(ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index]) - if (temp > 0.0): - test_results['output_obs_vel_err_peak'][0] = temp - test_results['output_obs_vel_err_mean'][0] = np.mean(ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index]) - temp = np.amax(ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index]) - if (temp > 0.0): - test_results['output_obs_pos_err_peak'][0] = temp - test_results['output_obs_pos_err_mean'][0] = np.mean(ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index]) + test_results['output_obs_ang_err_median'][0] = np.median(ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index]) + test_results['output_obs_vel_err_median'][0] = np.median(ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index]) + test_results['output_obs_pos_err_median'][0] = np.median(ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index]) # reduction of status message data if (early_end_index > (late_start_index + 100)): @@ -1186,6 +1296,10 @@ if (early_end_index > (late_start_index + 100)): test_results['ofx_fail_percentage'][0] = 100.0 * (ofx_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed test_results['ofy_fail_percentage'][0] = 100.0 * (ofy_innov_fail[late_start_index:early_end_index] > 0.5).sum() / num_valid_values_trimmed + # IMU bias checks + test_results['imu_dang_bias_median'][0] = (np.median(estimator_status['states[10]'])**2 + np.median(estimator_status['states[11]'])**2 + np.median(estimator_status['states[12]'])**2)**0.5 + test_results['imu_dvel_bias_median'][0] = (np.median(estimator_status['states[13]'])**2 + np.median(estimator_status['states[14]'])**2 + np.median(estimator_status['states[15]'])**2)**0.5 + # Check for internal filter nummerical faults test_results['filter_faults_max'][0] = np.amax(estimator_status['filter_fault_flags']) @@ -1232,6 +1346,8 @@ if (test_results.get('hagl_percentage_amber')[0] > check_levels.get('hagl_amber_ if (test_results.get('tas_percentage_amber')[0] > check_levels.get('tas_amber_warn_pct')): test_results['master_status'][0] = 'Warning' test_results['tas_sensor_status'][0] = 'Warning' + +# check for IMU sensor warnings if ((test_results.get('imu_coning_peak')[0] > check_levels.get('imu_coning_peak_warn')) or (test_results.get('imu_coning_mean')[0] > check_levels.get('imu_coning_mean_warn')) or (test_results.get('imu_hfdang_peak')[0] > check_levels.get('imu_hfdang_peak_warn')) or @@ -1239,15 +1355,18 @@ if ((test_results.get('imu_coning_peak')[0] > check_levels.get('imu_coning_peak_ (test_results.get('imu_hfdvel_peak')[0] > check_levels.get('imu_hfdvel_peak_warn')) or (test_results.get('imu_hfdvel_mean')[0] > check_levels.get('imu_hfdvel_mean_warn'))): test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' -if ((test_results.get('output_obs_ang_err_peak')[0] > check_levels.get('obs_ang_err_peak_warn')) or -(test_results.get('output_obs_ang_err_mean')[0] > check_levels.get('obs_ang_err_mean_warn')) or -(test_results.get('output_obs_vel_err_peak')[0] > check_levels.get('obs_vel_err_peak_warn')) or -(test_results.get('output_obs_vel_err_mean')[0] > check_levels.get('obs_vel_err_mean_warn')) or -(test_results.get('output_obs_pos_err_peak')[0] > check_levels.get('obs_pos_err_peak_warn')) or -(test_results.get('output_obs_pos_err_mean')[0] > check_levels.get('obs_pos_err_mean_warn'))): + test_results['imu_sensor_status'][0] = 'Warning - Vibration' + +if ((test_results.get('imu_dang_bias_median')[0] > check_levels.get('imu_dang_bias_median_warn')) or +(test_results.get('imu_dvel_bias_median')[0] > check_levels.get('imu_dvel_bias_median_warn'))): + test_results['master_status'][0] = 'Warning' + test_results['imu_sensor_status'][0] = 'Warning - Bias' + +if ((test_results.get('output_obs_ang_err_median')[0] > check_levels.get('obs_ang_err_median_warn')) or +(test_results.get('output_obs_vel_err_median')[0] > check_levels.get('obs_vel_err_median_warn')) or +(test_results.get('output_obs_pos_err_median')[0] > check_levels.get('obs_pos_err_median_warn'))): test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' + test_results['imu_sensor_status'][0] = 'Warning - Output Predictor' # check for failures if ((test_results.get('magx_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or