mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: Relative Error based Lane-Switching
Improvments to the lane selection logic, we accumulate error for each EKF lane relative to the primary for a more robust core selection
This commit is contained in:
parent
56cbcb42ee
commit
d7edc946b6
|
@ -639,6 +639,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
||||||
// @RebootRequired: True
|
// @RebootRequired: True
|
||||||
AP_GROUPINFO("GSF_RST_MAX", 60, NavEKF3, _gsfResetMaxCount, 2),
|
AP_GROUPINFO("GSF_RST_MAX", 60, NavEKF3, _gsfResetMaxCount, 2),
|
||||||
|
|
||||||
|
// @Param: ERR_THRESH
|
||||||
|
// @DisplayName: EKF3 Lane Relative Error Sensitivity Threshold
|
||||||
|
// @Description: lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError, lowering this makes lane switching more sensitive to smaller error differences
|
||||||
|
// @Range: 0.05 1
|
||||||
|
// @Increment: 0.05
|
||||||
|
// @User: Advanced
|
||||||
|
AP_GROUPINFO("ERR_THRESH", 61, NavEKF3, _err_thresh, 0.2),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -703,14 +711,14 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
_imuMask.set(_imuMask.get() & mask);
|
_imuMask.set(_imuMask.get() & mask);
|
||||||
|
|
||||||
// initialise the setup variables
|
// initialise the setup variables
|
||||||
for (uint8_t i=0; i<7; i++) {
|
for (uint8_t i=0; i<MAX_EKF_CORES; i++) {
|
||||||
coreSetupRequired[i] = false;
|
coreSetupRequired[i] = false;
|
||||||
coreImuIndex[i] = 0;
|
coreImuIndex[i] = 0;
|
||||||
}
|
}
|
||||||
num_cores = 0;
|
num_cores = 0;
|
||||||
|
|
||||||
// count IMUs from mask
|
// count IMUs from mask
|
||||||
for (uint8_t i=0; i<7; i++) {
|
for (uint8_t i=0; i<MAX_EKF_CORES; i++) {
|
||||||
if (_imuMask & (1U<<i)) {
|
if (_imuMask & (1U<<i)) {
|
||||||
coreSetupRequired[num_cores] = true;
|
coreSetupRequired[num_cores] = true;
|
||||||
coreImuIndex[num_cores] = i;
|
coreImuIndex[num_cores] = i;
|
||||||
|
@ -720,7 +728,7 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
|
|
||||||
// check if there is enough memory to create the EKF cores
|
// check if there is enough memory to create the EKF cores
|
||||||
if (hal.util->available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
|
if (hal.util->available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
|
||||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: not enough memory");
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF3 not enough memory");
|
||||||
_enable.set(0);
|
_enable.set(0);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -729,7 +737,7 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
core = (NavEKF3_core*)hal.util->malloc_type(sizeof(NavEKF3_core)*num_cores, AP_HAL::Util::MEM_FAST);
|
core = (NavEKF3_core*)hal.util->malloc_type(sizeof(NavEKF3_core)*num_cores, AP_HAL::Util::MEM_FAST);
|
||||||
if (core == nullptr) {
|
if (core == nullptr) {
|
||||||
_enable.set(0);
|
_enable.set(0);
|
||||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: allocation failed");
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF3 allocation failed");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -756,6 +764,9 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// set relative error scores for all cores to 0
|
||||||
|
resetCoreErrors();
|
||||||
|
|
||||||
// Set the primary initially to be the lowest index
|
// Set the primary initially to be the lowest index
|
||||||
primary = 0;
|
primary = 0;
|
||||||
|
|
||||||
|
@ -769,6 +780,9 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
ret &= core[i].InitialiseFilterBootstrap();
|
ret &= core[i].InitialiseFilterBootstrap();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// set last time the cores were primary to 0
|
||||||
|
memset(coreLastTimePrimary_us, 0, sizeof(coreLastTimePrimary_us));
|
||||||
|
|
||||||
// zero the structs used capture reset events
|
// zero the structs used capture reset events
|
||||||
memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
|
memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
|
||||||
memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
|
memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
|
||||||
|
@ -778,7 +792,10 @@ bool NavEKF3::InitialiseFilter(void)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update Filter States - this should be called whenever new IMU data is available
|
/*
|
||||||
|
Update Filter States - this should be called whenever new IMU data is available
|
||||||
|
Execution speed governed by SCHED_LOOP_RATE
|
||||||
|
*/
|
||||||
void NavEKF3::UpdateFilter(void)
|
void NavEKF3::UpdateFilter(void)
|
||||||
{
|
{
|
||||||
if (!core) {
|
if (!core) {
|
||||||
|
@ -814,37 +831,64 @@ void NavEKF3::UpdateFilter(void)
|
||||||
}
|
}
|
||||||
runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
|
runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
|
||||||
}
|
}
|
||||||
float primaryErrorScore = core[primary].errorScore();
|
|
||||||
if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection) {
|
bool armed = hal.util->get_soft_armed();
|
||||||
float lowestErrorScore = 0.67f * primaryErrorScore;
|
|
||||||
uint8_t newPrimaryIndex = primary; // index for new primary
|
// core selection is only available after the vehicle is armed, else forced to lane 0 if its healthy
|
||||||
|
if (runCoreSelection && armed) {
|
||||||
|
// update this instance's error scores for all active cores and get the primary core's error score
|
||||||
|
float primaryErrorScore = updateCoreErrorScores();
|
||||||
|
|
||||||
|
// update the accumulated relative error scores for all active cores
|
||||||
|
updateCoreRelativeErrors();
|
||||||
|
|
||||||
|
bool betterCore = false;
|
||||||
|
bool altCoreAvailable = false;
|
||||||
|
float bestCoreError = 0; // looking for cores that have error lower than the current primary
|
||||||
|
uint8_t newPrimaryIndex = primary;
|
||||||
|
|
||||||
|
// loop through all available cores to find if an alternative core is available
|
||||||
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
|
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
|
||||||
|
|
||||||
if (coreIndex != primary) {
|
if (coreIndex != primary) {
|
||||||
// an alternative core is available for selection only if healthy and if states have been updated on this time step
|
float altCoreError = coreRelativeErrors[coreIndex];
|
||||||
bool altCoreAvailable = core[coreIndex].healthy() && statePredictEnabled[coreIndex];
|
|
||||||
|
|
||||||
// If the primary core is unhealthy and another core is available, then switch now
|
// an alternative core is available for selection based on 2 conditions -
|
||||||
// If the primary core is still healthy,then switching is optional and will only be done if
|
// 1. healthy and states have been updated on this time step
|
||||||
// a core with a significantly lower error score can be found
|
// 2. has relative error less than primary core error
|
||||||
float altErrorScore = core[coreIndex].errorScore();
|
// 3. not been the primary core for at least 10 seconds
|
||||||
if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) {
|
altCoreAvailable = core[coreIndex].healthy() &&
|
||||||
|
altCoreError < bestCoreError &&
|
||||||
|
(imuSampleTime_us - coreLastTimePrimary_us[coreIndex] > 1E7);
|
||||||
|
|
||||||
|
if (altCoreAvailable) {
|
||||||
|
// if this core has a significantly lower relative error to the active primary, we consider it as a
|
||||||
|
// better core and would like to switch to it even if the current primary is healthy
|
||||||
|
betterCore = altCoreError <= -BETTER_THRESH; // a better core if its relative error is below a substantial level than the primary's
|
||||||
|
bestCoreError = altCoreError;
|
||||||
newPrimaryIndex = coreIndex;
|
newPrimaryIndex = coreIndex;
|
||||||
lowestErrorScore = altErrorScore;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// update the yaw and position reset data to capture changes due to the lane switch
|
altCoreAvailable = newPrimaryIndex != primary;
|
||||||
if (newPrimaryIndex != primary) {
|
|
||||||
|
// Switch cores if another core is available and the active primary core meets one of the following conditions -
|
||||||
|
// 1. has a bad error score
|
||||||
|
// 2. is unhealthy
|
||||||
|
// 3. is healthy, but a better core is available
|
||||||
|
// also update the yaw and position reset data to capture changes due to the lane switch
|
||||||
|
if (altCoreAvailable && (primaryErrorScore > 1.0f || !core[primary].healthy() || betterCore)) {
|
||||||
updateLaneSwitchYawResetData(newPrimaryIndex, primary);
|
updateLaneSwitchYawResetData(newPrimaryIndex, primary);
|
||||||
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
|
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
|
||||||
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
||||||
|
resetCoreErrors();
|
||||||
|
coreLastTimePrimary_us[primary] = imuSampleTime_us;
|
||||||
primary = newPrimaryIndex;
|
primary = newPrimaryIndex;
|
||||||
lastLaneSwitch_ms = AP_HAL::millis();
|
lastLaneSwitch_ms = AP_HAL::millis();
|
||||||
}
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (primary != 0 && core[0].healthy() && !hal.util->get_soft_armed()) {
|
if (primary != 0 && core[0].healthy() && !armed) {
|
||||||
// when on the ground and disarmed force the first lane. This
|
// when on the ground and disarmed force the first lane. This
|
||||||
// avoids us ending with with a lottery for which IMU is used
|
// avoids us ending with with a lottery for which IMU is used
|
||||||
// in each flight. Otherwise the alignment of the timing of
|
// in each flight. Otherwise the alignment of the timing of
|
||||||
|
@ -895,7 +939,7 @@ void NavEKF3::checkLaneSwitch(void)
|
||||||
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
||||||
primary = newPrimaryIndex;
|
primary = newPrimaryIndex;
|
||||||
lastLaneSwitch_ms = now;
|
lastLaneSwitch_ms = now;
|
||||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: lane switch %u", primary);
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -906,6 +950,45 @@ void NavEKF3::requestYawReset(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
Update this instance error score value for all active cores
|
||||||
|
*/
|
||||||
|
float NavEKF3::updateCoreErrorScores()
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < num_cores; i++) {
|
||||||
|
coreErrorScores[i] = core[i].errorScore();
|
||||||
|
}
|
||||||
|
return coreErrorScores[primary];
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
Update the relative error for all alternate available cores with respect to primary core's error.
|
||||||
|
A positive relative error for a core means it has been more erroneous than the existing primary.
|
||||||
|
A negative relative error indicates a core which can be switched to.
|
||||||
|
*/
|
||||||
|
void NavEKF3::updateCoreRelativeErrors()
|
||||||
|
{
|
||||||
|
float error = 0;
|
||||||
|
for (uint8_t i = 0; i < num_cores; i++) {
|
||||||
|
if (i != primary) {
|
||||||
|
error = coreErrorScores[i] - coreErrorScores[primary];
|
||||||
|
// reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes
|
||||||
|
if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
|
||||||
|
coreRelativeErrors[i] += error;
|
||||||
|
coreRelativeErrors[i] = constrain_float(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset the relative error values
|
||||||
|
void NavEKF3::resetCoreErrors(void)
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < num_cores; i++) {
|
||||||
|
coreRelativeErrors[i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// Check basic filter health metrics and return a consolidated health status
|
// Check basic filter health metrics and return a consolidated health status
|
||||||
bool NavEKF3::healthy(void) const
|
bool NavEKF3::healthy(void) const
|
||||||
{
|
{
|
||||||
|
|
|
@ -58,9 +58,19 @@ public:
|
||||||
|
|
||||||
// Check basic filter health metrics and return a consolidated health status
|
// Check basic filter health metrics and return a consolidated health status
|
||||||
bool healthy(void) const;
|
bool healthy(void) const;
|
||||||
|
|
||||||
// Check that all cores are started and healthy
|
// Check that all cores are started and healthy
|
||||||
bool all_cores_healthy(void) const;
|
bool all_cores_healthy(void) const;
|
||||||
|
|
||||||
|
// Update instance error scores for all available cores
|
||||||
|
float updateCoreErrorScores(void);
|
||||||
|
|
||||||
|
// Update relative error scores for all alternate available cores
|
||||||
|
void updateCoreRelativeErrors(void);
|
||||||
|
|
||||||
|
// Reset error scores for all available cores
|
||||||
|
void resetCoreErrors(void);
|
||||||
|
|
||||||
// returns the index of the primary core
|
// returns the index of the primary core
|
||||||
// return -1 if no primary core selected
|
// return -1 if no primary core selected
|
||||||
int8_t getPrimaryCoreIndex(void) const;
|
int8_t getPrimaryCoreIndex(void) const;
|
||||||
|
@ -495,6 +505,7 @@ private:
|
||||||
AP_Int8 _gsfUseMask; // mask controlling which EKF3 instances will use EKF-GSF yaw estimator data to assit with yaw resets
|
AP_Int8 _gsfUseMask; // mask controlling which EKF3 instances will use EKF-GSF yaw estimator data to assit with yaw resets
|
||||||
AP_Int16 _gsfResetDelay; // number of mSec from loss of navigation to requesting a reset using EKF-GSF yaw estimator data
|
AP_Int16 _gsfResetDelay; // number of mSec from loss of navigation to requesting a reset using EKF-GSF yaw estimator data
|
||||||
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
|
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
|
||||||
|
AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
|
||||||
|
|
||||||
// Possible values for _flowUse
|
// Possible values for _flowUse
|
||||||
#define FLOW_USE_NONE 0
|
#define FLOW_USE_NONE 0
|
||||||
|
@ -567,9 +578,16 @@ private:
|
||||||
float core_delta; // the amount of D position change between cores when a change happened
|
float core_delta; // the amount of D position change between cores when a change happened
|
||||||
} pos_down_reset_data;
|
} pos_down_reset_data;
|
||||||
|
|
||||||
bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
|
#define MAX_EKF_CORES 3 // maximum allowed EKF Cores to be instantiated
|
||||||
bool coreSetupRequired[7]; // true when this core index needs to be setup
|
#define CORE_ERR_LIM 1 // -LIM to LIM relative error range for a core
|
||||||
uint8_t coreImuIndex[7]; // IMU index used by this core
|
#define BETTER_THRESH 0.5 // a lane should have this much relative error difference to be considered for overriding a healthy primary core
|
||||||
|
|
||||||
|
bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
|
||||||
|
bool coreSetupRequired[MAX_EKF_CORES]; // true when this core index needs to be setup
|
||||||
|
uint8_t coreImuIndex[MAX_EKF_CORES]; // IMU index used by this core
|
||||||
|
float coreRelativeErrors[MAX_EKF_CORES]; // relative errors of cores with respect to primary
|
||||||
|
float coreErrorScores[MAX_EKF_CORES]; // the instance error values used to update relative core error
|
||||||
|
uint64_t coreLastTimePrimary_us[MAX_EKF_CORES]; // last time we were using this core as primary
|
||||||
|
|
||||||
bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited
|
bool inhibitGpsVertVelUse; // true when GPS vertical velocity use is prohibited
|
||||||
|
|
||||||
|
|
|
@ -99,7 +99,9 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
|
||||||
innovMY : (int16_t)(magInnov.y),
|
innovMY : (int16_t)(magInnov.y),
|
||||||
innovMZ : (int16_t)(magInnov.z),
|
innovMZ : (int16_t)(magInnov.z),
|
||||||
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
innovYaw : (int16_t)(100*degrees(yawInnov)),
|
||||||
innovVT : (int16_t)(100*tasInnov)
|
innovVT : (int16_t)(100*tasInnov),
|
||||||
|
rerr : coreRelativeErrors[_core],
|
||||||
|
errorScore : coreErrorScores[_core]
|
||||||
};
|
};
|
||||||
AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
|
AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
|
||||||
}
|
}
|
||||||
|
@ -177,7 +179,7 @@ void NavEKF3::Log_Write_XKF5(uint64_t time_us) const
|
||||||
angErr : (float)predictorErrors.x,
|
angErr : (float)predictorErrors.x,
|
||||||
velErr : (float)predictorErrors.y,
|
velErr : (float)predictorErrors.y,
|
||||||
posErr : (float)predictorErrors.z
|
posErr : (float)predictorErrors.z
|
||||||
};
|
};
|
||||||
AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
|
AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -45,6 +45,20 @@ float NavEKF3_core::errorScore() const
|
||||||
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
|
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
|
||||||
// Check altimeter fusion performance
|
// Check altimeter fusion performance
|
||||||
score = MAX(score, hgtTestRatio);
|
score = MAX(score, hgtTestRatio);
|
||||||
|
// Check airspeed fusion performance - only when we are using at least 2 airspeed sensors so we can switch lanes with
|
||||||
|
// a better one. This only comes into effect for a forward flight vehicle. A sensitivity factor of 0.3 is added to keep the
|
||||||
|
// EKF less sensitive to innovations arising due events like strong gusts of wind, thus, prevent reporting high error scores
|
||||||
|
if (assume_zero_sideslip()) {
|
||||||
|
const auto *arsp = AP::airspeed();
|
||||||
|
if (arsp->get_num_sensors() >= 2 && (frontend->_affinity & EKF_AFFINITY_ARSP)) {
|
||||||
|
score = MAX(score, 0.3f * tasTestRatio);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// Check magnetometer fusion performance - need this when magnetometer affinity is enabled to override the inherent compass
|
||||||
|
// switching mechanism, and instead be able to move to a better lane
|
||||||
|
if (frontend->_affinity & EKF_AFFINITY_MAG) {
|
||||||
|
score = MAX(score, 0.3f * (magTestRatio.x + magTestRatio.y + magTestRatio.z));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
return score;
|
return score;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue