fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:54:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/EKFGSF_yaw.cpp
View File

@ -79,16 +79,22 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
if (!bad_update) {
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) = 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);
}
// normalise the weighting function
if (total_weight > 1e-10f) {
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;
} else {
// calculation has collapsed so reset
// all weights have collapsed due to excessive innovation variances so reset filters
initialiseEKFGSF();
}