Browse Source

AP_NavEKF2: Group GPS innovation tests when compiling error score

mission-4.1.18
priseborough 8 years ago committed by Randy Mackay
parent
commit
7d48054e6f
  1. 6
      libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

6
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@ -45,9 +45,11 @@ float NavEKF2_core::errorScore() const
{ {
float score = 0.0f; float score = 0.0f;
if (tiltAlignComplete && yawAlignComplete) { if (tiltAlignComplete && yawAlignComplete) {
score = MAX(score, velTestRatio); // Check GPS fusion performance
score = MAX(score, posTestRatio); score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
// Check altimeter fusion performance
score = MAX(score, hgtTestRatio); score = MAX(score, hgtTestRatio);
// Check attitude corrections
const float tiltErrThreshold = 0.05f; const float tiltErrThreshold = 0.05f;
score = MAX(score, tiltErrFilt / tiltErrThreshold); score = MAX(score, tiltErrFilt / tiltErrThreshold);
} }

Loading…
Cancel
Save