Merge pull request AtsushiSakai#238 from AtsushiSakai/fix_randn_problem_with_covariance_matrix

fix_randn_problem_with_covariance_matrix

Author: AtsushiSakai

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -1,4 +1,3 @@
-
 """
 
 Ensemble Kalman Filter(EnKF) localization sample
@@ -10,13 +9,14 @@
 
 """
 
-import numpy as np
 import math
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 #  Simulation parameter
-Qsim = np.diag([0.2, np.deg2rad(1.0)])**2
-Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
+Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 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]
@@ -30,13 +30,12 @@
 
 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):
-
     xTrue = motion_model(xTrue, u)
 
     z = np.zeros((0, 4))
@@ -45,18 +44,18 @@ def observation(xTrue, xd, u, RFID):
 
         dx = RFID[i, 0] - xTrue[0, 0]
         dy = RFID[i, 1] - xTrue[1, 0]
-        d = math.sqrt(dx**2 + dy**2)
+        d = math.sqrt(dx ** 2 + dy ** 2)
         angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
         if d <= MAX_RANGE:
-            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
-            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
+            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]])
             z = np.vstack((z, zi))
 
     # add noise to input
     ud = np.array([[
-        u[0, 0] + np.random.randn() * Rsim[0, 0],
-        u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
+        u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5,
+        u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5]]).T
 
     xd = motion_model(xd, ud)
     return xTrue, z, xd, ud
@@ -77,15 +76,13 @@ def motion_model(x, u):
     return x
 
 
-def calc_LM_Pos(x, landmarks):
-    landmarks_pos = np.zeros((2*landmarks.shape[0], 1))
+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() * \
-            Qsim[0, 0]/np.sqrt(2)
-        landmarks_pos[2*i+1] = x[1, 0] + lm[0] * \
-            math.sin(x[2, 0] + lm[1]) + np.random.randn() * \
-            Qsim[0, 0]/np.sqrt(2)
+        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)
     return landmarks_pos
 
 
@@ -95,25 +92,26 @@ def calc_covariance(xEst, px):
     for i in range(px.shape[1]):
         dx = (px[:, i] - xEst)[0:3]
         cov += dx.dot(dx.T)
+    cov /= NP
 
     return cov
 
 
-def enkf_localization(px, xEst, PEst, z, u):
+def enkf_localization(px, z, u):
     """
     Localization with Ensemble Kalman filter
     """
-    pz = np.zeros((z.shape[0]*2, NP))  # Particle store of z
+    pz = np.zeros((z.shape[0] * 2, NP))  # Particle store of z
     for ip in range(NP):
         x = np.array([px[:, ip]]).T
 
         #  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_sim[0, 0] ** 0.5
+        ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
         ud = np.array([[ud1, ud2]]).T
         x = motion_model(x, ud)
         px[:, ip] = x[:, 0]
-        z_pos = calc_LM_Pos(x, z)
+        z_pos = observe_landmark_position(x, z)
         pz[:, ip] = z_pos[:, 0]
 
     x_ave = np.mean(px, axis=1)
@@ -122,12 +120,12 @@ def enkf_localization(px, xEst, PEst, z, u):
     z_ave = np.mean(pz, axis=1)
     z_dif = pz - np.tile(z_ave, (NP, 1)).T
 
-    U = 1/(NP-1) * x_dif @ z_dif.T
-    V = 1/(NP-1) * z_dif @ z_dif.T
+    U = 1 / (NP - 1) * x_dif @ z_dif.T
+    V = 1 / (NP - 1) * z_dif @ z_dif.T
 
     K = U @ np.linalg.inv(V)  # Kalman Gain
 
-    z_lm_pos = z[:, [2, 3]].reshape(-1,)
+    z_lm_pos = z[:, [2, 3]].reshape(-1, )
 
     px_hat = px + K @ (np.tile(z_lm_pos, (NP, 1)).T - pz)
 
@@ -139,32 +137,32 @@ def enkf_localization(px, xEst, PEst, z, u):
 
 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])
+    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]]))
@@ -182,17 +180,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]
+    RF_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 of x
 
     xDR = np.zeros((4, 1))  # Dead reckoning
@@ -206,9 +202,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, RF_ID)
 
-        xEst, PEst, px = enkf_localization(px, xEst, PEst, z, ud)
+        xEst, PEst, px = enkf_localization(px, z, ud)
 
         # store data history
         hxEst = np.hstack((hxEst, xEst))
@@ -220,20 +216,19 @@ def main():
 
             for i in range(len(z[:, 0])):
                 plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
-            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+            plt.plot(RF_ID[:, 0], RF_ID[:, 1], "*k")
             plt.plot(px[0, :], px[1, :], ".r")
             plt.plot(np.array(hxTrue[0, :]).flatten(),
                      np.array(hxTrue[1, :]).flatten(), "-b")
             plt.plot(np.array(hxDR[0, :]).flatten(),
                      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)
 
 
 if __name__ == '__main__':
     main()
-

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -7,21 +7,22 @@
 """
 
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 # Covariance for EKF simulation
 Q = np.diag([
-    0.1, # variance of location on x-axis
-    0.1, # variance of location on y-axis
-    np.deg2rad(1.0), # variance of yaw angle
-    1.0 # variance of velocity
-    ])**2  # predict state covariance
-R = np.diag([1.0, 1.0])**2  # Observation x,y position covariance
+    0.1,  # variance of location on x-axis
+    0.1,  # variance of location on y-axis
+    np.deg2rad(1.0),  # variance of yaw angle
+    1.0  # variance of velocity
+]) ** 2  # predict state covariance
+R = np.diag([1.0, 1.0]) ** 2  # Observation x,y position covariance
 
 #  Simulation parameter
-INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
-GPS_NOISE = np.diag([0.5, 0.5])**2
+INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
+GPS_NOISE = np.diag([0.5, 0.5]) ** 2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -37,7 +38,6 @@ def calc_input():
 
 
 def observation(xTrue, xd, u):
-
     xTrue = motion_model(xTrue, u)
 
     # add noise to gps x-y
@@ -52,7 +52,6 @@ def observation(xTrue, xd, u):
 
 
 def motion_model(x, u):
-
     F = np.array([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
@@ -79,7 +78,7 @@ def observation_model(x):
     return z
 
 
-def jacobF(x, u):
+def jacob_f(x, u):
     """
     Jacobian of Motion Model
 
@@ -105,7 +104,7 @@ def jacobF(x, u):
     return jF
 
 
-def jacobH(x):
+def jacob_h():
     # Jacobian of Observation Model
     jH = np.array([
         [1, 0, 0, 0],
@@ -116,20 +115,19 @@ def jacobH(x):
 
 
 def ekf_estimation(xEst, PEst, z, u):
-
     #  Predict
     xPred = motion_model(xEst, u)
-    jF = jacobF(xPred, u)
-    PPred = jF@PEst@jF.T + Q
+    jF = jacob_f(xPred, u)
+    PPred = jF @ PEst @ jF.T + Q
 
     #  Update
-    jH = jacobH(xPred)
+    jH = jacob_h()
     zPred = observation_model(xPred)
     y = z - zPred
-    S = jH@PPred@jH.T + R
-    K = PPred@jH.T@np.linalg.inv(S)
-    xEst = xPred + K@y
-    PEst = (np.eye(len(xEst)) - K@jH)@PPred
+    S = jH @ PPred @ jH.T + R
+    K = PPred @ jH.T @ np.linalg.inv(S)
+    xEst = xPred + K @ y
+    PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
 
     return xEst, PEst
 
@@ -151,9 +149,9 @@ 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(eigvec[bigind, 1], eigvec[bigind, 0])
-    R = np.array([[math.cos(angle), math.sin(angle)],
-                  [-math.sin(angle), math.cos(angle)]])
-    fx = R@(np.array([x, y]))
+    rot = np.array([[math.cos(angle), math.sin(angle)],
+                    [-math.sin(angle), math.cos(angle)]])
+    fx = rot @ (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")