forked from Archive/PX4-Autopilot
Tools/ecl_ekf: Update EKF log analysis
Add assessment of IMU bias and mag field estimation Reduce warning false positives by adjusting thresholds and eliminating use of peak value plots for output observer monitoring Clear each figure after saving to reduce memory usage
This commit is contained in:
Paul Riseborough
Lorenz Meier
parent
ed9a9b772e
commit
2a34bde0e9
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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,9 +984,114 @@ 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 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(16,figsize=(20,13))
|
||||
plt.figure(20,figsize=(20,13))
|
||||
|
||||
plt.subplot(2,1,1)
|
||||
plt.plot(1e-6*estimator_status['timestamp'] , estimator_status['states[22]'],'b')
|
||||
|
|
@ -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'
|
||||
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 - Output Predictor'
|
||||
|
||||
# check for failures
|
||||
if ((test_results.get('magx_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or
|
||||
|
|
|
|||
Loading…
Reference in New Issue