5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00

AP_NavEKF3: Lock in wind state estimates when using srag to dead reckon

This commit is contained in:
Paul Riseborough 2022-06-26 08:47:08 +10:00 committed by Andrew Tridgell
parent bb74cb9be0
commit a25aa8d2ac

View File

@ -1116,12 +1116,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
lastInhibitMagStates = inhibitMagStates;
if (!inhibitWindStates) {
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
windVelVar *= 10.0f;
const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
if (isDragFusionDeadReckoning) {
// when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate
P[23][23] = P[22][22] = 0.0f;
} else {
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
windVelVar *= 10.0f;
}
for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
}
for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
}
// set variables used to calculate covariance growth