Skip to content

Commit

Permalink
clean up fast slam codes (#1012)
Browse files Browse the repository at this point in the history 
* clean up fast slam codes

* clean up fast slam codes

* clean up fast slam codes
  • Loading branch information
@AtsushiSakai
AtsushiSakai committed May 4, 2024
1 parent c9ae446 commit 16c16b6
Show file tree
Hide file tree
Showing 2 changed files with 104 additions and 106 deletions.
100 changes: 49 additions & 51 deletions SLAM/FastSLAM1/fast_slam1.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
R = np.diag([1.0, np.deg2rad(20.0)]) ** 2

# Simulation parameter
Q_sim = np.diag([0.3, np.deg2rad(2.0)]) ** 2
R_sim = np.diag([0.5, np.deg2rad(10.0)]) ** 2
Q_SIM = np.diag([0.3, np.deg2rad(2.0)]) ** 2
R_SIM = np.diag([0.5, np.deg2rad(10.0)]) ** 2
OFFSET_YAW_RATE_NOISE = 0.01

DT = 0.1 # time tick [s]
Expand Down Expand Up @@ -72,19 +72,18 @@ def normalize_weight(particles):


def calc_final_state(particles):
xEst = np.zeros((STATE_SIZE, 1))
x_est = np.zeros((STATE_SIZE, 1))

particles = normalize_weight(particles)

for i in range(N_PARTICLE):
xEst[0, 0] += particles[i].w * particles[i].x
xEst[1, 0] += particles[i].w * particles[i].y
xEst[2, 0] += particles[i].w * particles[i].yaw
x_est[0, 0] += particles[i].w * particles[i].x
x_est[1, 0] += particles[i].w * particles[i].y
x_est[2, 0] += particles[i].w * particles[i].yaw

xEst[2, 0] = pi_2_pi(xEst[2, 0])
# print(xEst)
x_est[2, 0] = pi_2_pi(x_est[2, 0])

return xEst
return x_est


def predict_particles(particles, u):
Expand Down Expand Up @@ -235,28 +234,27 @@ def resampling(particles):
pw = np.array(pw)

n_eff = 1.0 / (pw @ pw.T) # Effective particle number
# print(n_eff)

if n_eff < NTH: # resampling
w_cum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resample_id = base + np.random.rand(base.shape[0]) / N_PARTICLE

inds = []
ind = 0
indexes = []
index = 0
for ip in range(N_PARTICLE):
while (ind < w_cum.shape[0] - 1) \
and (resample_id[ip] > w_cum[ind]):
ind += 1
inds.append(ind)
while (index < w_cum.shape[0] - 1) \
and (resample_id[ip] > w_cum[index]):
index += 1
indexes.append(index)

tmp_particles = particles[:]
for i in range(len(inds)):
particles[i].x = tmp_particles[inds[i]].x
particles[i].y = tmp_particles[inds[i]].y
particles[i].yaw = tmp_particles[inds[i]].yaw
particles[i].lm = tmp_particles[inds[i]].lm[:, :]
particles[i].lmP = tmp_particles[inds[i]].lmP[:, :]
for i in range(len(indexes)):
particles[i].x = tmp_particles[indexes[i]].x
particles[i].y = tmp_particles[indexes[i]].y
particles[i].yaw = tmp_particles[indexes[i]].yaw
particles[i].lm = tmp_particles[indexes[i]].lm[:, :]
particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :]
particles[i].w = 1.0 / N_PARTICLE

return particles
Expand All @@ -275,34 +273,34 @@ def calc_input(time):
return u


def observation(xTrue, xd, u, rfid):
def observation(x_true, xd, u, rfid):
# calc true state
xTrue = motion_model(xTrue, u)
x_true = motion_model(x_true, u)

# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(rfid[:, 0])):

dx = rfid[i, 0] - xTrue[0, 0]
dy = rfid[i, 1] - xTrue[1, 0]
dx = rfid[i, 0] - x_true[0, 0]
dy = rfid[i, 1] - x_true[1, 0]
d = math.hypot(dx, dy)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
angle = pi_2_pi(math.atan2(dy, dx) - x_true[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
angle_with_noize = angle + np.random.randn() * Q_sim[
dn = d + np.random.randn() * Q_SIM[0, 0] ** 0.5 # add noise
angle_with_noize = angle + np.random.randn() * Q_SIM[
1, 1] ** 0.5 # add noise
zi = np.array([dn, pi_2_pi(angle_with_noize), i]).reshape(3, 1)
z = np.hstack((z, zi))

# add noise to input
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_sim[
ud1 = u[0, 0] + np.random.randn() * R_SIM[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_SIM[
1, 1] ** 0.5 + OFFSET_YAW_RATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)

xd = motion_model(xd, ud)

return xTrue, z, xd, ud
return x_true, z, xd, ud


def motion_model(x, u):
Expand Down Expand Up @@ -331,7 +329,7 @@ def main():
time = 0.0

# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
rfid = np.array([[10.0, -2.0],
[15.0, 10.0],
[15.0, 15.0],
[10.0, 20.0],
Expand All @@ -340,53 +338,53 @@ def main():
[-5.0, 5.0],
[-10.0, 15.0]
])
n_landmark = RFID.shape[0]
n_landmark = rfid.shape[0]

# State Vector [x y yaw v]'
xEst = np.zeros((STATE_SIZE, 1)) # SLAM estimation
xTrue = np.zeros((STATE_SIZE, 1)) # True state
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
x_est = np.zeros((STATE_SIZE, 1)) # SLAM estimation
x_true = np.zeros((STATE_SIZE, 1)) # True state
x_dr = np.zeros((STATE_SIZE, 1)) # Dead reckoning

# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
hist_x_est = x_est
hist_x_true = x_true
hist_x_dr = x_dr

particles = [Particle(n_landmark) for _ in range(N_PARTICLE)]

while SIM_TIME >= time:
time += DT
u = calc_input(time)

xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
x_true, z, x_dr, ud = observation(x_true, x_dr, u, rfid)

particles = fast_slam1(particles, ud, z)

xEst = calc_final_state(particles)
x_est = calc_final_state(particles)

x_state = xEst[0: STATE_SIZE]
x_state = x_est[0: STATE_SIZE]

# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
hist_x_est = np.hstack((hist_x_est, x_state))
hist_x_dr = np.hstack((hist_x_dr, x_dr))
hist_x_true = np.hstack((hist_x_true, x_true))

if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event', lambda event:
[exit(0) if event.key == 'escape' else None])
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(rfid[:, 0], rfid[:, 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")

plt.plot(hxTrue[0, :], hxTrue[1, :], "-b")
plt.plot(hxDR[0, :], hxDR[1, :], "-k")
plt.plot(hxEst[0, :], hxEst[1, :], "-r")
plt.plot(xEst[0], xEst[1], "xk")
plt.plot(hist_x_true[0, :], hist_x_true[1, :], "-b")
plt.plot(hist_x_dr[0, :], hist_x_dr[1, :], "-k")
plt.plot(hist_x_est[0, :], hist_x_est[1, :], "-r")
plt.plot(x_est[0], x_est[1], "xk")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
Expand Down
Loading

0 comments on commit 16c16b6

Please sign in to comment.