Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: 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 12:15:31 +11:00 committed by Andrew Tridgell
parent aaf558f593
commit 248ef92ed7
2 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -466,6 +466,7 @@ private:
const uint16_t gndEffectTimeout_ms = 1000; // time in msec that ground effect mode is active after being activated const uint16_t gndEffectTimeout_ms = 1000; // time in msec that ground effect mode is active after being activated
const float gndEffectBaroScaler = 4.0f; // scaler applied to the barometer observation variance when ground effect mode is active const float gndEffectBaroScaler = 4.0f; // scaler applied to the barometer observation variance when ground effect mode is active
const uint8_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec const uint8_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec
const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
// origin set by one of the cores // origin set by one of the cores
struct Location common_EKF_origin; struct Location common_EKF_origin;

17
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -781,10 +781,11 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = euler321.z; measured_yaw = euler321.z;
} else { } else {
if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance; float gsfYaw, gsfYawVariance, velInnovLength;
if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(15.0f))) { gsfYawVariance < sq(radians(15.0f)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
measured_yaw = gsfYaw; measured_yaw = gsfYaw;
R_YAW = gsfYawVariance; R_YAW = gsfYawVariance;
} else { } else {
@ -841,10 +842,11 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = euler312.z; measured_yaw = euler312.z;
} else { } else {
if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance; float gsfYaw, gsfYawVariance, velInnovLength;
if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
gsfYawVariance < sq(radians(15.0f))) { gsfYawVariance < sq(radians(15.0f)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
measured_yaw = gsfYaw; measured_yaw = gsfYaw;
R_YAW = gsfYawVariance; R_YAW = gsfYawVariance;
} else { } else {
@ -1166,8 +1168,11 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
return false; return false;
}; };
float yawEKFGSF, yawVarianceEKFGSF; float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) { if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(15.0f)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
// keep roll and pitch and reset yaw // keep roll and pitch and reset yaw
resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false); resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false);