Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Require GSF yaw history for reset when not using a yaw sensor

Browse Source
This commit is contained in:
Paul Riseborough 2020-06-21 11:58:14 +10:00 committed by Peter Barker
parent f86c2e1db6
commit 21ea5d5039
5 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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

11
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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);

1
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -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);

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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();
}

7
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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
};