Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Allow wind to relearn rapidly when GPS is re-enabled

This commit is contained in:
Paul Riseborough 2023-09-27 09:01:58 +10:00 committed by Andrew Tridgell
parent 8639543cdd
commit ffde7f815c
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1086,10 +1086,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
if (!inhibitWindStates) {
const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
if (treatWindStatesAsTruth) {
const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
if (newTreatWindStatesAsTruth) {
treatWindStatesAsTruth = true;
P[23][23] = P[22][22] = 0.0f;
} else {
if (treatWindStatesAsTruth) {
treatWindStatesAsTruth = false;
if (windStateIsObservable) {
// allow EKF to relearn wind states rapidly
P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX);
}
}
ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
if (!tasDataDelayed.allowFusion) {
// Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor