diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index f6073ae4ba..c09d05c889 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -642,6 +642,18 @@ void NavEKF3_core::runYawEstimatorCorrection() Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise); yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc); + + // after velocity data has been fused the yaw variance esitmate will have been refreshed and + // is used maintain a history of validity + float yawEKFGSF, yawVarianceEKFGSF; + bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)); + if (canUseEKFGSF) { + if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) { + EKFGSF_yaw_valid_count++; + } + } else { + EKFGSF_yaw_valid_count = 0; + } } // action an external reset request diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 4d3878a660..1cf038f461 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -264,7 +264,9 @@ void NavEKF3_core::SelectMagFusion() if ((effectiveMagCal == MagCal::GSF_YAW || !use_compass()) && effectiveMagCal != MagCal::EXTERNAL_YAW && effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { - if (!yawAlignComplete) { + + // because this type of reset event is not as time critical, require a continuous history of valid estimates + if (!yawAlignComplete && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) { yawAlignComplete = EKFGSF_resetMainFilterYaw(); } @@ -278,7 +280,8 @@ void NavEKF3_core::SelectMagFusion() } float yawEKFGSF, yawVarianceEKFGSF; - bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)); + bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && + is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)); if (yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip()) { // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement // for non fixed wing platform types @@ -933,7 +936,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) if (usePredictedYaw && yawEstimator != nullptr) { canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) - && gsfYawVariance < sq(radians(15.0f)); + && gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)); } // yaw measurement error variance (rad^2) @@ -1483,7 +1486,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() }; float yawEKFGSF, yawVarianceEKFGSF; - if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) { + if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG))) { // keep roll and pitch and reset yaw resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 3c43759fc0..e4b923d147 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -416,6 +416,7 @@ void NavEKF3_core::detectFlight() EKFGSF_yaw_reset_ms = 0; EKFGSF_yaw_reset_request_ms = 0; EKFGSF_yaw_reset_count = 0; + EKFGSF_yaw_valid_count = 0; EKFGSF_run_filterbank = true; Vector3f gyroBias; getGyroBias(gyroBias); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 329060c932..16b12ec8e1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -439,6 +439,7 @@ void NavEKF3_core::InitialiseVariables() EKFGSF_yaw_reset_request_ms = 0; EKFGSF_yaw_reset_count = 0; EKFGSF_run_filterbank = false; + EKFGSF_yaw_valid_count = 0; effectiveMagCal = effective_magCal(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a0b5d51560..3765ff2cce 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -79,6 +79,12 @@ // number of seconds a request to reset the yaw to the GSF estimate is active before it times out #define YAW_RESET_TO_GSF_TIMEOUT_MS 5000 +// accuracy threshold applied to GSF yaw estimate use +#define GSF_YAW_ACCURACY_THRESHOLD_DEG 15.0f + +// number of continuous valid GSF yaw estimates required to confirm valid hostory +#define GSF_YAW_VALID_HISTORY_THRESHOLD 5 + class AP_AHRS; class NavEKF3_core : public NavEKF_core_common @@ -1483,4 +1489,5 @@ private: uint32_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec) uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed bool EKFGSF_run_filterbank; // true when the filter bank is active + uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate };