Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: add error checking and isolation to 3D amg fusion

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-10 17:04:31 +10:00 committed by Andrew Tridgell
parent 51a3df0319
commit a0b1dc5325
1 changed files with 46 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -559,24 +559,6 @@ void NavEKF2_core::FuseMagnetometer()
hal.util->perf_begin(_perf_test[5]);
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
}
// Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
if (magFuseTiltInhibit) {
stateStruct.angErr.x = 0.0f;
stateStruct.angErr.y = 0.0f;
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
@ -609,15 +591,54 @@ void NavEKF2_core::FuseMagnetometer()
KHP[i][j] = res;
}
}
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
}
}
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
ForceSymmetry();
ConstrainVariances();
// update the states
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
}
// Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
if (magFuseTiltInhibit) {
stateStruct.angErr.x = 0.0f;
stateStruct.angErr.y = 0.0f;
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
} else {
// record bad axis
if (obsIndex == 0) {
faultStatus.bad_xmag = true;
} else if (obsIndex == 1) {
faultStatus.bad_ymag = true;
} else if (obsIndex == 2) {
faultStatus.bad_zmag = true;
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent
// ill-condiioning.
ForceSymmetry();
ConstrainVariances();
hal.util->perf_end(_perf_test[5]);