diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 3d4b0f643d..99c31339de 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -97,32 +97,6 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, // all weights have collapsed due to excessive innovation variances so reset filters initialiseEKFGSF(); } - - // Enforce a minimum weighting value. This was added during initial development but has not been needed - // subsequently, so this block of code and the corresponding _weight_min can be removed if we get - // through testing without any weighting function issues. - if (_weight_min > FLT_EPSILON) { - float correction_sum = 0.0f; // amount the sum of weights has been increased by application of the limit - bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased - float unmodified_weights_sum = 0.0f; // sum of unmodified weights - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - if (_model_weights(model_index) < _weight_min) { - correction_sum += _weight_min - _model_weights(model_index); - _model_weights(model_index) = _weight_min; - change_mask[model_index] = true; - } else { - unmodified_weights_sum += _model_weights(model_index); - } - } - - // rescale the unmodified weights to make the total sum unity - const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min); - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - if (!change_mask[model_index]) { - _model_weights(model_index) = _weight_min + scale_factor * (_model_weights(model_index) - _weight_min); - } - } - } } } } else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) { diff --git a/EKF/EKFGSF_yaw.h b/EKF/EKFGSF_yaw.h index 9e1e6291e4..3f74e50257 100644 --- a/EKF/EKFGSF_yaw.h +++ b/EKF/EKFGSF_yaw.h @@ -58,7 +58,6 @@ private: const float _accel_noise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2) const float _tilt_gain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec) const float _gyro_bias_gain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec) - const float _weight_min{0.0f}; // minimum value of an individual model weighting // Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters