feat: check zero division

Signed-off-by: TetsuKawa <[email protected]>
tier4 · Dec 6, 2024 · 186b343 · 186b343
1 parent db51b36
commit 186b343
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_;