almost done but needs code cleaning

Author: AtsushiSakai

Localization/histogram_filter/histogram_filter.py

@@ -2,6 +2,11 @@
 
 Histogram Filter 2D localization example
 
+
+In this simulation, x,y are unknown, yaw is known.
+
+Initial position is not needed.
+
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
@@ -11,29 +16,59 @@
 import matplotlib.pyplot as plt
 import copy
 from scipy.stats import norm
+from scipy.ndimage import gaussian_filter
+
+# Parameters
+NOISE_RANGE = 2.0  # [m] 1σ range noise parameter
+NOISE_SPEED = 0.5  # [m/s] 1σ speed noise parameter
 
 EXTEND_AREA = 10.0  # [m] grid map extention length
 SIM_TIME = 50.0  # simulation time [s]
 DT = 0.1  # time tick [s]
 MAX_RANGE = 10.0  # maximum observation range
+MOTION_STD = 1.0
+RANGE_STD = 3.0  # standard diviation for gaussian distribution
 
 show_animation = True
 
 
-def observation_update(gmap, z, std, xyreso, minx, miny):
+class grid_map():
+
+    def __init__(self):
+        self.data = None
+        self.xyreso = None
+        self.minx = None
+        self.miny = None
+        self.maxx = None
+        self.maxx = None
+
+
+def histogram_filter_localization(gmap, u, z, yaw, dx, dy):
+
+    gmap, dx, dy = motion_update(gmap, u, yaw, dx, dy)
+
+    gmap = observation_update(gmap, z, RANGE_STD)
+
+    return gmap.data, dx, dy
+
+
+def observation_update(gmap, z, std):
 
     for iz in range(z.shape[0]):
-        for ix in range(len(gmap)):
-            for iy in range(len(gmap[ix])):
+        for ix in range(len(gmap.data)):
+            for iy in range(len(gmap.data[ix])):
 
+                # observation range
                 zr = z[iz, 0]
-                x = ix * xyreso + minx
-                y = iy * xyreso + miny
 
+                # predicted range
+                x = ix * gmap.xyreso + gmap.minx
+                y = iy * gmap.xyreso + gmap.miny
                 d = math.sqrt((x - z[iz, 1])**2 + (y - z[iz, 2])**2)
 
+                # likelihood
                 pdf = (1.0 - norm.cdf(abs(d - zr), 0.0, std))
-                gmap[ix][iy] *= pdf
+                gmap.data[ix][iy] *= pdf
 
     gmap = normalize_probability(gmap)
 
@@ -64,20 +99,16 @@ def motion_model(x, u):
     return x
 
 
-def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
-    x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
-                    slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
-
+def draw_heatmap(data, mx, my):
     maxp = max([max(igmap) for igmap in data])
-    plt.pcolor(x, y, data, vmax=maxp, cmap=plt.cm.Blues)
+    plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.Blues)
     plt.axis("equal")
 
 
 def observation(xTrue, u, RFID):
 
     xTrue = motion_model(xTrue, u)
 
-    # add noise to gps x-y
     z = np.matrix(np.zeros((0, 3)))
 
     for i in range(len(RFID[:, 0])):
@@ -86,69 +117,79 @@ def observation(xTrue, u, RFID):
         dy = xTrue[1, 0] - RFID[i, 1]
         d = math.sqrt(dx**2 + dy**2)
         if d <= MAX_RANGE:
-            dn = d
+            # add noise to range observation
+            dn = d + np.random.randn() * NOISE_RANGE
             zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
             z = np.vstack((z, zi))
 
-    return xTrue, z
+    # add noise to speed
+    ud = u[:, :]
+    ud[0] += np.random.randn() * NOISE_SPEED
+
+    return xTrue, z, ud
 
 
 def normalize_probability(gmap):
 
-    sump = sum([sum(igmap) for igmap in gmap])
+    sump = sum([sum(igmap) for igmap in gmap.data])
 
-    for i in range(len(gmap)):
-        for ii in range(len(gmap[i])):
-            gmap[i][ii] /= sump
+    for i in range(len(gmap.data)):
+        for ii in range(len(gmap.data[i])):
+            gmap.data[i][ii] /= sump
 
     return gmap
 
 
 def init_gmap(xyreso):
 
-    minx = -15.0
-    miny = -5.0
-    maxx = 15.0
-    maxy = 25.0
-    xw = int(round((maxx - minx) / xyreso))
-    yw = int(round((maxy - miny) / xyreso))
+    gmap = grid_map()
 
-    gmap = [[1.0 for i in range(yw)] for i in range(xw)]
+    gmap.xyreso = xyreso
+    gmap.minx = -15.0
+    gmap.miny = -5.0
+    gmap.maxx = 15.0
+    gmap.maxy = 25.0
+    gmap.xw = int(round((gmap.maxx - gmap.minx) / gmap.xyreso))
+    gmap.yw = int(round((gmap.maxy - gmap.miny) / gmap.xyreso))
+
+    gmap.data = [[1.0 for i in range(gmap.yw)] for i in range(gmap.xw)]
     gmap = normalize_probability(gmap)
 
-    return gmap, minx, maxx, miny, maxy,
+    return gmap
 
 
 def map_shift(gmap, xshift, yshift):
 
-    tgmap = copy.deepcopy(gmap)
+    tgmap = copy.deepcopy(gmap.data)
 
-    lenx = len(gmap)
-    leny = len(gmap[0])
+    lenx = len(gmap.data)
+    leny = len(gmap.data[0])
 
     for ix in range(lenx):
         for iy in range(leny):
             nix = ix + xshift
             niy = iy + yshift
 
             if nix >= 0 and nix < lenx and niy >= 0 and niy < leny:
-                gmap[ix + xshift][iy + yshift] = tgmap[ix][iy]
+                gmap.data[ix + xshift][iy + yshift] = tgmap[ix][iy]
 
     return gmap
 
 
-def motion_update(gmap, u, yaw, dx, dy, xyreso, minx, miny):
+def motion_update(gmap, u, yaw, dx, dy):
 
     dx += DT * math.cos(yaw) * u[0]
     dy += DT * math.sin(yaw) * u[0]
 
-    xshift = dx // xyreso
-    yshift = dy // xyreso
+    xshift = dx // gmap.xyreso
+    yshift = dy // gmap.xyreso
 
     if abs(xshift) >= 1.0 or abs(yshift) >= 1.0:
         gmap = map_shift(gmap, int(xshift), int(yshift))
-        dx -= xshift * xyreso
-        dy -= yshift * xyreso
+        dx -= xshift * gmap.xyreso
+        dy -= yshift * gmap.xyreso
+
+    gmap.data = gaussian_filter(gmap.data, sigma=MOTION_STD)
 
     return gmap, dx, dy
 
@@ -157,7 +198,6 @@ def main():
     print(__file__ + " start!!")
 
     xyreso = 0.5  # xy grid resolution
-    STD = 1.0  # standard diviation for gaussian distribution
 
     # RFID positions [x, y]
     RFID = np.array([[10.0, 0.0],
@@ -169,24 +209,28 @@ def main():
 
     xTrue = np.matrix(np.zeros((4, 1)))
 
-    gmap, minx, maxx, miny, maxy = init_gmap(xyreso)
+    gmap = init_gmap(xyreso)
 
     dx, dy = 0.0, 0.0
 
+    mx, my = np.mgrid[slice(gmap.minx - gmap.xyreso / 2.0, gmap.maxx + gmap.xyreso / 2.0, gmap.xyreso),
+                      slice(gmap.miny - gmap.xyreso / 2.0, gmap.maxy + gmap.xyreso / 2.0, gmap.xyreso)]
+
     while SIM_TIME >= time:
         time += DT
 
         u = calc_input()
-        xTrue, z = observation(xTrue, u, RFID)
 
-        gmap, dx, dy = motion_update(
-            gmap, u, xTrue[2, 0], dx, dy, xyreso, minx, miny)
+        # Orientation is known in this simulation
+        yaw = xTrue[2, 0]
+        xTrue, z, ud = observation(xTrue, u, RFID)
 
-        gmap = observation_update(gmap, z, STD, xyreso, minx, miny)
+        gmap.data, dx, dy = histogram_filter_localization(
+            gmap, u, z, yaw, dx, dy)
 
         if show_animation:
             plt.cla()
-            draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
+            draw_heatmap(gmap.data, mx, my)
             plt.plot(xTrue[0, :], xTrue[1, :], "xr")
             plt.plot(RFID[:, 0], RFID[:, 1], ".k")
             for i in range(z.shape[0]):