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

AP_NavEKF2: Improve numerical error protection in optical flow fusion

Browse Source 
Abort fusion of data if variances will become negative and publish the fault
This commit is contained in:
Paul Riseborough 2016-05-16 16:16:00 +10:00 committed by Andrew Tridgell
parent 42da33593d
commit 65f63341d7
2 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -428,9 +428,12 @@ void NavEKF2_core::FuseOptFlow()
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) {
t62 = 1.0f/t61;
faultStatus.bad_yflow = false;
} else {
t61 = 0.0f;
t62 = 1.0f/R_LOS;
faultStatus.bad_yflow = true;
return;
}
varInnovOptFlow[0] = t61;
@ -579,9 +582,12 @@ void NavEKF2_core::FuseOptFlow()
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t61 > R_LOS) {
t62 = 1.0f/t61;
faultStatus.bad_yflow = false;
} else {
t61 = 0.0f;
t62 = 1.0f/R_LOS;
faultStatus.bad_yflow = true;
return;
}
varInnovOptFlow[1] = t61;
@ -634,18 +640,6 @@ void NavEKF2_core::FuseOptFlow()
// record the last time observations were accepted for fusion
prevFlowFuseTime_ms = imuSampleTime_ms;
// 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] * innovOptFlow[obsIndex];
}
// 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
@ -674,17 +668,49 @@ void NavEKF2_core::FuseOptFlow()
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();
// 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] * innovOptFlow[obsIndex];
}
// 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_xflow = true;
} else if (obsIndex == 1) {
faultStatus.bad_yflow = true;
}
}
}
// fix basic numerical errors
ForceSymmetry();
ConstrainVariances();
}
}

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -871,7 +871,7 @@ private:
uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
float meaHgtAtTakeOff; // height measured at commencement of takeoff
// flags indicating severw numerical errors in innovation variance calculation for different fusion operations
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {
bool bad_xmag:1;
bool bad_ymag:1;
@ -886,6 +886,8 @@ private:
bool bad_dpos:1;
bool bad_yaw:1;
bool bad_decl:1;
bool bad_xflow:1;
bool bad_yflow:1;
} faultStatus;
// flags indicating which GPS quality checks are failing