AP_NavEKF3: Fix bug preventing velocity reset if badIMUdata

This commit is contained in:
Paul Riseborough 2021-06-17 08:52:40 +10:00 committed by Andrew Tridgell
parent b73182db3c
commit 5a793e77c2
1 changed files with 20 additions and 22 deletions

View File

@ -589,12 +589,10 @@ void NavEKF3_core::SelectVelPosFusion()
void NavEKF3_core::FuseVelPosNED() void NavEKF3_core::FuseVelPosNED()
{ {
// health is set bad until test passed // health is set bad until test passed
bool velHealth = false; // boolean true if velocity measurements have passed innovation consistency check
bool posHealth = false; // boolean true if position measurements have passed innovation consistency check bool posHealth = false; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
// declare variables used to check measurement errors bool velCheckPassed = false; // boolean true if velocity measurements have passed innovation consistency checks
Vector3f velInnov;
// declare variables used to control access to arrays // declare variables used to control access to arrays
bool fuseData[6] = {false,false,false,false,false,false}; bool fuseData[6] = {false,false,false,false,false,false};
@ -734,47 +732,47 @@ void NavEKF3_core::FuseVelPosNED()
} }
} }
// test velocity measurements // Test velocity measurements
if (fuseVelData) { if (fuseVelData) {
// test velocity measurements
uint8_t imax = 2; uint8_t imax = 2;
// Don't fuse vertical velocity observations if disabled in sources or not available // Don't fuse vertical velocity observations if disabled in sources or not available
if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE ||
!gpsDataDelayed.have_vz) && !useExtNavVel) { !gpsDataDelayed.have_vz) && !useExtNavVel) {
imax = 1; imax = 1;
} }
// Apply an innovation consistency threshold test
float innovVelSumSq = 0; // sum of squares of velocity innovations float innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances float varVelSum = 0; // sum of velocity innovation variances
for (uint8_t i = 0; i<=imax; i++) { for (uint8_t i = 0; i<=imax; i++) {
// velocity states start at index 4
stateIndex = i + 4; stateIndex = i + 4;
// calculate innovations using blended and single IMU predicted states const float innovation = stateStruct.velocity[i] - velPosObs[i];
velInnov[i] = stateStruct.velocity[i] - velPosObs[i]; // blended innovVelSumSq += sq(innovation);
// calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
// sum the innovation and innovation variances
innovVelSumSq += sq(velInnov[i]);
varVelSum += varInnovVelPos[i]; varVelSum += varInnovVelPos[i];
} }
// apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate the test ratio
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
// fail if the ratio is greater than 1 if (velTestRatio < 1.0f) {
velHealth = ((velTestRatio < 1.0f) || badIMUdata); velCheckPassed = true;
// use velocity data if healthy, timed out, or in constant position mode
if (velHealth || velTimeout) {
velHealth = true;
// restart the timeout count
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
}
// Use velocity data if healthy, timed out or when IMU fault has been detected
// Always fuse data if bad IMU to prevent aliasing and clipping pulling the state estimate away
// from the measurement un-opposed if test threshold is exceeded.
if (velCheckPassed || velTimeout || badIMUdata) {
// If we are doing full aiding and velocity fusion times out, reset to the external sensor velocity // If we are doing full aiding and velocity fusion times out, reset to the external sensor velocity
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) { if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
// reset the velocity to the external sensor velocity
ResetVelocity(resetDataSource::DEFAULT); ResetVelocity(resetDataSource::DEFAULT);
// don't fuse external sensor velocity data on this time step
// Don't fuse the same data we have used to reset states.
fuseVelData = false; fuseVelData = false;
// Reset the normalised innovation to avoid failing the bad fusion tests // Reset the normalised innovation to avoid failing the bad fusion tests
velTestRatio = 0.0f; velTestRatio = 0.0f;
} }
} else {
fuseVelData = false;
} }
} }
@ -819,7 +817,7 @@ void NavEKF3_core::FuseVelPosNED()
} }
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health // set range for sequential fusion of velocity and position measurements depending on which data is available and its health
if (fuseVelData && velHealth) { if (fuseVelData) {
fuseData[0] = true; fuseData[0] = true;
fuseData[1] = true; fuseData[1] = true;
if (useGpsVertVel || useExtNavVel) { if (useGpsVertVel || useExtNavVel) {