diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 8404646c..db715a6d 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -68,7 +68,7 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std): # predicted range x = ix * gmap.xy_reso + gmap.minx y = iy * gmap.xy_reso + gmap.miny - d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2) + d = math.hypot(x - z[iz, 1], y - z[iz, 2]) # likelihood pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))