diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py new file mode 100644 index 00000000..38f46028 --- /dev/null +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -0,0 +1,276 @@ +""" + +Graph 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 graph_based_slam(xEst, PEst, u, z): + + # Predict + S = STATE_SIZE + xEst[0:S] = motion_model(xEst[0:S], u) + G, Fx = jacob_motion(xEst[0:S], u) + PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx + initP = np.eye(2) + + # Update + for iz in range(len(z[:, 0])): # for each observation + minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) + + nLM = calc_n_LM(xEst) + if minid == nLM: + print("New LM") + # Extend state and covariance matrix + xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :]))) + PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))), + np.hstack((np.zeros((LM_SIZE, len(xEst))), initP)))) + xEst = xAug + PEst = PAug + + lm = get_LM_Pos_from_state(xEst, minid) + y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid) + + K = PEst * H.T * np.linalg.inv(S) + xEst = xEst + K * y + PEst = (np.eye(len(xEst)) - K * H) * PEst + + 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 jacob_motion(x, u): + + Fx = np.hstack((np.eye(STATE_SIZE), np.zeros( + (STATE_SIZE, LM_SIZE * calc_n_LM(x))))) + + jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])], + [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])], + [0.0, 0.0, 0.0]]) + + G = np.eye(STATE_SIZE) + Fx.T * jF * Fx + + return G, Fx, + + +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) + + mdist.append(M_DIST_TH) # new landmark + + minid = mdist.index(min(mdist)) + + return minid + + +def calc_innovation(lm, xEst, PEst, z, LMid): + delta = lm - xEst[0:2] + q = (delta.T * delta)[0, 0] + zangle = math.atan2(delta[1], delta[0]) - xEst[2] + zp = [math.sqrt(q), pi_2_pi(zangle)] + y = (z - zp).T + y[1] = pi_2_pi(y[1]) + H = jacobH(q, delta, xEst, LMid + 1) + S = H * PEst * H.T + Cx[0:2, 0:2] + + return y, S, H + + +def jacobH(q, delta, x, i): + sq = math.sqrt(q) + G = np.matrix([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]], + [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]]) + + G = G / q + nLM = calc_n_LM(x) + F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM)))) + F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))), + np.eye(2), np.zeros((2, 2 * nLM - 2 * i)))) + + F = np.vstack((F1, F2)) + + H = G * F + + return H + + +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 = graph_based_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()