aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Add velocity innovation check to use of EKF-GSF yaw

Browse Source 
Only apply to non fly forward vehicle, eg copters, because magnitude of velocity innovation produced by plane launches is TBD.
This commit is contained in:
Paul Riseborough 2020-11-16 11:53:35 +11:00 committed by Andrew Tridgell
parent 78e10e99f5
commit aaf558f593
3 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -524,6 +524,7 @@ private:
const uint8_t sensorIntervalMin_ms = 50; // The minimum allowed time between measurements from any non-IMU sensor (msec)
const uint8_t flowIntervalMin_ms = 20; // The minimum allowed time between measurements from optical flow sensors (msec)
const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec)
const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
// time at start of current filter update
uint64_t imuSampleTime_us;

7
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -646,8 +646,11 @@ void NavEKF3_core::runYawEstimatorCorrection()
// 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));
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
if (canUseEKFGSF) {
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
EKFGSF_yaw_valid_count++;

23
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -221,10 +221,12 @@ void NavEKF3_core::SelectMagFusion()
yawAngDataDelayed.type = 1;
}
float yawEKFGSF, yawVarianceEKFGSF;
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
bool canUseEKFGSF = yawEstimator != nullptr &&
yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
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
@ -864,12 +866,12 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
// external yaw available check
bool canUseGsfYaw = false;
float gsfYaw = 0.0f;
float gsfYawVariance = 0.0f;
float gsfYaw, gsfYawVariance, velInnovLength;
if (usePredictedYaw && yawEstimator != nullptr) {
canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance)
&& is_positive(gsfYawVariance)
&& gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
}
// yaw measurement error variance (rad^2)
@ -1461,8 +1463,11 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
return false;
};
float yawEKFGSF, yawVarianceEKFGSF;
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG))) {
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
// keep roll and pitch and reset yaw
rotationOrder order;