From d7edc946b61cb787d5f5a428b14a9960e3d5fb37 Mon Sep 17 00:00:00 2001 From: Harshit Kumar Sankhla Date: Fri, 19 Jun 2020 11:51:46 +0530 Subject: [PATCH] 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 --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 129 ++++++++++++++++---- libraries/AP_NavEKF3/AP_NavEKF3.h | 24 +++- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 6 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 14 +++ 4 files changed, 145 insertions(+), 28 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 0e8d03c6fd..b45a26df25 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -639,6 +639,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @RebootRequired: True 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 }; @@ -703,14 +711,14 @@ bool NavEKF3::InitialiseFilter(void) _imuMask.set(_imuMask.get() & mask); // initialise the setup variables - for (uint8_t i=0; i<7; i++) { + for (uint8_t i=0; iavailable_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); 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); if (core == nullptr) { _enable.set(0); - gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: allocation failed"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF3 allocation failed"); return false; } @@ -756,6 +764,9 @@ bool NavEKF3::InitialiseFilter(void) return false; } + // set relative error scores for all cores to 0 + resetCoreErrors(); + // Set the primary initially to be the lowest index primary = 0; @@ -769,6 +780,9 @@ bool NavEKF3::InitialiseFilter(void) 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 memset(&yaw_reset_data, 0, sizeof(yaw_reset_data)); memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data)); @@ -778,7 +792,10 @@ bool NavEKF3::InitialiseFilter(void) 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) { if (!core) { @@ -814,37 +831,64 @@ void NavEKF3::UpdateFilter(void) } runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7; } - float primaryErrorScore = core[primary].errorScore(); - if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection) { - float lowestErrorScore = 0.67f * primaryErrorScore; - uint8_t newPrimaryIndex = primary; // index for new primary + + bool armed = hal.util->get_soft_armed(); + + // 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 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; - lowestErrorScore = altErrorScore; } } } - // update the yaw and position reset data to capture changes due to the lane switch - if (newPrimaryIndex != primary) { + altCoreAvailable = 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); updateLaneSwitchPosResetData(newPrimaryIndex, primary); updateLaneSwitchPosDownResetData(newPrimaryIndex, primary); + resetCoreErrors(); + coreLastTimePrimary_us[primary] = imuSampleTime_us; primary = newPrimaryIndex; 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 // avoids us ending with with a lottery for which IMU is used // in each flight. Otherwise the alignment of the timing of @@ -895,7 +939,7 @@ void NavEKF3::checkLaneSwitch(void) updateLaneSwitchPosDownResetData(newPrimaryIndex, primary); primary = newPrimaryIndex; 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 bool NavEKF3::healthy(void) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 060508ea03..c64de2f7e9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -58,9 +58,19 @@ public: // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; + // Check that all cores are started and healthy 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 // return -1 if no primary core selected 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_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_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 #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 } pos_down_reset_data; - bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started - bool coreSetupRequired[7]; // true when this core index needs to be setup - uint8_t coreImuIndex[7]; // IMU index used by this core +#define MAX_EKF_CORES 3 // maximum allowed EKF Cores to be instantiated +#define CORE_ERR_LIM 1 // -LIM to LIM relative error range for a 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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 956f6107c4..4128b96b45 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -99,7 +99,9 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const innovMY : (int16_t)(magInnov.y), innovMZ : (int16_t)(magInnov.z), 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)); } @@ -177,7 +179,7 @@ void NavEKF3::Log_Write_XKF5(uint64_t time_us) const angErr : (float)predictorErrors.x, velErr : (float)predictorErrors.y, posErr : (float)predictorErrors.z - }; + }; AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 848dddb714..46ece610f6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -45,6 +45,20 @@ float NavEKF3_core::errorScore() const score = MAX(score, 0.5f * (velTestRatio + posTestRatio)); // Check altimeter fusion performance 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; }