fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: yaw estimator additional validity checks

Browse Source
This commit is contained in:
Daniel Agar 2025-01-17 12:50:10 -05:00 committed by Mathieu Bresciani
parent 1eb9434b8c
commit b1ca0495e2
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
const float yaw_var = _yawEstimator.getYawVar();
return (yaw_var > 0.f)
&& (yaw_var < sq(_params.EKFGSF_yaw_err_max))
&& PX4_ISFINITE(yaw_var);
}
bool Ekf::isYawFailure() const

4
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
View File

@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
}
if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) {
reset();
}
}
}