diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index ca967da9..d69a2f83 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -1,9 +1,6 @@ """ - Extended Kalman Filter SLAM example - author: Atsushi Sakai (@Atsushi_twi) - """ import numpy as np @@ -50,13 +47,12 @@ def ekf_slam(xEst, PEst, u, z): 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 + K = PEst.dot(H.T).dot(np.linalg.inv(S)) + xEst = xEst + K.dot(y) + PEst = (np.eye(len(xEst)) - K.dot(H)).dot(PEst) xEst[2] = pi_2_pi(xEst[2]) @@ -66,7 +62,7 @@ def ekf_slam(xEst, PEst, u, z): def calc_input(): v = 1.0 # [m/s] yawrate = 0.1 # [rad/s] - u = np.matrix([v, yawrate]).T + u = np.array([[v, yawrate]]).T return u @@ -75,7 +71,7 @@ def observation(xTrue, xd, u, RFID): xTrue = motion_model(xTrue, u) # add noise to gps x-y - z = np.matrix(np.zeros((0, 3))) + z = np.zeros((0, 3)) for i in range(len(RFID[:, 0])): @@ -86,31 +82,29 @@ def observation(xTrue, xd, u, RFID): 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]) + zi = np.array([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 + ud = np.array([[ + u[0, 0] + np.random.randn() * Rsim[0, 0], + u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T xd = motion_model(xd, ud) - return xTrue, z, xd, ud def motion_model(x, u): - F = np.matrix([[1.0, 0, 0], + F = np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) - B = np.matrix([[DT * math.cos(x[2, 0]), 0], + B = np.array([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) - x = F * x + B * u - + x = F.dot(x) + B .dot(u) return x @@ -124,7 +118,7 @@ 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])], + jF = np.array([[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]]) @@ -134,11 +128,12 @@ def jacob_motion(x, u): 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]) + zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1]) + zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[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 @@ -162,7 +157,7 @@ def search_correspond_LM_ID(xAug, PAug, zi): 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(y.T.dot(np.linalg.inv(S)).dot(y)) mdist.append(M_DIST_TH) # new landmark @@ -173,20 +168,21 @@ def search_correspond_LM_ID(xAug, PAug, zi): 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)] + q = (delta.T.dot(delta))[0, 0] + #zangle = math.atan2(delta[1], delta[0]) - xEst[2] + zangle = math.atan2(delta[1,0], delta[0,0]) - xEst[2] + zp = np.array([[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] + S = H.dot(PEst).dot(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]], + G = np.array([[-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 @@ -197,7 +193,7 @@ def jacobH(q, delta, x, i): F = np.vstack((F1, F2)) - H = G * F + H = G.dot(F) return H @@ -218,11 +214,11 @@ def main(): [-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))) + xEst = np.zeros((STATE_SIZE, 1)) + xTrue = np.zeros((STATE_SIZE, 1)) PEst = np.eye(STATE_SIZE) - xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning + xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning # history hxEst = xEst @@ -239,6 +235,7 @@ def main(): x_state = xEst[0:STATE_SIZE] + # store data history hxEst = np.hstack((hxEst, x_state)) hxDR = np.hstack((hxDR, xDR)) @@ -255,16 +252,17 @@ def main(): 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.plot(hxTrue[0, :], + hxTrue[1, :], "-b") + plt.plot(hxDR[0, :], + hxDR[1, :], "-k") + plt.plot(hxEst[0, :], + hxEst[1, :], "-r") plt.axis("equal") plt.grid(True) plt.pause(0.001) + if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 6d301aa6..daaeab2c 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -1,9 +1,6 @@ """ - Iterative Closest Point (ICP) SLAM example - author: Atsushi Sakai (@Atsushi_twi) - """ import math @@ -20,15 +17,12 @@ show_animation = True def ICP_matching(ppoints, cpoints): """ Iterative Closest Point matching - - input ppoints: 2D points in the previous frame cpoints: 2D points in the current frame - - output R: Rotation matrix T: Translation vector - """ H = None # homogeneraous transformation matrix @@ -51,7 +45,7 @@ def ICP_matching(ppoints, cpoints): Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) # update current points - cpoints = (Rt * cpoints) + Tt + cpoints = (Rt.dot(cpoints)) + Tt[:,np.newaxis] H = update_homogenerous_matrix(H, Rt, Tt) @@ -66,15 +60,15 @@ def ICP_matching(ppoints, cpoints): print("Not Converge...", error, dError, count) break - R = np.matrix(H[0:2, 0:2]) - T = np.matrix(H[0:2, 2]) + R = np.array(H[0:2, 0:2]) + T = np.array(H[0:2, 2]) return R, T def update_homogenerous_matrix(Hin, R, T): - H = np.matrix(np.zeros((3, 3))) + H = np.zeros((3, 3)) H[0, 0] = R[0, 0] H[1, 0] = R[1, 0] @@ -82,8 +76,8 @@ def update_homogenerous_matrix(Hin, R, T): H[1, 1] = R[1, 1] H[2, 2] = 1.0 - H[0, 2] = T[0, 0] - H[1, 2] = T[1, 0] + H[0, 2] = T[0] + H[1, 2] = T[1] if Hin is None: return H @@ -117,17 +111,18 @@ def nearest_neighbor_assosiation(ppoints, cpoints): def SVD_motion_estimation(ppoints, cpoints): - pm = np.matrix(np.mean(ppoints, axis=1)) - cm = np.matrix(np.mean(cpoints, axis=1)) + pm = np.asarray(np.mean(ppoints, axis=1)) + cm = np.asarray(np.mean(cpoints, axis=1)) + print(cm) - pshift = np.matrix(ppoints - pm) - cshift = np.matrix(cpoints - cm) + pshift = np.array(ppoints - pm[:,np.newaxis]) + cshift = np.array(cpoints - cm[:,np.newaxis]) - W = cshift * pshift.T + W = cshift.dot(pshift.T) u, s, vh = np.linalg.svd(W) - R = (u * vh).T - t = pm - R * cm + R = (u.dot(vh)).T + t = pm - R.dot(cm) return R, t @@ -147,17 +142,18 @@ def main(): # previous points px = (np.random.rand(nPoint) - 0.5) * fieldLength py = (np.random.rand(nPoint) - 0.5) * fieldLength - ppoints = np.matrix(np.vstack((px, py))) + ppoints = np.vstack((px, py)) # current points cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0] for (x, y) in zip(px, py)] cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] for (x, y) in zip(px, py)] - cpoints = np.matrix(np.vstack((cx, cy))) + cpoints = np.vstack((cx, cy)) + print(cpoints) R, T = ICP_matching(ppoints, cpoints) if __name__ == '__main__': - main() + main() \ No newline at end of file