Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

ecl_ekf tools: add sideslip and gps fix type to fix bit error (#9619)

Browse Source 
analyse_logdata_ekf:
- add sideslip innovation fail check flag to fix wrongly selected
bits for hagl innovation and optical flow innovations
- plot sideslip innovation fail
- add gps fix type fail flag to fix wrongly selected bits for all
gps check fail flags
- plot gps fix type
This commit is contained in:
JohannesBrand
2018-06-10 03:57:46 +02:00
committed by Daniel Agar
parent 7278bdd4ab
commit 3b4d9efc8f
1 changed files with 40 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/ecl_ekf/analyse_logdata_ekf.py
+40 -29
View File
@@ -710,9 +710,10 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
# 5 - true if the Z magnetometer observation has been rejected # 5 - true if the Z magnetometer observation has been rejected
# 6 - true if the yaw observation has been rejected # 6 - true if the yaw observation has been rejected
# 7 - true if the airspeed observation has been rejected # 7 - true if the airspeed observation has been rejected
# 8 - true if the height above ground observation has been rejected # 8 - true if synthetic sideslip observation has been rejected
# 9 - true if the X optical flow observation has been rejected # 9 - true if the height above ground observation has been rejected
# 10 - true if the Y optical flow observation has been rejected # 10 - true if the X optical flow observation has been rejected
# 11 - true if the Y optical flow observation has been rejected
vel_innov_fail = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1 vel_innov_fail = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1
posh_innov_fail = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1 posh_innov_fail = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1
posv_innov_fail = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1 posv_innov_fail = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1
@@ -721,14 +722,15 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
magz_innov_fail = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1 magz_innov_fail = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1
yaw_innov_fail = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1 yaw_innov_fail = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1
tas_innov_fail = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1 tas_innov_fail = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1
hagl_innov_fail = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1 sli_innov_fail = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
ofx_innov_fail = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1 hagl_innov_fail = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
ofy_innov_fail = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1 ofx_innov_fail = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
ofy_innov_fail = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1
if plot: if plot:
# plot innovation_check_flags summary # plot innovation_check_flags summary
plt.figure(11, figsize=(20, 13)) plt.figure(11, figsize=(20, 13))
plt.subplot(5, 1, 1) plt.subplot(6, 1, 1)
plt.title('EKF Innovation Test Fails') plt.title('EKF Innovation Test Fails')
plt.plot(status_time, vel_innov_fail, 'b', label='vel NED') plt.plot(status_time, vel_innov_fail, 'b', label='vel NED')
plt.plot(status_time, posh_innov_fail, 'r', label='pos NE') plt.plot(status_time, posh_innov_fail, 'r', label='pos NE')
@@ -736,14 +738,14 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
plt.ylabel('failed') plt.ylabel('failed')
plt.legend(loc='upper left') plt.legend(loc='upper left')
plt.grid() plt.grid()
plt.subplot(5, 1, 2) plt.subplot(6, 1, 2)
plt.plot(status_time, posv_innov_fail, 'b', label='hgt absolute') plt.plot(status_time, posv_innov_fail, 'b', label='hgt absolute')
plt.plot(status_time, hagl_innov_fail, 'r', label='hgt above ground') plt.plot(status_time, hagl_innov_fail, 'r', label='hgt above ground')
plt.ylim(-0.1, 1.1) plt.ylim(-0.1, 1.1)
plt.ylabel('failed') plt.ylabel('failed')
plt.legend(loc='upper left') plt.legend(loc='upper left')
plt.grid() plt.grid()
plt.subplot(5, 1, 3) plt.subplot(6, 1, 3)
plt.plot(status_time, magx_innov_fail, 'b', label='mag_x') plt.plot(status_time, magx_innov_fail, 'b', label='mag_x')
plt.plot(status_time, magy_innov_fail, 'r', label='mag_y') plt.plot(status_time, magy_innov_fail, 'r', label='mag_y')
plt.plot(status_time, magz_innov_fail, 'g', label='mag_z') plt.plot(status_time, magz_innov_fail, 'g', label='mag_z')
@@ -752,13 +754,19 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
plt.ylim(-0.1, 1.1) plt.ylim(-0.1, 1.1)
plt.ylabel('failed') plt.ylabel('failed')
plt.grid() plt.grid()
plt.subplot(5, 1, 4) plt.subplot(6, 1, 4)
plt.plot(status_time, tas_innov_fail, 'b', label='airspeed') plt.plot(status_time, tas_innov_fail, 'b', label='airspeed')
plt.ylim(-0.1, 1.1) plt.ylim(-0.1, 1.1)
plt.ylabel('failed') plt.ylabel('failed')
plt.legend(loc='upper left') plt.legend(loc='upper left')
plt.grid() plt.grid()
plt.subplot(5, 1, 5) plt.subplot(6, 1, 5)
plt.plot(status_time, sli_innov_fail, 'b', label='sideslip')
plt.ylim(-0.1, 1.1)
plt.ylabel('failed')
plt.legend(loc='upper left')
plt.grid()
plt.subplot(6, 1, 6)
plt.plot(status_time, ofx_innov_fail, 'b', label='flow X') plt.plot(status_time, ofx_innov_fail, 'b', label='flow X')
plt.plot(status_time, ofy_innov_fail, 'r', label='flow Y') plt.plot(status_time, ofy_innov_fail, 'r', label='flow Y')
plt.ylim(-0.1, 1.1) plt.ylim(-0.1, 1.1)
@@ -770,26 +778,29 @@ def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_leve
plt.close(11) plt.close(11)
# gps_check_fail_flags summary # gps_check_fail_flags summary
plt.figure(12, figsize=(20, 13)) plt.figure(12, figsize=(20, 13))
# 0 : minimum required sat count fail # 0 : insufficient fix type (no 3D solution)
# 1 : minimum required GDoP fail # 1 : minimum required sat count fail
# 2 : maximum allowed horizontal position error fail # 2 : minimum required GDoP fail
# 3 : maximum allowed vertical position error fail # 3 : maximum allowed horizontal position error fail
# 4 : maximum allowed speed error fail # 4 : maximum allowed vertical position error fail
# 5 : maximum allowed horizontal position drift fail # 5 : maximum allowed speed error fail
# 6 : maximum allowed vertical position drift fail # 6 : maximum allowed horizontal position drift fail
# 7 : maximum allowed horizontal speed fail # 7 : maximum allowed vertical position drift fail
# 8 : maximum allowed vertical velocity discrepancy fail # 8 : maximum allowed horizontal speed fail
nsat_fail = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1 # 9 : maximum allowed vertical velocity discrepancy fail
gdop_fail = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1 gfix_fail = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1
herr_fail = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1 nsat_fail = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1
verr_fail = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1 gdop_fail = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1
serr_fail = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1 herr_fail = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1
hdrift_fail = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1 verr_fail = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1
vdrift_fail = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1 serr_fail = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1
hspd_fail = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1 hdrift_fail = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1
veld_diff_fail = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1 vdrift_fail = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1
hspd_fail = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1
veld_diff_fail = ((2 ** 9 & estimator_status['gps_check_fail_flags']) > 0) * 1
plt.subplot(2, 1, 1) plt.subplot(2, 1, 1)
plt.title('GPS Direct Output Check Failures') plt.title('GPS Direct Output Check Failures')
plt.plot(status_time, gfix_fail, 'k', label='fix type')
plt.plot(status_time, nsat_fail, 'b', label='N sats') plt.plot(status_time, nsat_fail, 'b', label='N sats')
plt.plot(status_time, gdop_fail, 'r', label='GDOP') plt.plot(status_time, gdop_fail, 'r', label='GDOP')
plt.plot(status_time, herr_fail, 'g', label='horiz pos error') plt.plot(status_time, herr_fail, 'g', label='horiz pos error')