From 0bd52c34fb0a19548a4168debe2b2a7acc4ce3d3 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 7 Mar 2018 10:24:43 -0800 Subject: [PATCH] start implementing fast slam --- SLAM/FastSLAM/fast_slam.py | 204 +++++++++++++++++++++++++++++++++++++ 1 file changed, 204 insertions(+) create mode 100644 SLAM/FastSLAM/fast_slam.py diff --git a/SLAM/FastSLAM/fast_slam.py b/SLAM/FastSLAM/fast_slam.py new file mode 100644 index 00000000..6e8286cd --- /dev/null +++ b/SLAM/FastSLAM/fast_slam.py @@ -0,0 +1,204 @@ +""" + +Fast SLAM example + +author: Atsushi Sakai (@Atsushi_twi) + +""" + +import numpy as np +import math +import matplotlib.pyplot as plt + + +# EKF state covariance +Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2 + +# Simulation parameter +Qsim = np.diag([0.2, math.radians(1.0)])**2 +Rsim = np.diag([1.0, math.radians(10.0)])**2 + +DT = 0.1 # time tick [s] +SIM_TIME = 50.0 # simulation time [s] +MAX_RANGE = 20.0 # maximum observation range +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] + +show_animation = True + + +def fast_slam(xEst, PEst, u, z): + + xEst[2] = pi_2_pi(xEst[2]) + + return xEst, PEst + + +def calc_input(): + v = 1.0 # [m/s] + yawrate = 0.1 # [rad/s] + u = np.matrix([v, yawrate]).T + return u + + +def observation(xTrue, xd, u, RFID): + + xTrue = motion_model(xTrue, u) + + # add noise to gps x-y + z = np.matrix(np.zeros((0, 3))) + + for i in range(len(RFID[:, 0])): + + 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)) + if d <= MAX_RANGE: + dn = d + np.random.randn() * Qsim[0, 0] # add noise + anglen = angle + np.random.randn() * Qsim[1, 1] # add noise + zi = np.matrix([dn, anglen, i]) + z = np.vstack((z, zi)) + + # add noise to input + ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] + ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + ud = np.matrix([ud1, ud2]).T + + xd = motion_model(xd, ud) + + return xTrue, z, xd, ud + + +def motion_model(x, u): + + F = np.matrix([[1.0, 0, 0], + [0, 1.0, 0], + [0, 0, 1.0]]) + + B = np.matrix([[DT * math.cos(x[2, 0]), 0], + [DT * math.sin(x[2, 0]), 0], + [0.0, DT]]) + + x = F * x + B * u + + return x + + +def calc_n_LM(x): + n = int((len(x) - STATE_SIZE) / LM_SIZE) + return n + + +def calc_LM_Pos(x, z): + + zp = np.zeros((2, 1)) + + zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1]) + zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1]) + + return zp + + +def get_LM_Pos_from_state(x, ind): + + lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :] + + return lm + + +def search_correspond_LM_ID(xAug, PAug, zi): + """ + Landmark association with Mahalanobis distance + """ + + nLM = calc_n_LM(xAug) + + mdist = [] + + for i in range(nLM): + # lm = get_LM_Pos_from_state(xAug, i) + # # y, S, H = calc_innovation(lm, xAug, PAug, zi, i) + # mdist.append(y.T * np.linalg.inv(S) * y) + pass + + mdist.append(M_DIST_TH) # new landmark + + minid = mdist.index(min(mdist)) + + return minid + + +def pi_2_pi(angle): + while(angle > math.pi): + angle = angle - 2.0 * math.pi + + while(angle < -math.pi): + angle = angle + 2.0 * math.pi + + return angle + + +def main(): + print(__file__ + " start!!") + + time = 0.0 + + # RFID positions [x, y] + RFID = np.array([[10.0, -2.0], + [15.0, 10.0], + [3.0, 15.0], + [-5.0, 20.0]]) + + # State Vector [x y yaw v]' + xEst = np.matrix(np.zeros((STATE_SIZE, 1))) + xTrue = np.matrix(np.zeros((STATE_SIZE, 1))) + PEst = np.eye(STATE_SIZE) + + xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning + + # history + hxEst = xEst + hxTrue = xTrue + hxDR = xTrue + + while SIM_TIME >= time: + time += DT + u = calc_input() + + xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) + + xEst, PEst = fast_slam(xEst, PEst, ud, z) + + x_state = xEst[0:STATE_SIZE] + + # store data history + hxEst = np.hstack((hxEst, x_state)) + hxDR = np.hstack((hxDR, xDR)) + hxTrue = np.hstack((hxTrue, xTrue)) + + if show_animation: + plt.cla() + + plt.plot(RFID[:, 0], RFID[:, 1], "*k") + plt.plot(xEst[0], xEst[1], ".r") + + # plot landmark + for i in range(calc_n_LM(xEst)): + plt.plot(xEst[STATE_SIZE + i * 2], + xEst[STATE_SIZE + i * 2 + 1], "xg") + + plt.plot(np.array(hxTrue[0, :]).flatten(), + np.array(hxTrue[1, :]).flatten(), "-b") + plt.plot(np.array(hxDR[0, :]).flatten(), + np.array(hxDR[1, :]).flatten(), "-k") + plt.plot(np.array(hxEst[0, :]).flatten(), + np.array(hxEst[1, :]).flatten(), "-r") + plt.axis("equal") + plt.grid(True) + plt.pause(0.001) + + +if __name__ == '__main__': + main()