github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 08:18:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #118 from daniel-s-ingram/master

Browse Source 
#115
This commit is contained in:
Atsushi Sakai
2018-11-03 10:59:47 +09:00
committed by GitHub
parent 8b4c96e5d9 a9aa0c9c6d
commit df346debb8
3 changed files with 63 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -27,7 +27,7 @@ show_animation = True
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
@@ -38,12 +38,12 @@ def observation(xTrue, xd, u):
# add noise to gps x-y
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
z = np.matrix([zx, zy])
z = np.array([[zx, zy]])
# 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([[ud1, ud2]]).T
xd = motion_model(xd, ud)
@@ -52,29 +52,29 @@ def observation(xTrue, xd, u):
def motion_model(x, u):
F = np.matrix([[1.0, 0, 0, 0],
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 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],
[1.0, 0.0]])
x = F * x + B * u
x = F.dot(x) + B.dot(u)
return x
def observation_model(x):
# Observation Model
H = np.matrix([
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
z = H * x
z = H.dot(x)
return z
@@ -96,7 +96,7 @@ def jacobF(x, u):
"""
yaw = x[2, 0]
v = u[0, 0]
jF = np.matrix([
jF = np.array([
[1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
[0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
[0.0, 0.0, 1.0, 0.0],
@@ -107,7 +107,7 @@ def jacobF(x, u):
def jacobH(x):
# Jacobian of Observation Model
jH = np.matrix([
jH = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
@@ -126,10 +126,10 @@ def ekf_estimation(xEst, PEst, z, u):
jH = jacobH(xPred)
zPred = observation_model(xPred)
y = z.T - zPred
S = jH * PPred * jH.T + R
K = PPred * jH.T * np.linalg.inv(S)
xEst = xPred + K * y
PEst = (np.eye(len(xEst)) - K * jH) * PPred
S = jH.dot(PPred).dot(jH.T) + R
K = PPred.dot(jH.T).dot(np.linalg.inv(S))
xEst = xPred + K.dot(y)
PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred)
return xEst, PEst
@@ -151,9 +151,9 @@ def plot_covariance_ellipse(xEst, PEst):
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.matrix([[math.cos(angle), math.sin(angle)],
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R * np.matrix([x, y])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -165,11 +165,11 @@ def main():
time = 0.0
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((4, 1)))
xTrue = np.matrix(np.zeros((4, 1)))
xEst = np.array(np.zeros((4, 1)))
xTrue = np.array(np.zeros((4, 1)))
PEst = np.eye(4)
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
xDR = np.array(np.zeros((4, 1))) # Dead reckoning
# history
hxEst = xEst

63
Localization/particle_filter/particle_filter.py
View File

@@ -32,7 +32,7 @@ show_animation = True
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
@@ -41,7 +41,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])):
@@ -50,13 +50,13 @@ def observation(xTrue, xd, u, RFID):
d = math.sqrt(dx**2 + dy**2)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
zi = np.array([[dn, RFID[i, 0], RFID[i, 1]]])
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([[ud1, ud2]]).T
xd = motion_model(xd, ud)
@@ -65,17 +65,17 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u):
F = np.matrix([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.matrix([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F * x + B * u
x = F.dot(x) + B.dot(u)
return x
@@ -88,11 +88,11 @@ def gauss_likelihood(x, sigma):
def calc_covariance(xEst, px, pw):
cov = np.matrix(np.zeros((3, 3)))
cov = np.zeros((3, 3))
for i in range(px.shape[1]):
dx = (px[:, i] - xEst)[0:3]
cov += pw[0, i] * dx * dx.T
cov += pw[0, i] * dx.dot(dx.T)
return cov
@@ -103,13 +103,12 @@ def pf_localization(px, pw, xEst, PEst, z, u):
"""
for ip in range(NP):
x = px[:, ip]
x = np.array([px[:, ip]]).T
w = pw[0, ip]
# Predict with ramdom input sampling
# Predict with random input sampling
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([[ud1, ud2]]).T
x = motion_model(x, ud)
# Calc Inportance Weight
@@ -120,12 +119,12 @@ def pf_localization(px, pw, xEst, PEst, z, u):
dz = prez - z[i, 0]
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
px[:, ip] = x
px[:, ip] = x[:, 0]
pw[0, ip] = w
pw = pw / pw.sum() # normalize
xEst = px * pw.T
xEst = px.dot(pw.T)
PEst = calc_covariance(xEst, px, pw)
px, pw = resampling(px, pw)
@@ -138,21 +137,21 @@ def resampling(px, pw):
low variance re-sampling
"""
Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number
Neff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if Neff < NTh:
wcum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
resampleid = base + np.random.rand(base.shape[1]) / NP
resampleid = base + np.random.rand(base.shape[0]) / NP
inds = []
ind = 0
for ip in range(NP):
while resampleid[0, ip] > wcum[0, ind]:
while resampleid[ip] > wcum[ind]:
ind += 1
inds.append(ind)
px = px[:, inds]
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
return px, pw
@@ -185,9 +184,9 @@ def plot_covariance_ellipse(xEst, PEst):
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.matrix([[math.cos(angle), math.sin(angle)],
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R * np.matrix([x, y])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -205,13 +204,13 @@ def main():
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((4, 1)))
xTrue = np.matrix(np.zeros((4, 1)))
xEst = np.zeros((4, 1))
xTrue = np.zeros((4, 1))
PEst = np.eye(4)
px = np.matrix(np.zeros((4, NP))) # Particle store
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
px = np.zeros((4, NP)) # Particle store
pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
xDR = np.zeros((4, 1)) # Dead reckoning
# history
hxEst = xEst

26
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -79,7 +79,7 @@ def pi_2_pi(angle):
def get_linear_model_matrix(v, phi, delta):
A = np.matrix(np.zeros((NX, NX)))
A = np.zeros((NX, NX))
A[0, 0] = 1.0
A[1, 1] = 1.0
A[2, 2] = 1.0
@@ -90,7 +90,7 @@ def get_linear_model_matrix(v, phi, delta):
A[1, 3] = DT * v * math.cos(phi)
A[3, 2] = DT * math.tan(delta) / WB
B = np.matrix(np.zeros((NX, NU)))
B = np.zeros((NX, NU))
B[2, 0] = DT
B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)
@@ -104,10 +104,10 @@ def get_linear_model_matrix(v, phi, delta):
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
outline = np.matrix([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
fr_wheel = np.matrix([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
[-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
rr_wheel = np.copy(fr_wheel)
@@ -117,22 +117,22 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
rl_wheel = np.copy(rr_wheel)
rl_wheel[1, :] *= -1
Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
Rot1 = np.array([[math.cos(yaw), math.sin(yaw)],
[-math.sin(yaw), math.cos(yaw)]])
Rot2 = np.matrix([[math.cos(steer), math.sin(steer)],
Rot2 = np.array([[math.cos(steer), math.sin(steer)],
[-math.sin(steer), math.cos(steer)]])
fr_wheel = (fr_wheel.T * Rot2).T
fl_wheel = (fl_wheel.T * Rot2).T
fr_wheel = (fr_wheel.T.dot(Rot2)).T
fl_wheel = (fl_wheel.T.dot(Rot2)).T
fr_wheel[0, :] += WB
fl_wheel[0, :] += WB
fr_wheel = (fr_wheel.T * Rot1).T
fl_wheel = (fl_wheel.T * Rot1).T
fr_wheel = (fr_wheel.T.dot(Rot1)).T
fl_wheel = (fl_wheel.T.dot(Rot1)).T
outline = (outline.T * Rot1).T
rr_wheel = (rr_wheel.T * Rot1).T
rl_wheel = (rl_wheel.T * Rot1).T
outline = (outline.T.dot(Rot1)).T
rr_wheel = (rr_wheel.T.dot(Rot1)).T
rl_wheel = (rl_wheel.T.dot(Rot1)).T
outline[0, :] += x
outline[1, :] += y