diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 65003f5fe9864..8e8dcf3ee124f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -79,7 +79,10 @@ class Simple1DFilter dev_ = dev_ + proc_dev_x_d; // Update step - double kalman_gain = dev_ / (dev_ + obs_dev); + double kalman_gain = 1.0; + if (dev_ + obs_dev != 0) { + kalman_gain = dev_ / (dev_ + obs_dev); + } x_ = x_ + kalman_gain * (obs - x_); dev_ = (1 - kalman_gain) * dev_;