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

EKF: Protect against collapse of GSF weights

Browse Source
This commit is contained in:
Paul Riseborough
2020-10-04 10:00:41 +11:00
committed by Paul Riseborough
parent 4c2355a638
commit bf0d70fc90
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+5 -2
View File
@@ -80,13 +80,16 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution
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), 0.0f);
_model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 1E-5f);
total_weight += _model_weights(model_index);
}
// normalise the weighting function
if (total_weight > 1e-15f) {
if (total_weight > 1e-10f) {
_model_weights /= total_weight;
} else {
// calculation has collapsed so reset
initialiseEKFGSF();
}
// Enforce a minimum weighting value. This was added during initial development but has not been needed