Merge pull request AtsushiSakai#235 from AtsushiSakai/fix_particle_filter

Fix AtsushiSakai#229

Author: AtsushiSakai

Localization/particle_filter/particle_filter.py

@@ -6,17 +6,18 @@
 
 """
 
-import numpy as np
 import math
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 # Estimation parameter of PF
-Q = np.diag([0.1])**2  # range error
-R = np.diag([1.0, np.deg2rad(40.0)])**2  # input error
+Q = np.diag([0.2]) ** 2  # range error
+R = np.diag([2.0, np.deg2rad(40.0)]) ** 2  # input error
 
 #  Simulation parameter
-Qsim = np.diag([0.2])**2
-Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
+Q_sim = np.diag([0.2]) ** 2
+R_sim = np.diag([1.0, np.deg2rad(30.0)]) ** 2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -31,31 +32,30 @@
 
 def calc_input():
     v = 1.0  # [m/s]
-    yawrate = 0.1  # [rad/s]
-    u = np.array([[v, yawrate]]).T
+    yaw_rate = 0.1  # [rad/s]
+    u = np.array([[v, yaw_rate]]).T
     return u
 
 
-def observation(xTrue, xd, u, RFID):
-
+def observation(xTrue, xd, u, RF_ID):
     xTrue = motion_model(xTrue, u)
 
     # add noise to gps x-y
     z = np.zeros((0, 3))
 
-    for i in range(len(RFID[:, 0])):
+    for i in range(len(RF_ID[:, 0])):
 
-        dx = xTrue[0, 0] - RFID[i, 0]
-        dy = xTrue[1, 0] - RFID[i, 1]
-        d = math.sqrt(dx**2 + dy**2)
+        dx = xTrue[0, 0] - RF_ID[i, 0]
+        dy = xTrue[1, 0] - RF_ID[i, 1]
+        d = math.sqrt(dx ** 2 + dy ** 2)
         if d <= MAX_RANGE:
-            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
-            zi = np.array([[dn, RFID[i, 0], RFID[i, 1]]])
+            dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5  # add noise
+            zi = np.array([[dn, RF_ID[i, 0], RF_ID[i, 1]]])
             z = np.vstack((z, zi))
 
     # add noise to input
-    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
-    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
+    ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
+    ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
     ud = np.array([[ud1, ud2]]).T
 
     xd = motion_model(xd, ud)
@@ -64,7 +64,6 @@ def observation(xTrue, xd, u, RFID):
 
 
 def motion_model(x, u):
-
     F = np.array([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
@@ -93,30 +92,32 @@ def calc_covariance(xEst, px, pw):
     for i in range(px.shape[1]):
         dx = (px[:, i] - xEst)[0:3]
         cov += pw[0, i] * dx.dot(dx.T)
+    cov /= NP
 
     return cov
 
 
-def pf_localization(px, pw, xEst, PEst, z, u):
+def pf_localization(px, pw, z, u):
     """
     Localization with Particle filter
     """
 
     for ip in range(NP):
         x = np.array([px[:, ip]]).T
         w = pw[0, ip]
+
         #  Predict with random input sampling
-        ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
-        ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
+        ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
+        ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
         ud = np.array([[ud1, ud2]]).T
         x = motion_model(x, ud)
 
         #  Calc Importance Weight
         for i in range(len(z[:, 0])):
             dx = x[0, 0] - z[i, 1]
             dy = x[1, 0] - z[i, 2]
-            prez = math.sqrt(dx**2 + dy**2)
-            dz = prez - z[i, 0]
+            pre_z = math.sqrt(dx ** 2 + dy ** 2)
+            dz = pre_z - z[i, 0]
             w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
 
         px[:, ip] = x[:, 0]
@@ -127,66 +128,66 @@ def pf_localization(px, pw, xEst, PEst, z, u):
     xEst = px.dot(pw.T)
     PEst = calc_covariance(xEst, px, pw)
 
-    px, pw = resampling(px, pw)
+    px, pw = re_sampling(px, pw)
 
     return xEst, PEst, px, pw
 
 
-def resampling(px, pw):
+def re_sampling(px, pw):
     """
     low variance re-sampling
     """
 
-    Neff = 1.0 / (pw.dot(pw.T))[0, 0]  # Effective particle number
-    if Neff < NTh:
-        wcum = np.cumsum(pw)
+    N_eff = 1.0 / (pw.dot(pw.T))[0, 0]  # Effective particle number
+    if N_eff < NTh:
+        w_cum = np.cumsum(pw)
         base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
-        resampleid = base + np.random.rand(base.shape[0]) / NP
+        re_sample_id = base + np.random.rand(base.shape[0]) / NP
 
-        inds = []
+        indexes = []
         ind = 0
         for ip in range(NP):
-            while resampleid[ip] > wcum[ind]:
+            while re_sample_id[ip] > w_cum[ind]:
                 ind += 1
-            inds.append(ind)
+            indexes.append(ind)
 
-        px = px[:, inds]
+        px = px[:, indexes]
         pw = np.zeros((1, NP)) + 1.0 / NP  # init weight
 
     return px, pw
 
 
 def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     Pxy = PEst[0:2, 0:2]
-    eigval, eigvec = np.linalg.eig(Pxy)
+    eig_val, eig_vec = np.linalg.eig(Pxy)
 
-    if eigval[0] >= eigval[1]:
-        bigind = 0
-        smallind = 1
+    if eig_val[0] >= eig_val[1]:
+        big_ind = 0
+        small_ind = 1
     else:
-        bigind = 1
-        smallind = 0
+        big_ind = 1
+        small_ind = 0
 
     t = np.arange(0, 2 * math.pi + 0.1, 0.1)
 
-    # eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
+    # 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(eigval[bigind])
+        a = math.sqrt(eig_val[big_ind])
     except ValueError:
         a = 0
 
     try:
-        b = math.sqrt(eigval[smallind])
+        b = math.sqrt(eig_val[small_ind])
     except ValueError:
         b = 0
 
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
-    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
-    R = np.array([[math.cos(angle), math.sin(angle)],
-                  [-math.sin(angle), math.cos(angle)]])
-    fx = R.dot(np.array([[x, y]]))
+    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]]))
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")
@@ -197,16 +198,15 @@ def main():
 
     time = 0.0
 
-    # RFID positions [x, y]
-    RFID = np.array([[10.0, 0.0],
-                     [10.0, 10.0],
-                     [0.0, 15.0],
-                     [-5.0, 20.0]])
+    # RF_ID positions [x, y]
+    RFi_ID = np.array([[10.0, 0.0],
+                       [10.0, 10.0],
+                       [0.0, 15.0],
+                       [-5.0, 20.0]])
 
     # State Vector [x y yaw v]'
     xEst = np.zeros((4, 1))
     xTrue = np.zeros((4, 1))
-    PEst = np.eye(4)
 
     px = np.zeros((4, NP))  # Particle store
     pw = np.zeros((1, NP)) + 1.0 / NP  # Particle weight
@@ -221,9 +221,9 @@ def main():
         time += DT
         u = calc_input()
 
-        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFi_ID)
 
-        xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
+        xEst, PEst, px, pw = pf_localization(px, pw, z, ud)
 
         # store data history
         hxEst = np.hstack((hxEst, xEst))
@@ -235,7 +235,7 @@ def main():
 
             for i in range(len(z[:, 0])):
                 plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
-            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+            plt.plot(RFi_ID[:, 0], RFi_ID[:, 1], "*k")
             plt.plot(px[0, :], px[1, :], ".r")
             plt.plot(np.array(hxTrue[0, :]).flatten(),
                      np.array(hxTrue[1, :]).flatten(), "-b")