""" Graph SLAM example author: Atsushi Sakai (@Atsushi_twi) """ import numpy as np import math import copy import itertools import matplotlib.pyplot as plt # Simulation parameter Qsim = np.diag([0.2, math.radians(1.0)])**2 Rsim = np.diag([1.0, math.radians(10.0)])**2 DT = 1.0 # time tick [s] SIM_TIME = 50.0 # simulation time [s] MAX_RANGE = 20.0 # maximum observation range STATE_SIZE = 3 # State size [x,y,yaw] # Covariance parameter of Graph Based SLAM C_SIGMA1 = 1.0 C_SIGMA2 = 0.1 C_SIGMA3 = 0.1 MAX_ITR = 20 # Maximuma iteration show_animation = True class Edge(): def __init__(self): self.e = np.zeros((3, 1)) self.omega = np.zeros((3, 3)) # information matrix self.d_t = 0.0 self.d_td = 0.0 self.yaw_t = 0.0 self.yaw_td = 0.0 self.angle_t = 0.0 self.angle_td = 0.0 self.id1 = 0 self.id2 = 0 def cal_observation_sigma(d): sigma = np.zeros((3, 3)) sigma[0, 0] = (d * C_SIGMA1)**2 sigma[1, 1] = (d * C_SIGMA2)**2 sigma[2, 2] = C_SIGMA3**2 return sigma def calc_rotational_matrix(angle): Rt = np.matrix([[math.cos(angle), -math.sin(angle), 0], [math.sin(angle), math.cos(angle), 0], [0, 0, 1.0]]) return Rt def calc_edge(xt, yt, yawt, xtd, ytd, yawtd, dt, anglet, phit, dtd, angletd, phitd, t, td): edge = Edge() tangle1 = pi_2_pi(yawt + anglet) tangle2 = pi_2_pi(yawt + anglet) t1 = dt * math.cos(tangle1) t2 = dtd * math.cos(tangle2) t3 = dt * math.sin(tangle1) t4 = dtd * math.sin(tangle2) edge.e[0, 0] = xtd - xt - t1 + t2 edge.e[1, 0] = ytd - yt - t3 + t4 edge.e[2, 0] = pi_2_pi(yawtd - yawt - phit + phitd) sig_t = cal_observation_sigma(dt) sig_td = cal_observation_sigma(dtd) Rt = calc_rotational_matrix(tangle1) Rtd = calc_rotational_matrix(tangle2) edge.omega = np.linalg.inv(Rt * sig_t * Rt.T + Rtd * sig_td * Rtd.T) edge.d_t, edge.d_td = dt, dtd edge.yaw_t, edge.yaw_td = yawt, yawtd edge.angle_t, edge.angle_td = anglet, angletd edge.id1, edge.id2 = t, td return edge def calc_edges(xlist, zlist): edges = [] zids = list(itertools.combinations(range(len(zlist)), 2)) for (t, td) in zids: # print(xlist) # print(zlist) xt, yt, yawt = xlist[0, t], xlist[1, t], xlist[2, t] xtd, ytd, yawtd = xlist[0, td], xlist[1, td], xlist[2, td] # print(zlist[t]) # print(zlist[td]) for iz1 in range(len(zlist[t][:, 0])): for iz2 in range(len(zlist[td][:, 0])): if zlist[t][iz1, 3] == zlist[td][iz2, 3]: dt, anglet, phit = zlist[t][iz1, 0], zlist[t][iz1, 1], zlist[t][iz1, 2] dtd, angletd, phitd = zlist[td][iz2, 0], zlist[td][iz2, 1], zlist[td][iz2, 2] edge = calc_edge(xt, yt, yawt, xtd, ytd, yawtd, dt, anglet, phit, dtd, angletd, phitd, t, td) edges.append(edge) break return edges def calc_jacobian(edge): t = edge.yaw_t + edge.angle_t A = np.matrix([[-1.0, 0, edge.d_t * math.sin(t)], [0, -1.0, -edge.d_t * math.cos(t)], [0, 0, -1.0]]) td = edge.yaw_td + edge.angle_td B = np.matrix([[1.0, 0, -edge.d_td * math.sin(td)], [0, 1.0, edge.d_td * math.cos(td)], [0, 0, 1.0]]) return A, B def fill_H_and_b(H, b, edge): A, B = calc_jacobian(edge) id1 = edge.id1 * 3 id2 = edge.id2 * 3 H[id1:id1 + 3, id1:id1 + 3] += A.T * edge.omega * A H[id1:id1 + 3, id2:id2 + 3] += A.T * edge.omega * B H[id2:id2 + 3, id1:id1 + 3] += B.T * edge.omega * A H[id2:id2 + 3, id2:id2 + 3] += B.T * edge.omega * B b[id1:id1 + 3, 0] += (A.T * edge.omega * edge.e) b[id2:id2 + 3, 0] += (B.T * edge.omega * edge.e) return H, b def graph_based_slam(u, z, hxDR, hz): print("start graph based slam") x_opt = copy.deepcopy(hxDR) n = len(hz) * 3 # return x_opt for itr in range(MAX_ITR): edges = calc_edges(x_opt, hz) # print("n edges:", len(edges)) H = np.matrix(np.zeros((n, n))) b = np.matrix(np.zeros((n, 1))) for edge in edges: H, b = fill_H_and_b(H, b, edge) H[0:3, 0:3] += np.identity(3) * 10000 # to fix origin dx = - np.linalg.inv(H).dot(b) # print(dx) for i in range(len(hz)): x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0] diff = dx.T.dot(dx) print("iteration: %d, diff: %f" % (itr + 1, diff)) if diff < 1.0e-5: break # print(x_opt) return x_opt 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, 4))) 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)) - xTrue[2, 0] phi = 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, phi, 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 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, yaw] RFID = np.array([[10.0, -2.0, 0.0], [15.0, 10.0, 0.0], [3.0, 15.0, 0.0], [-5.0, 20.0, 0.0]]) # State Vector [x y yaw v]' xTrue = np.matrix(np.zeros((STATE_SIZE, 1))) xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning # history hxTrue = xTrue hxDR = xTrue hz = [] while SIM_TIME >= time: time += DT u = calc_input() xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) hxDR = np.hstack((hxDR, xDR)) hz.append(z) # store data history hxTrue = np.hstack((hxTrue, xTrue)) x_opt = graph_based_slam(ud, z, hxDR, hz) if show_animation: plt.cla() plt.plot(RFID[:, 0], RFID[:, 1], "*k") 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(x_opt[0, :]).flatten(), np.array(x_opt[1, :]).flatten(), "-r") plt.axis("equal") plt.grid(True) plt.show() if __name__ == '__main__': main()