diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index fc7b85e31a..f907ed9143 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -45,9 +45,11 @@ float NavEKF2_core::errorScore() const { float score = 0.0f; if (tiltAlignComplete && yawAlignComplete) { - score = MAX(score, velTestRatio); - score = MAX(score, posTestRatio); + // Check GPS fusion performance + score = MAX(score, 0.5f * (velTestRatio + posTestRatio)); + // Check altimeter fusion performance score = MAX(score, hgtTestRatio); + // Check attitude corrections const float tiltErrThreshold = 0.05f; score = MAX(score, tiltErrFilt / tiltErrThreshold); }