aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Reduce unnecessary EKF core switching

Browse Source 
1) Do not switch to a core until its states have been updated.
2) Distinguish between mandated switches required for health failure and optional switches required due to innovation checks failing.
3) Apply hyseresis to innovation check levels
This commit is contained in:
priseborough 2016-11-22 09:21:01 +11:00 committed by Randy Mackay
parent de153ce1d7
commit e62863f9a1
3 changed files with 44 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -639,36 +639,53 @@ void NavEKF2::UpdateFilter(void)
const AP_InertialSensor &ins = _ahrs->get_ins();
bool statePredictEnabled[num_cores];
for (uint8_t i=0; i<num_cores; i++) {
// if the previous core has only recently finished a new state prediction cycle, then
// don't start a new cycle to allow time for fusion operations to complete if the update
// rate is higher than 200Hz
bool statePredictEnabled;
if ((i > 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) {
statePredictEnabled = false;
statePredictEnabled[i] = false;
} else {
statePredictEnabled = true;
statePredictEnabled[i] = true;
}
core[i].UpdateFilter(statePredictEnabled);
core[i].UpdateFilter(statePredictEnabled[i]);
}
// If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score
if (core[primary].faultScore() > 0.0f || !core[primary].healthy()) {
float score = 1e9f;
// If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
float primaryErrorScore = core[primary].errorScore();
if (primaryErrorScore > 1.0f || !core[primary].healthy()) {
float lowestErrorScore = primaryErrorScore; // lowest error score from all lanes
bool has_switched = false; // true if a switch has occurred this frame
for (uint8_t i=0; i<num_cores; i++) {
if (core[i].healthy()) {
float tempScore = core[i].faultScore();
if (tempScore < score) {
// update the yaw and position reset data to capture changes due to the lane switch
updateLaneSwitchYawResetData(has_switched, i, primary);
updateLaneSwitchPosResetData(has_switched, i, primary);
uint8_t newPrimaryIndex = primary; // index for new primary
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
if (coreIndex != primary) {
// an alternative core is available for selection only if healthy and if states have been updated on this time step
bool altCoreAvailable = core[coreIndex].healthy() && statePredictEnabled[coreIndex];
// If the primary core is unhealthy and another core is available, then switch now
if (!core[primary].healthy() && altCoreAvailable) {
newPrimaryIndex = coreIndex;
break;
}
// If the primary core is still healthy,then switching is optional and will only be done if
// a core with a significantly lower error score can be found
bool maySwitch = core[primary].healthy() && altCoreAvailable;
float altErrorScore = core[coreIndex].errorScore();
if ((altErrorScore < 0.67f * primaryErrorScore) && (altErrorScore < lowestErrorScore) && maySwitch) {
newPrimaryIndex = coreIndex;
lowestErrorScore = altErrorScore;
}
}
}
// update the yaw and position reset data to capture changes due to the lane switch
if (newPrimaryIndex != primary) {
updateLaneSwitchYawResetData(has_switched, newPrimaryIndex, primary);
updateLaneSwitchPosResetData(has_switched, newPrimaryIndex, primary);
primary = newPrimaryIndex;
has_switched = true;
primary = i;
score = tempScore;
}
}
}
}

22
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -39,25 +39,17 @@ bool NavEKF2_core::healthy(void) const
return true;
}
// Return a consolidated fault score where higher numbers are less healthy
// Return a consolidated error score where higher numbers represent larger errors
// Intended to be used by the front-end to determine which is the primary EKF
float NavEKF2_core::faultScore(void) const
float NavEKF2_core::errorScore() const
{
float score = 0.0f;
// If velocity, position or height measurements are failing consistency checks, this adds to the score
if (velTestRatio > 1.0f) {
score += velTestRatio-1.0f;
}
if (posTestRatio > 1.0f) {
score += posTestRatio-1.0f;
}
if (hgtTestRatio > 1.0f) {
score += hgtTestRatio-1.0f;
}
// If the tilt error is excessive this adds to the score
if (tiltAlignComplete && yawAlignComplete) {
score = MAX(score, velTestRatio);
score = MAX(score, posTestRatio);
score = MAX(score, hgtTestRatio);
const float tiltErrThreshold = 0.05f;
if (tiltAlignComplete && yawAlignComplete && tiltErrFilt > tiltErrThreshold) {
score += tiltErrFilt / tiltErrThreshold;
score = MAX(score, tiltErrFilt / tiltErrThreshold);
}
return score;
}

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -66,9 +66,9 @@ public:
// Check basic filter health metrics and return a consolidated health status
bool healthy(void) const;
// Return a consolidated fault score where higher numbers are less healthy
// Return a consolidated error score where higher numbers are less healthy
// Intended to be used by the front-end to determine which is the primary EKF
float faultScore(void) const;
float errorScore(void) const;
// Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false