|
|
|
@ -165,12 +165,12 @@ void Ekf::fuseGpsAntYaw()
@@ -165,12 +165,12 @@ void Ekf::fuseGpsAntYaw()
|
|
|
|
|
// check if the innovation variance calculation is badly conditioned
|
|
|
|
|
if (_heading_innov_var >= R_YAW) { |
|
|
|
|
// 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; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// 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
|
|
|
|
|
initialiseCovariance(); |
|
|
|
@ -255,7 +255,7 @@ void Ekf::fuseGpsAntYaw()
@@ -255,7 +255,7 @@ void Ekf::fuseGpsAntYaw()
|
|
|
|
|
// if the covariance correction will result in a negative variance, then
|
|
|
|
|
// the covariance marix is unhealthy and must be corrected
|
|
|
|
|
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++) { |
|
|
|
|
if (P[i][i] < KHP[i][i]) { |
|
|
|
@ -267,7 +267,7 @@ void Ekf::fuseGpsAntYaw()
@@ -267,7 +267,7 @@ void Ekf::fuseGpsAntYaw()
|
|
|
|
|
healthy = false; |
|
|
|
|
|
|
|
|
|
// update individual measurement health status
|
|
|
|
|
_fault_status.flags.bad_mag_hdg = true; |
|
|
|
|
_fault_status.flags.bad_hdg = true; |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|