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

ekf2: extract logic to test yaw emergency estimate quality

Browse Source
This commit is contained in:
bresch 2022-01-18 16:53:58 +01:00 committed by Daniel Agar
parent ea80c5027e
commit e89e3c1b0c
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/ekf.h
View File

@ -1036,6 +1036,9 @@ private:
// Returns true if the reset was successful
bool resetYawToEKFGSF();
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
void resetGpsDriftCheckFilters();
};

25
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1727,21 +1727,11 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
// Returns true if the reset was successful
bool Ekf::resetYawToEKFGSF()
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float new_yaw_variance = _yawEstimator.getYawVar();
const bool has_converged = new_yaw_variance < sq(_params.EKFGSF_yaw_err_max);
if (!has_converged) {
return false;
}
const float new_yaw = _yawEstimator.getYaw();
resetQuatStateYaw(new_yaw, new_yaw_variance, true);
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true);
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
resetVelocity();
@ -1782,6 +1772,17 @@ bool Ekf::resetYawToEKFGSF()
return true;
}
bool Ekf::isYawEmergencyEstimateAvailable() const
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{