AP_NavEKF2: fixed bug in EKF lane selection

this fixes an issue in selecting the best lane to change to when we
have 3 or more EKF cores. The bug is that if the current lane is
unhealthy it would always choose the last healthy lane instead of
choosing the lane with the lowest score

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -741,7 +741,7 @@ void NavEKF2::UpdateFilter(void)
                 // 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
                 float altErrorScore = core[coreIndex].errorScore();
-                if (altCoreAvailable && (!core[primary].healthy() || altErrorScore < lowestErrorScore)) {
+                if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) {
                     newPrimaryIndex = coreIndex;
                     lowestErrorScore = altErrorScore;
                 }