Skip to content

Commit 26b2c55

Browse files
AtsushiSakaiAtsushiSakai
authored
fix inaccurate covariance calculation to add a new landmark in FastSLAM1,2 (AtsushiSakai#334)
1 parent 03a92fc commit 26b2c55

File tree

2 files changed

+96
-80
lines changed

2 files changed

+96
-80
lines changed

‎SLAM/FastSLAM1/fast_slam1.py

Lines changed: 58 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -16,16 +16,16 @@
1616
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2
1717

1818
# Simulation parameter
19-
Qsim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
20-
Rsim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
21-
OFFSET_YAWRATE_NOISE = 0.01
19+
Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
20+
R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
21+
OFFSET_YAW_RATE_NOISE = 0.01
2222

2323
DT = 0.1 # time tick [s]
2424
SIM_TIME = 50.0 # simulation time [s]
2525
MAX_RANGE = 20.0 # maximum observation range
2626
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
2727
STATE_SIZE = 3 # State size [x,y,yaw]
28-
LM_SIZE = 2 # LM srate size [x,y]
28+
LM_SIZE = 2 # LM state size [x,y]
2929
N_PARTICLE = 100 # number of particle
3030
NTH = N_PARTICLE / 1.5 # Number of particle for re-sampling
3131

@@ -34,15 +34,15 @@
3434

3535
class Particle:
3636

37-
def __init__(self, N_LM):
37+
def __init__(self, n_landmark):
3838
self.w = 1.0 / N_PARTICLE
3939
self.x = 0.0
4040
self.y = 0.0
4141
self.yaw = 0.0
4242
# landmark x-y positions
43-
self.lm = np.zeros((N_LM, LM_SIZE))
43+
self.lm = np.zeros((n_landmark, LM_SIZE))
4444
# landmark position covariance
45-
self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))
45+
self.lmP = np.zeros((n_landmark * LM_SIZE, LM_SIZE))
4646

4747

4848
def fast_slam1(particles, u, z):
@@ -56,11 +56,11 @@ def fast_slam1(particles, u, z):
5656

5757

5858
def normalize_weight(particles):
59-
sumw = sum([p.w for p in particles])
59+
sum_w = sum([p.w for p in particles])
6060

6161
try:
6262
for i in range(N_PARTICLE):
63-
particles[i].w /= sumw
63+
particles[i].w /= sum_w
6464
except ZeroDivisionError:
6565
for i in range(N_PARTICLE):
6666
particles[i].w = 1.0 / N_PARTICLE
@@ -101,7 +101,7 @@ def predict_particles(particles, u):
101101
return particles
102102

103103

104-
def add_new_lm(particle, z, Q_cov):
104+
def add_new_landmark(particle, z, Q_cov):
105105
r = z[0]
106106
b = z[1]
107107
lm_id = int(z[2])
@@ -113,10 +113,14 @@ def add_new_lm(particle, z, Q_cov):
113113
particle.lm[lm_id, 1] = particle.y + r * s
114114

115115
# covariance
116-
Gz = np.array([[c, -r * s],
117-
[s, r * c]])
118-
119-
particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz @ Q_cov @ Gz.T
116+
dx = r * c
117+
dy = r * s
118+
d2 = dx**2 + dy**2
119+
d = math.sqrt(d2)
120+
Gz = np.array([[dx / d, dy / d],
121+
[-dy / d2, dx / d2]])
122+
particle.lmP[2 * lm_id:2 * lm_id + 2] = np.linalg.inv(
123+
Gz) @ Q_cov @ np.linalg.inv(Gz.T)
120124

121125
return particle
122126

@@ -146,10 +150,10 @@ def update_kf_with_cholesky(xf, Pf, v, Q_cov, Hf):
146150
S = Hf @ PHt + Q_cov
147151

148152
S = (S + S.T) * 0.5
149-
SChol = np.linalg.cholesky(S).T
150-
SCholInv = np.linalg.inv(SChol)
151-
W1 = PHt @ SCholInv
152-
W = W1 @ SCholInv.T
153+
s_chol = np.linalg.cholesky(S).T
154+
s_chol_inv = np.linalg.inv(s_chol)
155+
W1 = PHt @ s_chol_inv
156+
W = W1 @ s_chol_inv.T
153157

154158
x = xf + W @ v
155159
P = Pf - W1 @ W1.T
@@ -187,7 +191,7 @@ def compute_weight(particle, z, Q_cov):
187191
try:
188192
invS = np.linalg.inv(Sf)
189193
except np.linalg.linalg.LinAlgError:
190-
print("singuler")
194+
print("singular")
191195
return 1.0
192196

193197
num = math.exp(-0.5 * dx.T @ invS @ dx)
@@ -201,12 +205,12 @@ def compute_weight(particle, z, Q_cov):
201205
def update_with_observation(particles, z):
202206
for iz in range(len(z[0, :])):
203207

204-
lmid = int(z[2, iz])
208+
landmark_id = int(z[2, iz])
205209

206210
for ip in range(N_PARTICLE):
207211
# new landmark
208-
if abs(particles[ip].lm[lmid, 0]) <= 0.01:
209-
particles[ip] = add_new_lm(particles[ip], z[:, iz], Q)
212+
if abs(particles[ip].lm[landmark_id, 0]) <= 0.01:
213+
particles[ip] = add_new_landmark(particles[ip], z[:, iz], Q)
210214
# known landmark
211215
else:
212216
w = compute_weight(particles[ip], z[:, iz], Q)
@@ -229,28 +233,29 @@ def resampling(particles):
229233

230234
pw = np.array(pw)
231235

232-
Neff = 1.0 / (pw @ pw.T) # Effective particle number
233-
# print(Neff)
236+
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
237+
# print(n_eff)
234238

235-
if Neff < NTH: # resampling
236-
wcum = np.cumsum(pw)
239+
if n_eff < NTH: # resampling
240+
w_cum = np.cumsum(pw)
237241
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
238-
resampleid = base + np.random.rand(base.shape[0]) / N_PARTICLE
242+
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
239243

240244
inds = []
241245
ind = 0
242246
for ip in range(N_PARTICLE):
243-
while (ind < wcum.shape[0] - 1) and (resampleid[ip] > wcum[ind]):
247+
while (ind < w_cum.shape[0] - 1) \
248+
and (resample_id[ip] > w_cum[ind]):
244249
ind += 1
245250
inds.append(ind)
246251

247-
tparticles = particles[:]
252+
tmp_particles = particles[:]
248253
for i in range(len(inds)):
249-
particles[i].x = tparticles[inds[i]].x
250-
particles[i].y = tparticles[inds[i]].y
251-
particles[i].yaw = tparticles[inds[i]].yaw
252-
particles[i].lm = tparticles[inds[i]].lm[:, :]
253-
particles[i].lmP = tparticles[inds[i]].lmP[:, :]
254+
particles[i].x = tmp_particles[inds[i]].x
255+
particles[i].y = tmp_particles[inds[i]].y
256+
particles[i].yaw = tmp_particles[inds[i]].yaw
257+
particles[i].lm = tmp_particles[inds[i]].lm[:, :]
258+
particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
254259
particles[i].w = 1.0 / N_PARTICLE
255260

256261
return particles
@@ -259,37 +264,39 @@ def resampling(particles):
259264
def calc_input(time):
260265
if time <= 3.0: # wait at first
261266
v = 0.0
262-
yawrate = 0.0
267+
yaw_rate = 0.0
263268
else:
264269
v = 1.0 # [m/s]
265-
yawrate = 0.1 # [rad/s]
270+
yaw_rate = 0.1 # [rad/s]
266271

267-
u = np.array([v, yawrate]).reshape(2, 1)
272+
u = np.array([v, yaw_rate]).reshape(2, 1)
268273

269274
return u
270275

271276

272-
def observation(xTrue, xd, u, RFID):
277+
def observation(xTrue, xd, u, rfid):
273278
# calc true state
274279
xTrue = motion_model(xTrue, u)
275280

276281
# add noise to range observation
277282
z = np.zeros((3, 0))
278-
for i in range(len(RFID[:, 0])):
283+
for i in range(len(rfid[:, 0])):
279284

280-
dx = RFID[i, 0] - xTrue[0, 0]
281-
dy = RFID[i, 1] - xTrue[1, 0]
285+
dx = rfid[i, 0] - xTrue[0, 0]
286+
dy = rfid[i, 1] - xTrue[1, 0]
282287
d = math.hypot(dx, dy)
283288
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
284289
if d <= MAX_RANGE:
285-
dn = d + np.random.randn() * Qsim[0, 0] ** 0.5 # add noise
286-
anglen = angle + np.random.randn() * Qsim[1, 1] ** 0.5 # add noise
287-
zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1)
290+
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
291+
angle_with_noize = angle + np.random.randn() * Q_sim[
292+
1, 1] ** 0.5 # add noise
293+
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
288294
z = np.hstack((z, zi))
289295

290296
# add noise to input
291-
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] ** 0.5
292-
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] ** 0.5 + OFFSET_YAWRATE_NOISE
297+
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
298+
ud2 = u[1, 0] + np.random.randn() * R_sim[
299+
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
293300
ud = np.array([ud1, ud2]).reshape(2, 1)
294301

295302
xd = motion_model(xd, ud)
@@ -332,7 +339,7 @@ def main():
332339
[-5.0, 5.0],
333340
[-10.0, 15.0]
334341
])
335-
N_LM = RFID.shape[0]
342+
n_landmark = RFID.shape[0]
336343

337344
# State Vector [x y yaw v]'
338345
xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
@@ -344,7 +351,7 @@ def main():
344351
hxTrue = xTrue
345352
hxDR = xTrue
346353

347-
particles = [Particle(N_LM) for _ in range(N_PARTICLE)]
354+
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
348355

349356
while SIM_TIME >= time:
350357
time += DT
@@ -366,8 +373,9 @@ def main():
366373
if show_animation: # pragma: no cover
367374
plt.cla()
368375
# for stopping simulation with the esc key.
369-
plt.gcf().canvas.mpl_connect('key_release_event',
370-
lambda event: [exit(0) if event.key == 'escape' else None])
376+
plt.gcf().canvas.mpl_connect(
377+
'key_release_event', lambda event:
378+
[exit(0) if event.key == 'escape' else None])
371379
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
372380

373381
for i in range(N_PARTICLE):

‎SLAM/FastSLAM2/fast_slam2.py

Lines changed: 38 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -113,10 +113,14 @@ def add_new_lm(particle, z, Q_cov):
113113
particle.lm[lm_id, 1] = particle.y + r * s
114114

115115
# covariance
116-
Gz = np.array([[c, -r * s],
117-
[s, r * c]])
118-
119-
particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz @ Q_cov @ Gz.T
116+
dx = r * c
117+
dy = r * s
118+
d2 = dx ** 2 + dy ** 2
119+
d = math.sqrt(d2)
120+
Gz = np.array([[dx / d, dy / d],
121+
[-dy / d2, dx / d2]])
122+
particle.lmP[2 * lm_id:2 * lm_id + 2] = np.linalg.inv(
123+
Gz) @ Q_cov @ np.linalg.inv(Gz.T)
120124

121125
return particle
122126

@@ -224,11 +228,11 @@ def proposal_sampling(particle, z, Q_cov):
224228

225229
def update_with_observation(particles, z):
226230
for iz in range(len(z[0, :])):
227-
lmid = int(z[2, iz])
231+
landmark_id = int(z[2, iz])
228232

229233
for ip in range(N_PARTICLE):
230234
# new landmark
231-
if abs(particles[ip].lm[lmid, 0]) <= 0.01:
235+
if abs(particles[ip].lm[landmark_id, 0]) <= 0.01:
232236
particles[ip] = add_new_lm(particles[ip], z[:, iz], Q)
233237
# known landmark
234238
else:
@@ -254,27 +258,28 @@ def resampling(particles):
254258

255259
pw = np.array(pw)
256260

257-
Neff = 1.0 / (pw @ pw.T) # Effective particle number
261+
n_eff = 1.0 / (pw @ pw.T) # Effective particle number
258262

259-
if Neff < NTH: # resampling
260-
wcum = np.cumsum(pw)
263+
if n_eff < NTH: # resampling
264+
w_cum = np.cumsum(pw)
261265
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
262-
resamplei_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
266+
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE
263267

264268
inds = []
265269
ind = 0
266270
for ip in range(N_PARTICLE):
267-
while (ind < wcum.shape[0] - 1) and (resamplei_id[ip] > wcum[ind]):
271+
while (ind < w_cum.shape[0] - 1) \
272+
and (resample_id[ip] > w_cum[ind]):
268273
ind += 1
269274
inds.append(ind)
270275

271-
tparticles = particles[:]
276+
tmp_particles = particles[:]
272277
for i in range(len(inds)):
273-
particles[i].x = tparticles[inds[i]].x
274-
particles[i].y = tparticles[inds[i]].y
275-
particles[i].yaw = tparticles[inds[i]].yaw
276-
particles[i].lm = tparticles[inds[i]].lm[:, :]
277-
particles[i].lmP = tparticles[inds[i]].lmP[:, :]
278+
particles[i].x = tmp_particles[inds[i]].x
279+
particles[i].y = tmp_particles[inds[i]].y
280+
particles[i].yaw = tmp_particles[inds[i]].yaw
281+
particles[i].lm = tmp_particles[inds[i]].lm[:, :]
282+
particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
278283
particles[i].w = 1.0 / N_PARTICLE
279284

280285
return particles
@@ -283,12 +288,12 @@ def resampling(particles):
283288
def calc_input(time):
284289
if time <= 3.0: # wait at first
285290
v = 0.0
286-
yawrate = 0.0
291+
yaw_rate = 0.0
287292
else:
288293
v = 1.0 # [m/s]
289-
yawrate = 0.1 # [rad/s]
294+
yaw_rate = 0.1 # [rad/s]
290295

291-
u = np.array([v, yawrate]).reshape(2, 1)
296+
u = np.array([v, yaw_rate]).reshape(2, 1)
292297

293298
return u
294299

@@ -308,13 +313,15 @@ def observation(xTrue, xd, u, RFID):
308313
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
309314
if d <= MAX_RANGE:
310315
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
311-
anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise
312-
zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1)
316+
angle_noise = np.random.randn() * Q_sim[1, 1] ** 0.5
317+
angle_with_noise = angle + angle_noise # add noise
318+
zi = np.array([dn, pi_2_pi(angle_with_noise), i]).reshape(3, 1)
313319
z = np.hstack((z, zi))
314320

315321
# add noise to input
316322
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
317-
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
323+
ud2 = u[1, 0] + np.random.randn() * R_sim[
324+
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
318325
ud = np.array([ud1, ud2]).reshape(2, 1)
319326

320327
xd = motion_model(xd, ud)
@@ -357,7 +364,7 @@ def main():
357364
[-5.0, 5.0],
358365
[-10.0, 15.0]
359366
])
360-
N_LM = RFID.shape[0]
367+
n_landmark = RFID.shape[0]
361368

362369
# State Vector [x y yaw v]'
363370
xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
@@ -369,7 +376,7 @@ def main():
369376
hxTrue = xTrue
370377
hxDR = xTrue
371378

372-
particles = [Particle(N_LM) for _ in range(N_PARTICLE)]
379+
particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]
373380

374381
while SIM_TIME >= time:
375382
time += DT
@@ -391,14 +398,15 @@ def main():
391398
if show_animation: # pragma: no cover
392399
plt.cla()
393400
# for stopping simulation with the esc key.
394-
plt.gcf().canvas.mpl_connect('key_release_event',
395-
lambda event: [exit(0) if event.key == 'escape' else None])
401+
plt.gcf().canvas.mpl_connect(
402+
'key_release_event',
403+
lambda event: [exit(0) if event.key == 'escape' else None])
396404
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
397405

398406
for iz in range(len(z[:, 0])):
399-
lmid = int(z[2, iz])
400-
plt.plot([xEst[0], RFID[lmid, 0]], [
401-
xEst[1], RFID[lmid, 1]], "-k")
407+
landmark_id = int(z[2, iz])
408+
plt.plot([xEst[0], RFID[landmark_id, 0]], [
409+
xEst[1], RFID[landmark_id, 1]], "-k")
402410

403411
for i in range(N_PARTICLE):
404412
plt.plot(particles[i].x, particles[i].y, ".r")

0 commit comments

Comments
 (0)