From 8a34df6528c4def61f31dfb48089b7d5b33e3a0e Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 22 Nov 2016 09:21:01 +1100
Subject: [PATCH] AP_NavEKF2: Reduce unnecessary EKF core switching

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
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp         | 51 ++++++++++++++-------
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 24 ++++------
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      |  4 +-
 3 files changed, 44 insertions(+), 35 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index a370deb52c..d7c4bcd4ca 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -639,37 +639,54 @@ 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++) {
 
-                    has_switched = true;
-                    primary = i;
-                    score = tempScore;
+            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;
+        }
     }
 
     check_log_write();
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index 57677191bc..e6cb8745f1 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -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
-    const float tiltErrThreshold = 0.05f;
-    if (tiltAlignComplete && yawAlignComplete && tiltErrFilt > tiltErrThreshold) {
-        score += tiltErrFilt / tiltErrThreshold;
+    if (tiltAlignComplete && yawAlignComplete) {
+        score = MAX(score, velTestRatio);
+        score = MAX(score, posTestRatio);
+        score = MAX(score, hgtTestRatio);
+        const float tiltErrThreshold = 0.05f;
+        score = MAX(score, tiltErrFilt / tiltErrThreshold);
     }
     return score;
 }
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 5cac261b76..c797480c55 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -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