zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Use more generic variable name for bad yaw fusion flag 

This flag now reports on  fusion of data that is not from a magnetometer.
master
Paul Riseborough 6 years ago committed by Paul Riseborough
parent
eb52f7c410
commit
0220f469b0
3 changed files with 9 additions and 9 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      EKF/common.h
  2. 8
      EKF/gps_yaw_fusion.cpp
  3. 8
      EKF/mag_fusion.cpp

2
EKF/common.h
Unescape View File

@ -367,7 +367,7 @@ union fault_status_u {
bool bad_mag_x: 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error bool bad_mag_x: 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
bool bad_mag_y: 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error bool bad_mag_y: 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
bool bad_mag_z: 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error bool bad_mag_z: 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
bool bad_mag_hdg: 1; ///< 3 - true if the fusion of the magnetic heading has encountered a numerical error bool bad_hdg: 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error
bool bad_mag_decl: 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error bool bad_mag_decl: 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
bool bad_airspeed: 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error bool bad_airspeed: 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error
bool bad_sideslip: 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_sideslip: 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error

8
EKF/gps_yaw_fusion.cpp
Unescape View File

@ -165,12 +165,12 @@ void Ekf::fuseGpsAntYaw()
// check if the innovation variance calculation is badly conditioned // check if the innovation variance calculation is badly conditioned
if (_heading_innov_var >= R_YAW) { if (_heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault // the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_mag_hdg = false; _fault_status.flags.bad_hdg = false;
heading_innov_var_inv = 1.0f / _heading_innov_var; heading_innov_var_inv = 1.0f / _heading_innov_var;
} else { } else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_hdg = true; _fault_status.flags.bad_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step // we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
@ -255,7 +255,7 @@ void Ekf::fuseGpsAntYaw()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.flags.bad_mag_hdg = false; _fault_status.flags.bad_hdg = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
@ -267,7 +267,7 @@ void Ekf::fuseGpsAntYaw()
healthy = false; healthy = false;
// update individual measurement health status // update individual measurement health status
_fault_status.flags.bad_mag_hdg = true; _fault_status.flags.bad_hdg = true;
} }
} }

8
EKF/mag_fusion.cpp
Unescape View File

@ -666,12 +666,12 @@ void Ekf::fuseHeading()
// check if the innovation variance calculation is badly conditioned // check if the innovation variance calculation is badly conditioned
if (_heading_innov_var >= R_YAW) { if (_heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault // the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_mag_hdg = false; _fault_status.flags.bad_hdg = false;
heading_innov_var_inv = 1.0f / _heading_innov_var; heading_innov_var_inv = 1.0f / _heading_innov_var;
} else { } else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_hdg = true; _fault_status.flags.bad_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step // we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
@ -756,7 +756,7 @@ void Ekf::fuseHeading()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.flags.bad_mag_hdg = false; _fault_status.flags.bad_hdg = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
@ -768,7 +768,7 @@ void Ekf::fuseHeading()
healthy = false; healthy = false;
// update individual measurement health status // update individual measurement health status
_fault_status.flags.bad_mag_hdg = true; _fault_status.flags.bad_hdg = true;
} }
} }

Write Preview
Loading…
Cancel
Save