Skip to content

Commit 9225c6f

Browse files
committed
feat(ekf_localizer): check zero division (#1687)
feat: check zero division Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
1 parent 2902675 commit 9225c6f

File tree

1 file changed

+4
-1
lines changed

1 file changed

+4
-1
lines changed

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,10 @@ class Simple1DFilter
7979
dev_ = dev_ + proc_dev_x_d;
8080

8181
// Update step
82-
double kalman_gain = dev_ / (dev_ + obs_dev);
82+
double kalman_gain = 1.0;
83+
if (dev_ + obs_dev != 0) {
84+
kalman_gain = dev_ / (dev_ + obs_dev);
85+
}
8386
x_ = x_ + kalman_gain * (obs - x_);
8487
dev_ = (1 - kalman_gain) * dev_;
8588

0 commit comments

Comments
 (0)