Using scipy.spatial.rotation matrix (AtsushiSakai#335)

Author: AtsushiSakai

.travis.yml

@@ -18,10 +18,9 @@ before_install:
     - conda update -q conda
     # Useful for debugging any issues with conda
     - conda info -a
-#    - conda install python==3.6.8
 
 install:
-  - conda install numpy==1.15
+  - conda install numpy
   - conda install scipy
   - conda install matplotlib
   - conda install pandas

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -5,14 +5,16 @@
 author: Ryohei Sasaki(rsasaki0109)
 
 Ref:
-- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
+Ensemble Kalman filtering
+(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
 
 """
 
 import math
 
 import matplotlib.pyplot as plt
 import numpy as np
+from scipy.spatial.transform import Rotation as Rot
 
 #  Simulation parameter
 Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -48,8 +50,8 @@ def observation(xTrue, xd, u, RFID):
         angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
         if d <= MAX_RANGE:
             dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5  # add noise
-            anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
-            zi = np.array([dn, anglen, RFID[i, 0], RFID[i, 1]])
+            angle_with_noise = angle + np.random.randn() * Q_sim[1, 1] ** 0.5
+            zi = np.array([dn, angle_with_noise, RFID[i, 0], RFID[i, 1]])
             z = np.vstack((z, zi))
 
     # add noise to input
@@ -79,10 +81,12 @@ def motion_model(x, u):
 def observe_landmark_position(x, landmarks):
     landmarks_pos = np.zeros((2 * landmarks.shape[0], 1))
     for (i, lm) in enumerate(landmarks):
-        landmarks_pos[2 * i] = x[0, 0] + lm[0] * math.cos(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
-            0, 0] ** 0.5 / np.sqrt(2)
-        landmarks_pos[2 * i + 1] = x[1, 0] + lm[0] * math.sin(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
-            0, 0] ** 0.5 / np.sqrt(2)
+        index = 2 * i
+        q = Q_sim[0, 0] ** 0.5
+        landmarks_pos[index] = x[0, 0] + lm[0] * math.cos(
+            x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
+        landmarks_pos[index + 1] = x[1, 0] + lm[0] * math.sin(
+            x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
     return landmarks_pos
 
 
@@ -148,8 +152,9 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
 
     t = np.arange(0, 2 * math.pi + 0.1, 0.1)
 
-    # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
-    # close to 0 (~10^-20), catch these cases and set the respective variable to 0
+    # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
+    # numbers extremely close to 0 (~10^-20), catch these cases and set
+    # the respective variable to 0
     try:
         a = math.sqrt(eig_val[big_ind])
     except ValueError:
@@ -163,11 +168,11 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
-    R = np.array([[math.cos(angle), math.sin(angle)],
-                  [-math.sin(angle), math.cos(angle)]])
-    fx = R.dot(np.array([[x, y]]))
-    px = np.array(fx[0, :] + xEst[0, 0]).flatten()
-    py = np.array(fx[1, :] + xEst[1, 0]).flatten()
+    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
+    fx = np.stack([x, y]).T @ rot
+
+    px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
+    py = np.array(fx[:, 1] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")
 
 
@@ -214,8 +219,9 @@ def main():
         if show_animation:
             plt.cla()
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect('key_release_event',
-                    lambda event: [exit(0) if event.key == 'escape' else None])
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
 
             for i in range(len(z[:, 0])):
                 plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
@@ -227,7 +233,7 @@ def main():
                      np.array(hxDR[1, :]).flatten(), "-k")
             plt.plot(np.array(hxEst[0, :]).flatten(),
                      np.array(hxEst[1, :]).flatten(), "-r")
-            # plot_covariance_ellipse(xEst, PEst)
+            plot_covariance_ellipse(xEst, PEst)
             plt.axis("equal")
             plt.grid(True)
             plt.pause(0.001)

Localization/histogram_filter/histogram_filter.py

@@ -28,11 +28,11 @@
 RANGE_STD = 3.0  # standard deviation for observation gaussian distribution
 
 # grid map param
-XY_RESO = 0.5  # xy grid resolution
-MINX = -15.0
-MINY = -5.0
-MAXX = 15.0
-MAXY = 25.0
+XY_RESOLUTION = 0.5  # xy grid resolution
+MIN_X = -15.0
+MIN_Y = -5.0
+MAX_X = 15.0
+MAX_Y = 25.0
 
 # simulation parameters
 NOISE_RANGE = 2.0  # [m] 1σ range noise parameter
@@ -41,17 +41,17 @@
 show_animation = True
 
 
-class GridMap():
+class GridMap:
 
     def __init__(self):
         self.data = None
-        self.xy_reso = None
-        self.minx = None
-        self.miny = None
-        self.maxx = None
-        self.maxx = None
-        self.xw = None
-        self.yw = None
+        self.xy_resolution = None
+        self.min_x = None
+        self.min_y = None
+        self.max_x = None
+        self.max_y = None
+        self.x_w = None
+        self.y_w = None
         self.dx = 0.0  # movement distance
         self.dy = 0.0  # movement distance
 
@@ -64,10 +64,10 @@ def histogram_filter_localization(grid_map, u, z, yaw):
     return grid_map
 
 
-def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
+def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
     # predicted range
-    x = ix * gmap.xy_reso + gmap.minx
-    y = iy * gmap.xy_reso + gmap.miny
+    x = ix * grid_map.xy_resolution + grid_map.min_x
+    y = iy * grid_map.xy_resolution + grid_map.min_y
     d = math.hypot(x - z[iz, 1], y - z[iz, 2])
 
     # likelihood
@@ -76,16 +76,16 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
     return pdf
 
 
-def observation_update(gmap, z, std):
+def observation_update(grid_map, z, std):
     for iz in range(z.shape[0]):
-        for ix in range(gmap.xw):
-            for iy in range(gmap.yw):
-                gmap.data[ix][iy] *= calc_gaussian_observation_pdf(
-                    gmap, z, iz, ix, iy, std)
+        for ix in range(grid_map.x_w):
+            for iy in range(grid_map.y_w):
+                grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
+                    grid_map, z, iz, ix, iy, std)
 
-    gmap = normalize_probability(gmap)
+    grid_map = normalize_probability(grid_map)
 
-    return gmap
+    return grid_map
 
 
 def calc_input():
@@ -112,8 +112,8 @@ def motion_model(x, u):
 
 
 def draw_heat_map(data, mx, my):
-    maxp = max([max(igmap) for igmap in data])
-    plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues"))
+    max_value = max([max(i_data) for i_data in data])
+    plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
     plt.axis("equal")
 
 
@@ -140,43 +140,47 @@ def observation(xTrue, u, RFID):
     return xTrue, z, ud
 
 
-def normalize_probability(gmap):
-    sump = sum([sum(igmap) for igmap in gmap.data])
+def normalize_probability(grid_map):
+    sump = sum([sum(i_data) for i_data in grid_map.data])
 
-    for ix in range(gmap.xw):
-        for iy in range(gmap.yw):
-            gmap.data[ix][iy] /= sump
+    for ix in range(grid_map.x_w):
+        for iy in range(grid_map.y_w):
+            grid_map.data[ix][iy] /= sump
 
-    return gmap
+    return grid_map
 
 
-def init_gmap(xy_reso, minx, miny, maxx, maxy):
+def init_grid_map(xy_resolution, min_x, min_y, max_x, max_y):
     grid_map = GridMap()
 
-    grid_map.xy_reso = xy_reso
-    grid_map.minx = minx
-    grid_map.miny = miny
-    grid_map.maxx = maxx
-    grid_map.maxy = maxy
-    grid_map.xw = int(round((grid_map.maxx - grid_map.minx) / grid_map.xy_reso))
-    grid_map.yw = int(round((grid_map.maxy - grid_map.miny) / grid_map.xy_reso))
-
-    grid_map.data = [[1.0 for _ in range(grid_map.yw)] for _ in range(grid_map.xw)]
+    grid_map.xy_resolution = xy_resolution
+    grid_map.min_x = min_x
+    grid_map.min_y = min_y
+    grid_map.max_x = max_x
+    grid_map.max_y = max_y
+    grid_map.x_w = int(round((grid_map.max_x - grid_map.min_x)
+                             / grid_map.xy_resolution))
+    grid_map.y_w = int(round((grid_map.max_y - grid_map.min_y)
+                             / grid_map.xy_resolution))
+
+    grid_map.data = [[1.0 for _ in range(grid_map.y_w)]
+                     for _ in range(grid_map.x_w)]
     grid_map = normalize_probability(grid_map)
 
     return grid_map
 
 
 def map_shift(grid_map, x_shift, y_shift):
-    tgmap = copy.deepcopy(grid_map.data)
+    tmp_grid_map = copy.deepcopy(grid_map.data)
 
-    for ix in range(grid_map.xw):
-        for iy in range(grid_map.yw):
+    for ix in range(grid_map.x_w):
+        for iy in range(grid_map.y_w):
             nix = ix + x_shift
             niy = iy + y_shift
 
-            if 0 <= nix < grid_map.xw and 0 <= niy < grid_map.yw:
-                grid_map.data[ix + x_shift][iy + y_shift] = tgmap[ix][iy]
+            if 0 <= nix < grid_map.x_w and 0 <= niy < grid_map.y_w:
+                grid_map.data[ix + x_shift][iy + y_shift] =\
+                    tmp_grid_map[ix][iy]
 
     return grid_map
 
@@ -185,22 +189,26 @@ def motion_update(grid_map, u, yaw):
     grid_map.dx += DT * math.cos(yaw) * u[0]
     grid_map.dy += DT * math.sin(yaw) * u[0]
 
-    x_shift = grid_map.dx // grid_map.xy_reso
-    y_shift = grid_map.dy // grid_map.xy_reso
+    x_shift = grid_map.dx // grid_map.xy_resolution
+    y_shift = grid_map.dy // grid_map.xy_resolution
 
     if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0:  # map should be shifted
         grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
-        grid_map.dx -= x_shift * grid_map.xy_reso
-        grid_map.dy -= y_shift * grid_map.xy_reso
+        grid_map.dx -= x_shift * grid_map.xy_resolution
+        grid_map.dy -= y_shift * grid_map.xy_resolution
 
     grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
 
     return grid_map
 
 
-def calc_grid_index(gmap):
-    mx, my = np.mgrid[slice(gmap.minx - gmap.xy_reso / 2.0, gmap.maxx + gmap.xy_reso / 2.0, gmap.xy_reso),
-                      slice(gmap.miny - gmap.xy_reso / 2.0, gmap.maxy + gmap.xy_reso / 2.0, gmap.xy_reso)]
+def calc_grid_index(grid_map):
+    mx, my = np.mgrid[slice(grid_map.min_x - grid_map.xy_resolution / 2.0,
+                            grid_map.max_x + grid_map.xy_resolution / 2.0,
+                            grid_map.xy_resolution),
+                      slice(grid_map.min_y - grid_map.xy_resolution / 2.0,
+                            grid_map.max_y + grid_map.xy_resolution / 2.0,
+                            grid_map.xy_resolution)]
 
     return mx, my
 
@@ -217,7 +225,7 @@ def main():
     time = 0.0
 
     xTrue = np.zeros((4, 1))
-    grid_map = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
+    grid_map = init_grid_map(XY_RESOLUTION, MIN_X, MIN_Y, MAX_X, MAX_Y)
     mx, my = calc_grid_index(grid_map)  # for grid map visualization
 
     while SIM_TIME >= time:
@@ -234,8 +242,9 @@ def main():
         if show_animation:
             plt.cla()
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect('key_release_event',
-                    lambda event: [exit(0) if event.key == 'escape' else None])
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
             draw_heat_map(grid_map.data, mx, my)
             plt.plot(xTrue[0, :], xTrue[1, :], "xr")
             plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")

Localization/particle_filter/particle_filter.py

@@ -10,6 +10,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from scipy.spatial.transform import Rotation as Rot
 
 # Estimation parameter of PF
 Q = np.diag([0.2]) ** 2  # range error
@@ -172,8 +173,9 @@ def plot_covariance_ellipse(x_est, p_est):  # pragma: no cover
 
     t = np.arange(0, 2 * math.pi + 0.1, 0.1)
 
-    # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
-    # close to 0 (~10^-20), catch these cases and set the respective variable to 0
+    # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
+    # numbers extremely close to 0 (~10^-20), catch these cases and set the
+    # respective variable to 0
     try:
         a = math.sqrt(eig_val[big_ind])
     except ValueError:
@@ -187,9 +189,8 @@ def plot_covariance_ellipse(x_est, p_est):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
-    Rot = np.array([[math.cos(angle), -math.sin(angle)],
-                    [math.sin(angle), math.cos(angle)]])
-    fx = Rot.dot(np.array([[x, y]]))
+    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
+    fx = rot.dot(np.array([[x, y]]))
     px = np.array(fx[0, :] + x_est[0, 0]).flatten()
     py = np.array(fx[1, :] + x_est[1, 0]).flatten()
     plt.plot(px, py, "--r")
@@ -235,8 +236,9 @@ def main():
         if show_animation:
             plt.cla()
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect('key_release_event',
-                                         lambda event: [exit(0) if event.key == 'escape' else None])
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
 
             for i in range(len(z[:, 0])):
                 plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")