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

add fastslam2 gif

Browse Source
This commit is contained in:
Atsushi Sakai
2018-03-22 14:38:35 -07:00
parent 9b73af28d0
commit 119400e035
2 changed files with 16 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
SLAM/FastSLAM2/animation.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 7.7 MiB

27
SLAM/FastSLAM2/fast_slam2.py
View File

@@ -27,7 +27,7 @@ M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM srate size [x,y]
N_PARTICLE = 100 # number of particle
NTH = N_PARTICLE / 1.5 # Number of particle for re-sampling
NTH = N_PARTICLE / 1.0 # Number of particle for re-sampling
show_animation = True
@@ -61,15 +61,15 @@ def normalize_weight(particles):
sumw = sum([p.w for p in particles])
if sumw <= 0.0000001:
try:
for i in range(N_PARTICLE):
particles[i].w /= sumw
except ZeroDivisionError:
for i in range(N_PARTICLE):
particles[i].w = 1.0 / N_PARTICLE
return particles
for i in range(N_PARTICLE):
particles[i].w /= sumw
return particles
@@ -188,8 +188,8 @@ def compute_weight(particle, z, Q):
Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2])
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
dx = z[0, 0: 2].T - zp
dx[1, 0] = pi_2_pi(dx[1, 0])
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:
@@ -198,7 +198,7 @@ def compute_weight(particle, z, Q):
print("singuler")
return 1.0
num = math.exp(-0.5 * dx.T * invS * dx)
num = math.exp(-0.5 * dz.T * invS * dz)
den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
w = num / den
@@ -247,8 +247,8 @@ def update_with_observation(particles, z):
w = compute_weight(particles[ip], z[iz, :], Q)
particles[ip].w *= w
particles[ip] = proposal_sampling(particles[ip], z[iz, :], Q)
particles[ip] = update_landmark(particles[ip], z[iz, :], Q)
particles[ip] = proposal_sampling(particles[ip], z[iz, :], Q)
return particles
@@ -267,7 +267,7 @@ def resampling(particles):
pw = np.matrix(pw)
Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number
# print(Neff)
print(Neff)
if Neff < NTH: # resampling
wcum = np.cumsum(pw)
@@ -319,7 +319,7 @@ 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)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
angle = 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
@@ -414,6 +414,11 @@ def main():
plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
for iz in range(len(z[:, 0])):
lmid = int(z[iz, 2])
plt.plot([xEst[0, 0], RFID[lmid, 0]], [
xEst[1, 0], RFID[lmid, 1]], "-k")
for i in range(N_PARTICLE):
plt.plot(particles[i].x, particles[i].y, ".r")
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")