diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index de2e40ec..1bd924f9 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/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 @@ show_animation = True 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,7 +100,7 @@ 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) @@ -108,7 +108,7 @@ def predict_sigma_motion(sigma, u): 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]) diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index c9d029f9..1841849f 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/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 diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 8134f8fd..16628975 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/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