From 94335fda61bb57f5d7b3743396f2345abeaf43e8 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Fri, 6 Dec 2024 22:13:34 +0900 Subject: [PATCH] feat(ekf_localizer): check zero division (#1687) feat: check zero division Signed-off-by: TetsuKawa --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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_;