Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf-yaw-est: do not reset when all weights collapse

Browse Source 
The likelyhood can be really small when the innovation is suddenly
large. This can occur during a GNSS velocity glitch and shouldn't reset
the filter as it would bring it back to a really vulnerable state.
This commit is contained in:
bresch
2024-11-04 15:59:03 +01:00
committed by Daniel Agar
parent d8be547b06
commit 3ad4b57315
1 changed files with 2 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+2 -9
View File
@@ -132,13 +132,11 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution
const float min_weight = 1e-5f;
uint8_t n_weight_clips = 0;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
if (_model_weights(model_index) < min_weight) {
n_weight_clips++;
_model_weights(model_index) = min_weight;
}
@@ -146,13 +144,7 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
}
// normalise the weighting function
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;
} else {
// all weights have collapsed due to excessive innovation variances so reset filters
reset();
}
_model_weights /= total_weight;
}
// Calculate a composite yaw vector as a weighted average of the states for each model.
@@ -368,6 +360,7 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co
const float oldYaw = _ekf_gsf[model_index].X(2);
_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
_ekf_gsf[model_index].innov *= innov_comp_scale_factor;
const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;