Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough
2024-06-09 14:25:02 +10:00
committed by Andrew Tridgell
parent 8639543cdd
commit ffde7f815c
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+10 -2
View File
@@ -1086,10 +1086,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
if (!inhibitWindStates) { if (!inhibitWindStates) {
const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable; const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
if (treatWindStatesAsTruth) { if (newTreatWindStatesAsTruth) {
treatWindStatesAsTruth = true;
P[23][23] = P[22][22] = 0.0f; P[23][23] = P[22][22] = 0.0f;
} else { } 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))); 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