github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:28:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

firx fast slam bug

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-29 20:30:51 +09:00
parent e0c15292b2
commit 44231dcab7
3 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
Localization/unscented_kalman_filter/unscented_kalman_filter.py
View File

@@ -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])

5
SLAM/FastSLAM1/fast_slam1.py
View File

@@ -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

5
SLAM/FastSLAM2/fast_slam2.py
View File

@@ -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