dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Inform console of serious fusion numerical errors

This commit is contained in:
Paul Riseborough 2016-06-07 20:32:55 +10:00
parent dbfe8c0242
commit 381d99aed6
2 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/airspeed_fusion.cpp
View File

@ -40,6 +40,7 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
@ -92,9 +93,10 @@ void Ekf::fuseAirspeed()
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
_fault_status.flags.bad_airspeed = false;
} else { // Reset the estimator
} else { // Reset the estimator covarinace matrix
_fault_status.flags.bad_airspeed = true;
initialiseCovariance();
ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
return;
}

10
EKF/mag_fusion.cpp
View File

@ -39,6 +39,7 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
@ -154,8 +155,10 @@ void Ekf::fuseMag()
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_x = true;
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("EKF magX fusion numerical error - covariance reset");
return;
}
@ -204,8 +207,10 @@ void Ekf::fuseMag()
} else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_y = true;
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("EKF magY fusion numerical error - covariance reset");
return;
}
@ -251,11 +256,13 @@ void Ekf::fuseMag()
// the innovation variance contribution from the state covariances is non-negative - no fault
_fault_status.flags.bad_mag_z = false;
} else {
} else if (_mag_innov_var[2] > 0.0f) {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_z = true;
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("EKF magZ fusion numerical error - covariance reset");
return;
}
@ -559,6 +566,7 @@ void Ekf::fuseHeading()
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("EKF mag yaw fusion numerical error - covariance reset");
return;
}