feat(ekf_localizer): check zero division (#1687)

feat: check zero division Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
tier4 · Dec 6, 2024 · 90001d7 · 90001d7
1 parent 4569640
commit 90001d7
Showing 1 changed file with 4 additions and 1 deletion.
diff --git 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_;