Skip to content

Commit 1b1cc1d

Browse files
AtsushiSakaiAtsushiSakai
committed
add fast slam 1 test
1 parent b50f9dc commit 1b1cc1d

File tree

2 files changed

+50
-73
lines changed

2 files changed

+50
-73
lines changed

SLAM/FastSLAM/fast_slam.py renamed to SLAM/FastSLAM1/fast_slam1.py

Lines changed: 38 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -12,25 +12,20 @@
1212

1313

1414
# EKF state covariance
15-
Cx = np.matrix([[0.1, 0.0, 0.1],
16-
[0.0, math.radians(1.0), 0.1],
17-
[0.1, 0.0, math.radians(30.0)]])
18-
19-
R = np.diag([1.0, math.radians(10.0)])**2
15+
Q = np.diag([1.0, math.radians(4.0)])**2
16+
R = np.diag([0.5, math.radians(15.0)])**2
2017

2118
# Simulation parameter
22-
Qsim = np.diag([0.0, math.radians(0.0)])**2
23-
Rsim = np.diag([1.0, math.radians(10.0)])**2
19+
Qsim = np.diag([0.3, math.radians(2.0)])**2
20+
Rsim = np.diag([0.5, math.radians(10.0)])**2
2421

2522
DT = 0.1 # time tick [s]
2623
SIM_TIME = 50.0 # simulation time [s]
2724
MAX_RANGE = 20.0 # maximum observation range
2825
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
2926
STATE_SIZE = 3 # State size [x,y,yaw]
3027
LM_SIZE = 2 # LM srate size [x,y]
31-
3228
N_PARTICLE = 100 # number of particle
33-
3429
NTH = N_PARTICLE / 2.0 # Number of particle for re-sampling
3530

3631
show_animation = True
@@ -47,6 +42,19 @@ def __init__(self, N_LM):
4742
self.lmP = np.matrix(np.zeros((N_LM * 2, 2)))
4843

4944

45+
def fast_slam(particles, PEst, u, z):
46+
47+
particles = predict_particles(particles, u)
48+
49+
particles = update_with_observation(particles, z)
50+
51+
particles = resampling(particles)
52+
53+
xEst = calc_final_state(particles)
54+
55+
return xEst, PEst, particles
56+
57+
5058
def normalize_weight(particles):
5159

5260
sumw = sum([p.w for p in particles])
@@ -87,7 +95,7 @@ def predict_particles(particles, u):
8795
px[0, 0] = particles[i].x
8896
px[1, 0] = particles[i].y
8997
px[2, 0] = particles[i].yaw
90-
ud = u + (np.matrix(np.random.randn(1, 2)) * Rsim).T # add noise
98+
ud = u + (np.matrix(np.random.randn(1, 2)) * R).T # add noise
9199
px = motion_model(px, ud)
92100
particles[i].x = px[0, 0]
93101
particles[i].y = px[1, 0]
@@ -96,7 +104,7 @@ def predict_particles(particles, u):
96104
return particles
97105

98106

99-
def add_new_lm(particle, z):
107+
def add_new_lm(particle, z, Q):
100108

101109
r = z[0, 0]
102110
b = z[0, 1]
@@ -112,12 +120,12 @@ def add_new_lm(particle, z):
112120
Gz = np.matrix([[c, -r * s],
113121
[s, r * c]])
114122

115-
particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz * Cx[0: 2, 0: 2] * Gz.T
123+
particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz * Q * Gz.T
116124

117125
return particle
118126

119127

120-
def compute_jacobians(particle, xf, Pf, R):
128+
def compute_jacobians(particle, xf, Pf, Q):
121129
dx = xf[0, 0] - particle.x
122130
dy = xf[1, 0] - particle.y
123131
d2 = dx**2 + dy**2
@@ -131,14 +139,14 @@ def compute_jacobians(particle, xf, Pf, R):
131139
Hf = np.matrix([[dx / d, dy / d],
132140
[-dy / d2, dx / d2]])
133141

134-
Sf = Hf * Pf * Hf.T + R
142+
Sf = Hf * Pf * Hf.T + Q
135143

136144
return zp, Hv, Hf, Sf
137145

138146

139-
def update_KF_with_cholesky(xf, Pf, v, R, Hf):
147+
def update_KF_with_cholesky(xf, Pf, v, Q, Hf):
140148
PHt = Pf * Hf.T
141-
S = Hf * PHt + R
149+
S = Hf * PHt + Q
142150

143151
S = (S + S.T) * 0.5
144152
SChol = np.linalg.cholesky(S).T
@@ -152,31 +160,31 @@ def update_KF_with_cholesky(xf, Pf, v, R, Hf):
152160
return x, P
153161

154162

155-
def update_landmark(particle, z, R):
163+
def update_landmark(particle, z, Q):
156164

157165
lm_id = int(z[0, 2])
158166
xf = np.matrix(particle.lm[lm_id, :]).T
159167
Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2, :])
160168

161-
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, R)
169+
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
162170

163171
dz = z[0, 0: 2].T - zp
164172
dz[1, 0] = pi_2_pi(dz[1, 0])
165173

166-
xf, Pf = update_KF_with_cholesky(xf, Pf, dz, R, Hf)
174+
xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf)
167175

168176
particle.lm[lm_id, :] = xf.T
169177
particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf
170178

171179
return particle
172180

173181

174-
def compute_weight(particle, z, R):
182+
def compute_weight(particle, z, Q):
175183

176184
lm_id = int(z[0, 2])
177185
xf = np.matrix(particle.lm[lm_id, :]).T
178186
Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2])
179-
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, R)
187+
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
180188

181189
dx = z[0, 0: 2].T - zp
182190
dx[1, 0] = pi_2_pi(dx[1, 0])
@@ -204,13 +212,12 @@ def update_with_observation(particles, z):
204212
for ip in range(N_PARTICLE):
205213
# new landmark
206214
if abs(particles[ip].lm[lmid, 0]) <= 0.01:
207-
particles[ip] = add_new_lm(particles[ip], z[iz, :])
215+
particles[ip] = add_new_lm(particles[ip], z[iz, :], Q)
208216
# known landmark
209217
else:
210-
# w = p(z_k | x_k)
211-
w = compute_weight(particles[ip], z[iz, :], Cx[0: 2, 0: 2])
218+
w = compute_weight(particles[ip], z[iz, :], Q)
212219
particles[ip].w = particles[ip].w * w
213-
particles[ip] = update_landmark(particles[ip], z[iz, :], R)
220+
particles[ip] = update_landmark(particles[ip], z[iz, :], Q)
214221

215222
return particles
216223

@@ -229,9 +236,10 @@ def resampling(particles):
229236
pw = np.matrix(pw)
230237

231238
Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number
239+
# print(Neff)
232240

233241
if Neff < NTH: # resampling
234-
# print("resamping")
242+
# print("resampling")
235243
wcum = np.cumsum(pw)
236244
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
237245
resampleid = base + np.random.rand(base.shape[1]) / N_PARTICLE
@@ -255,23 +263,6 @@ def resampling(particles):
255263
return particles
256264

257265

258-
def fast_slam(particles, PEst, u, z):
259-
260-
# Predict
261-
particles = predict_particles(particles, u)
262-
263-
# Observation
264-
particles = update_with_observation(particles, z)
265-
266-
particles = normalize_weight(particles)
267-
268-
particles = resampling(particles)
269-
270-
xEst = calc_final_state(particles)
271-
272-
return xEst, PEst, particles
273-
274-
275266
def calc_input():
276267
v = 1.0 # [m/s]
277268
yawrate = 0.1 # [rad/s]
@@ -300,7 +291,7 @@ def observation(xTrue, xd, u, RFID):
300291

301292
# add noise to input
302293
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
303-
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
294+
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + 0.01
304295
ud = np.matrix([ud1, ud2]).T
305296

306297
xd = motion_model(xd, ud)
@@ -325,28 +316,6 @@ def motion_model(x, u):
325316
return x
326317

327318

328-
def calc_n_LM(x):
329-
n = int((len(x) - STATE_SIZE) / LM_SIZE)
330-
return n
331-
332-
333-
def calc_LM_Pos(x, z):
334-
335-
zp = np.zeros((2, 1))
336-
337-
zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
338-
zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
339-
340-
return zp
341-
342-
343-
def get_LM_Pos_from_state(x, ind):
344-
345-
lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
346-
347-
return lm
348-
349-
350319
def pi_2_pi(angle):
351320
while(angle > math.pi):
352321
angle = angle - 2.0 * math.pi
@@ -366,7 +335,9 @@ def main():
366335
RFID = np.array([[10.0, -2.0],
367336
[15.0, 10.0],
368337
[3.0, 15.0],
369-
[-5.0, 20.0]])
338+
[-5.0, 20.0],
339+
[-5.0, 5.0]
340+
])
370341
N_LM = RFID.shape[0]
371342

372343
# State Vector [x y yaw v]'
@@ -400,18 +371,12 @@ def main():
400371

401372
if show_animation:
402373
plt.cla()
403-
404374
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
405375

406376
for i in range(N_PARTICLE):
407377
plt.plot(particles[i].x, particles[i].y, ".r")
408378
plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
409379

410-
# plot landmark
411-
for i in range(calc_n_LM(xEst)):
412-
plt.plot(xEst[STATE_SIZE + i * 2],
413-
xEst[STATE_SIZE + i * 2 + 1], "xg")
414-
415380
plt.plot(np.array(hxTrue[0, :]).flatten(),
416381
np.array(hxTrue[1, :]).flatten(), "-b")
417382
plt.plot(np.array(hxDR[0, :]).flatten(),

‎tests/test_fast_slam1.py‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
from unittest import TestCase
2+
3+
from SLAM.FastSLAM1 import fast_slam1 as m
4+
5+
print(__file__)
6+
7+
8+
class Test(TestCase):
9+
10+
def test1(self):
11+
m.show_animation = False
12+
m.main()

0 commit comments

Comments
 (0)