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

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

Browse Source
This commit is contained in:
Paul Riseborough 2022-06-26 08:47:08 +10:00 committed by Andrew Tridgell
parent bb74cb9be0
commit a25aa8d2ac
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1116,6 +1116,11 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
lastInhibitMagStates = inhibitMagStates; lastInhibitMagStates = inhibitMagStates;
if (!inhibitWindStates) { if (!inhibitWindStates) {
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))); 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) { if (!tasDataDelayed.allowFusion) {
// Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor
@ -1123,6 +1128,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
} }
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 // set variables used to calculate covariance growth
dvx = imuDataDelayed.delVel.x; dvx = imuDataDelayed.delVel.x;