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

EKF: Only reset necessary terms when mag fusion covariance reset required

Browse Source 
Only the quaternion and mag fusion state covariances are used in the mag fusion calculations.
This commit is contained in:
Paul Riseborough
2016-06-08 11:58:33 +10:00
parent 381d99aed6
commit 99b34f0df4
3 changed files with 25 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+6 -6
View File
@@ -156,8 +156,8 @@ void Ekf::fuseMag()
// 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();
// we need to re-initialise covariances and abort this fusion step
resetMagCovariance();
ECL_ERR("EKF magX fusion numerical error - covariance reset");
return;
}
@@ -208,8 +208,8 @@ void Ekf::fuseMag()
// 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();
// we need to re-initialise covariances and abort this fusion step
resetMagCovariance();
ECL_ERR("EKF magY fusion numerical error - covariance reset");
return;
}
@@ -260,8 +260,8 @@ void Ekf::fuseMag()
// 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();
// we need to re-initialise covariances and abort this fusion step
resetMagCovariance();
ECL_ERR("EKF magZ fusion numerical error - covariance reset");
return;
}