fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 06:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: mag_fusion consolidate duplicate error handling

Browse Source
This commit is contained in:
Daniel Agar
2023-10-05 12:01:29 -04:00
parent 585687b766
commit e58ceba4b1
1 changed files with 21 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_fusion.cpp
+21 -60
View File
@@ -57,7 +57,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
Vector3f mag_innov;
Vector3f innov_var;
@@ -66,60 +65,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
const VectorState state_vector = _state.vector();
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
innov_var.copyTo(aid_src_mag.innovation_variance);
if (aid_src_mag.innovation_variance[0] < R_MAG) {
// 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 re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
return false;
}
_fault_status.flags.bad_mag_x = false;
// check innovation variances for being badly conditioned
if (aid_src_mag.innovation_variance[1] < R_MAG) {
// 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 re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return false;
}
_fault_status.flags.bad_mag_y = false;
if (aid_src_mag.innovation_variance[2] < R_MAG) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_z = true;
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return false;
}
_fault_status.flags.bad_mag_z = false;
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
mag_innov(2) = 0.0f;
@@ -129,21 +74,37 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
aid_src_mag.observation_variance[i] = R_MAG;
aid_src_mag.innovation[i] = mag_innov(i);
}
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
aid_src_mag.innovation[2] = 0.0f;
aid_src_mag.innovation_variance[i] = innov_var(i);
}
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);
// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
// check innovation variances for being badly conditioned
if (innov_var.min() < R_MAG) {
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("mag %s", numerical_error_covariance_reset_string);
return false;
}
// if any axis fails, abort the mag fusion
if (aid_src_mag.innovation_rejected) {
return false;