fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 11:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Improve protection for GSF weight collapse

Browse Source
This commit is contained in:
Paul Riseborough
2020-10-05 21:30:51 +11:00
committed by Paul Riseborough
parent 7c81350c7a
commit 759d217d5a
1 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+9 -3
View File
@@ -79,16 +79,22 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
if (!bad_update) { if (!bad_update) {
float total_weight = 0.0f; float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution // 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 ++) { for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 1E-5f); _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;
}
total_weight += _model_weights(model_index); total_weight += _model_weights(model_index);
} }
// normalise the weighting function // normalise the weighting function
if (total_weight > 1e-10f) { if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight; _model_weights /= total_weight;
} else { } else {
// calculation has collapsed so reset // all weights have collapsed due to excessive innovation variances so reset filters
initialiseEKFGSF(); initialiseEKFGSF();
} }