firx fast slam bug

Author: AtsushiSakai

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -1,6 +1,6 @@
 """
 
-Uncented kalman filter (UKF) localization sample
+Unscented kalman filter (UKF) localization sample
 
 author: Atsushi Sakai (@Atsushi_twi)
 
@@ -32,8 +32,8 @@
 
 def calc_input():
     v = 1.0  # [m/s]
-    yawrate = 0.1  # [rad/s]
-    u = np.array([[v, yawrate]]).T
+    yawRate = 0.1  # [rad/s]
+    u = np.array([[v, yawRate]]).T
     return u
 
 
@@ -100,15 +100,15 @@ def generate_sigma_points(xEst, PEst, gamma):
 
 
 def predict_sigma_motion(sigma, u):
-    #  Sigma Points predition with motion model
+    #  Sigma Points prediction with motion model
     for i in range(sigma.shape[1]):
         sigma[:, i:i + 1] = motion_model(sigma[:, i:i + 1], u)
 
     return sigma
 
 
 def predict_sigma_observation(sigma):
-    # Sigma Points predition with observation model
+    # Sigma Points prediction with observation model
     for i in range(sigma.shape[1]):
         sigma[0:2, i] = observation_model(sigma[:, i])
 
@@ -187,14 +187,14 @@ def plot_covariance_ellipse(xEst, PEst):
 
 
 def setup_ukf(nx):
-    lamda = ALPHA ** 2 * (nx + KAPPA) - nx
+    lamb = ALPHA ** 2 * (nx + KAPPA) - nx
     # calculate weights
-    wm = [lamda / (lamda + nx)]
-    wc = [(lamda / (lamda + nx)) + (1 - ALPHA ** 2 + BETA)]
+    wm = [lamb / (lamb + nx)]
+    wc = [(lamb / (lamb + nx)) + (1 - ALPHA ** 2 + BETA)]
     for i in range(2 * nx):
-        wm.append(1.0 / (2 * (nx + lamda)))
-        wc.append(1.0 / (2 * (nx + lamda)))
-    gamma = math.sqrt(nx + lamda)
+        wm.append(1.0 / (2 * (nx + lamb)))
+        wc.append(1.0 / (2 * (nx + lamb)))
+    gamma = math.sqrt(nx + lamb)
 
     wm = np.array([wm])
     wc = np.array([wc])

SLAM/FastSLAM1/fast_slam1.py

@@ -190,15 +190,14 @@ def compute_weight(particle, z, Q):
     dx = z[0, 0: 2].T - zp
     dx[1, 0] = pi_2_pi(dx[1, 0])
 
-    S = particle.lmP[2 * lm_id:2 * lm_id + 2]
     try:
-        invS = np.linalg.inv(S)
+        invS = np.linalg.inv(Sf)
     except np.linalg.linalg.LinAlgError:
         print("singuler")
         return 1.0
 
     num = math.exp(-0.5 * dx.T * invS * dx)
-    den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
+    den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
 
     w = num / den
 

SLAM/FastSLAM2/fast_slam2.py

@@ -191,15 +191,14 @@ def compute_weight(particle, z, Q):
     dz = z[0, 0: 2].T - zp
     dz[1, 0] = pi_2_pi(dz[1, 0])
 
-    S = particle.lmP[2 * lm_id:2 * lm_id + 2]
     try:
-        invS = np.linalg.inv(S)
+        invS = np.linalg.inv(Sf)
     except np.linalg.linalg.LinAlgError:
         print("singuler")
         return 1.0
 
     num = math.exp(-0.5 * dz.T * invS * dz)
-    den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
+    den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
 
     w = num / den