AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 17:07:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

Doing th np.matrix -> np.array conversion (#115)

Browse Source 
I completed the following:
- iterative closest point
- EKF slam
This commit is contained in:
Hajdu Csaba
2018-11-03 22:24:33 +01:00
parent df346debb8
commit 10a33323b7
2 changed files with 55 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -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()

40
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -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()