start graph_slam implementation

SLAM/GraphSLAM/graph_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 ekf_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 = ekf_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()