diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2e2edc45f46..9eac764ed35 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -2209,7 +2209,6 @@ void Ekf2::update_gps_offsets() { // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset - // Increase the filter time constant proportional to the inverse of the weighting // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering float alpha[GPS_MAX_RECEIVERS] = {}; float omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f); @@ -2217,16 +2216,8 @@ void Ekf2::update_gps_offsets() for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { if (_gps_state[i].time_usec - _time_prev_us[i] > 0) { // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter - float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]), - 0.0f, 1.0f); - - // scale the filter coefficient so that time constant is inversely proprtional to weighting - if (_blend_weights[i] > min_alpha) { - alpha[i] = min_alpha / _blend_weights[i]; - - } else { - alpha[i] = 1.0f; - } + alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]), + 0.0f, 1.0f); _time_prev_us[i] = _gps_state[i].time_usec; }