Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add underflow protection to yaw GSF weight normalisation

Browse Source 
This improves protection against a condition where if all yaw hypothesis filers have very large innovations, the weights of Gaussian densities can underflow leading to failure of the weights to converge.
This commit is contained in:
Paul Riseborough 2020-11-13 20:53:19 +11:00 committed by Andrew Tridgell
parent 82f68464d4
commit afb31bc4f5
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -166,17 +166,26 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution
const float min_weight = 1e-5f;
uint8_t n_clips = 0;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
newWeight[mdl_idx]= fmaxf(gaussianDensity(mdl_idx) * GSF.weights[mdl_idx], 0.0f);
newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
if (newWeight[mdl_idx] < min_weight) {
n_clips++;
newWeight[mdl_idx] = min_weight;
}
total_w += newWeight[mdl_idx];
}
// Normalise the sum of weights to unity
if (vel_fuse_running && is_positive(total_w)) {
// Reset the filters if all weights have underflowed due to excessive innovation variances
if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) {
float total_w_inv = 1.0f / total_w;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
}
} else {
resetEKFGSF();
}
}
}