Skip to content

Commit df346de

Browse files
AtsushiSakaiAtsushiSakai
authored
Merge pull request AtsushiSakai#118 from daniel-s-ingram/master
AtsushiSakai#115
2 parents 8b4c96e + a9aa0c9 commit df346de

File tree

3 files changed

+63
-64
lines changed

3 files changed

+63
-64
lines changed

‎Localization/extended_kalman_filter/extended_kalman_filter.py‎

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727
def calc_input():
2828
v = 1.0 # [m/s]
2929
yawrate = 0.1 # [rad/s]
30-
u = np.matrix([v, yawrate]).T
30+
u = np.array([[v, yawrate]]).T
3131
return u
3232

3333

@@ -38,12 +38,12 @@ def observation(xTrue, xd, u):
3838
# add noise to gps x-y
3939
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
4040
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
41-
z = np.matrix([zx, zy])
41+
z = np.array([[zx, zy]])
4242

4343
# add noise to input
4444
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
4545
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
46-
ud = np.matrix([ud1, ud2]).T
46+
ud = np.array([[ud1, ud2]]).T
4747

4848
xd = motion_model(xd, ud)
4949

@@ -52,29 +52,29 @@ def observation(xTrue, xd, u):
5252

5353
def motion_model(x, u):
5454

55-
F = np.matrix([[1.0, 0, 0, 0],
55+
F = np.array([[1.0, 0, 0, 0],
5656
[0, 1.0, 0, 0],
5757
[0, 0, 1.0, 0],
5858
[0, 0, 0, 0]])
5959

60-
B = np.matrix([[DT * math.cos(x[2, 0]), 0],
60+
B = np.array([[DT * math.cos(x[2, 0]), 0],
6161
[DT * math.sin(x[2, 0]), 0],
6262
[0.0, DT],
6363
[1.0, 0.0]])
6464

65-
x = F * x + B * u
65+
x = F.dot(x) + B.dot(u)
6666

6767
return x
6868

6969

7070
def observation_model(x):
7171
# Observation Model
72-
H = np.matrix([
72+
H = np.array([
7373
[1, 0, 0, 0],
7474
[0, 1, 0, 0]
7575
])
7676

77-
z = H * x
77+
z = H.dot(x)
7878

7979
return z
8080

@@ -96,7 +96,7 @@ def jacobF(x, u):
9696
"""
9797
yaw = x[2, 0]
9898
v = u[0, 0]
99-
jF = np.matrix([
99+
jF = np.array([
100100
[1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
101101
[0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
102102
[0.0, 0.0, 1.0, 0.0],
@@ -107,7 +107,7 @@ def jacobF(x, u):
107107

108108
def jacobH(x):
109109
# Jacobian of Observation Model
110-
jH = np.matrix([
110+
jH = np.array([
111111
[1, 0, 0, 0],
112112
[0, 1, 0, 0]
113113
])
@@ -126,10 +126,10 @@ def ekf_estimation(xEst, PEst, z, u):
126126
jH = jacobH(xPred)
127127
zPred = observation_model(xPred)
128128
y = z.T - zPred
129-
S = jH * PPred * jH.T + R
130-
K = PPred * jH.T * np.linalg.inv(S)
131-
xEst = xPred + K * y
132-
PEst = (np.eye(len(xEst)) - K * jH) * PPred
129+
S = jH.dot(PPred).dot(jH.T) + R
130+
K = PPred.dot(jH.T).dot(np.linalg.inv(S))
131+
xEst = xPred + K.dot(y)
132+
PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred)
133133

134134
return xEst, PEst
135135

@@ -151,9 +151,9 @@ def plot_covariance_ellipse(xEst, PEst):
151151
x = [a * math.cos(it) for it in t]
152152
y = [b * math.sin(it) for it in t]
153153
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
154-
R = np.matrix([[math.cos(angle), math.sin(angle)],
154+
R = np.array([[math.cos(angle), math.sin(angle)],
155155
[-math.sin(angle), math.cos(angle)]])
156-
fx = R * np.matrix([x, y])
156+
fx = R.dot(np.array([[x, y]]))
157157
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
158158
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
159159
plt.plot(px, py, "--r")
@@ -165,11 +165,11 @@ def main():
165165
time = 0.0
166166

167167
# State Vector [x y yaw v]'
168-
xEst = np.matrix(np.zeros((4, 1)))
169-
xTrue = np.matrix(np.zeros((4, 1)))
168+
xEst = np.array(np.zeros((4, 1)))
169+
xTrue = np.array(np.zeros((4, 1)))
170170
PEst = np.eye(4)
171171

172-
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
172+
xDR = np.array(np.zeros((4, 1))) # Dead reckoning
173173

174174
# history
175175
hxEst = xEst

‎Localization/particle_filter/particle_filter.py‎

Lines changed: 31 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
def calc_input():
3333
v = 1.0 # [m/s]
3434
yawrate = 0.1 # [rad/s]
35-
u = np.matrix([v, yawrate]).T
35+
u = np.array([[v, yawrate]]).T
3636
return u
3737

3838

@@ -41,7 +41,7 @@ def observation(xTrue, xd, u, RFID):
4141
xTrue = motion_model(xTrue, u)
4242

4343
# add noise to gps x-y
44-
z = np.matrix(np.zeros((0, 3)))
44+
z = np.zeros((0, 3))
4545

4646
for i in range(len(RFID[:, 0])):
4747

@@ -50,13 +50,13 @@ def observation(xTrue, xd, u, RFID):
5050
d = math.sqrt(dx**2 + dy**2)
5151
if d <= MAX_RANGE:
5252
dn = d + np.random.randn() * Qsim[0, 0] # add noise
53-
zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
53+
zi = np.array([[dn, RFID[i, 0], RFID[i, 1]]])
5454
z = np.vstack((z, zi))
5555

5656
# add noise to input
5757
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
5858
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
59-
ud = np.matrix([ud1, ud2]).T
59+
ud = np.array([[ud1, ud2]]).T
6060

6161
xd = motion_model(xd, ud)
6262

@@ -65,17 +65,17 @@ def observation(xTrue, xd, u, RFID):
6565

6666
def motion_model(x, u):
6767

68-
F = np.matrix([[1.0, 0, 0, 0],
69-
[0, 1.0, 0, 0],
70-
[0, 0, 1.0, 0],
71-
[0, 0, 0, 0]])
68+
F = np.array([[1.0, 0, 0, 0],
69+
[0, 1.0, 0, 0],
70+
[0, 0, 1.0, 0],
71+
[0, 0, 0, 0]])
7272

73-
B = np.matrix([[DT * math.cos(x[2, 0]), 0],
74-
[DT * math.sin(x[2, 0]), 0],
75-
[0.0, DT],
76-
[1.0, 0.0]])
73+
B = np.array([[DT * math.cos(x[2, 0]), 0],
74+
[DT * math.sin(x[2, 0]), 0],
75+
[0.0, DT],
76+
[1.0, 0.0]])
7777

78-
x = F * x + B * u
78+
x = F.dot(x) + B.dot(u)
7979

8080
return x
8181

@@ -88,11 +88,11 @@ def gauss_likelihood(x, sigma):
8888

8989

9090
def calc_covariance(xEst, px, pw):
91-
cov = np.matrix(np.zeros((3, 3)))
91+
cov = np.zeros((3, 3))
9292

9393
for i in range(px.shape[1]):
9494
dx = (px[:, i] - xEst)[0:3]
95-
cov += pw[0, i] * dx * dx.T
95+
cov += pw[0, i] * dx.dot(dx.T)
9696

9797
return cov
9898

@@ -103,13 +103,12 @@ def pf_localization(px, pw, xEst, PEst, z, u):
103103
"""
104104

105105
for ip in range(NP):
106-
x = px[:, ip]
106+
x = np.array([px[:, ip]]).T
107107
w = pw[0, ip]
108-
109-
# Predict with ramdom input sampling
108+
# Predict with random input sampling
110109
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
111110
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
112-
ud = np.matrix([ud1, ud2]).T
111+
ud = np.array([[ud1, ud2]]).T
113112
x = motion_model(x, ud)
114113

115114
# Calc Inportance Weight
@@ -120,12 +119,12 @@ def pf_localization(px, pw, xEst, PEst, z, u):
120119
dz = prez - z[i, 0]
121120
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
122121

123-
px[:, ip] = x
122+
px[:, ip] = x[:, 0]
124123
pw[0, ip] = w
125124

126125
pw = pw / pw.sum() # normalize
127126

128-
xEst = px * pw.T
127+
xEst = px.dot(pw.T)
129128
PEst = calc_covariance(xEst, px, pw)
130129

131130
px, pw = resampling(px, pw)
@@ -138,21 +137,21 @@ def resampling(px, pw):
138137
low variance re-sampling
139138
"""
140139

141-
Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number
140+
Neff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
142141
if Neff < NTh:
143142
wcum = np.cumsum(pw)
144143
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
145-
resampleid = base + np.random.rand(base.shape[1]) / NP
144+
resampleid = base + np.random.rand(base.shape[0]) / NP
146145

147146
inds = []
148147
ind = 0
149148
for ip in range(NP):
150-
while resampleid[0, ip] > wcum[0, ind]:
149+
while resampleid[ip] > wcum[ind]:
151150
ind += 1
152151
inds.append(ind)
153152

154153
px = px[:, inds]
155-
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight
154+
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
156155

157156
return px, pw
158157

@@ -185,9 +184,9 @@ def plot_covariance_ellipse(xEst, PEst):
185184
x = [a * math.cos(it) for it in t]
186185
y = [b * math.sin(it) for it in t]
187186
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
188-
R = np.matrix([[math.cos(angle), math.sin(angle)],
187+
R = np.array([[math.cos(angle), math.sin(angle)],
189188
[-math.sin(angle), math.cos(angle)]])
190-
fx = R * np.matrix([x, y])
189+
fx = R.dot(np.array([[x, y]]))
191190
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
192191
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
193192
plt.plot(px, py, "--r")
@@ -205,13 +204,13 @@ def main():
205204
[-5.0, 20.0]])
206205

207206
# State Vector [x y yaw v]'
208-
xEst = np.matrix(np.zeros((4, 1)))
209-
xTrue = np.matrix(np.zeros((4, 1)))
207+
xEst = np.zeros((4, 1))
208+
xTrue = np.zeros((4, 1))
210209
PEst = np.eye(4)
211210

212-
px = np.matrix(np.zeros((4, NP))) # Particle store
213-
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight
214-
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
211+
px = np.zeros((4, NP)) # Particle store
212+
pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
213+
xDR = np.zeros((4, 1)) # Dead reckoning
215214

216215
# history
217216
hxEst = xEst

‎PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ def pi_2_pi(angle):
7979

8080
def get_linear_model_matrix(v, phi, delta):
8181

82-
A = np.matrix(np.zeros((NX, NX)))
82+
A = np.zeros((NX, NX))
8383
A[0, 0] = 1.0
8484
A[1, 1] = 1.0
8585
A[2, 2] = 1.0
@@ -90,7 +90,7 @@ def get_linear_model_matrix(v, phi, delta):
9090
A[1, 3] = DT * v * math.cos(phi)
9191
A[3, 2] = DT * math.tan(delta) / WB
9292

93-
B = np.matrix(np.zeros((NX, NU)))
93+
B = np.zeros((NX, NU))
9494
B[2, 0] = DT
9595
B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)
9696

@@ -104,10 +104,10 @@ def get_linear_model_matrix(v, phi, delta):
104104

105105
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
106106

107-
outline = np.matrix([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
107+
outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
108108
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
109109

110-
fr_wheel = np.matrix([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
110+
fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
111111
[-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
112112

113113
rr_wheel = np.copy(fr_wheel)
@@ -117,22 +117,22 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
117117
rl_wheel = np.copy(rr_wheel)
118118
rl_wheel[1, :] *= -1
119119

120-
Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
120+
Rot1 = np.array([[math.cos(yaw), math.sin(yaw)],
121121
[-math.sin(yaw), math.cos(yaw)]])
122-
Rot2 = np.matrix([[math.cos(steer), math.sin(steer)],
122+
Rot2 = np.array([[math.cos(steer), math.sin(steer)],
123123
[-math.sin(steer), math.cos(steer)]])
124124

125-
fr_wheel = (fr_wheel.T * Rot2).T
126-
fl_wheel = (fl_wheel.T * Rot2).T
125+
fr_wheel = (fr_wheel.T.dot(Rot2)).T
126+
fl_wheel = (fl_wheel.T.dot(Rot2)).T
127127
fr_wheel[0, :] += WB
128128
fl_wheel[0, :] += WB
129129

130-
fr_wheel = (fr_wheel.T * Rot1).T
131-
fl_wheel = (fl_wheel.T * Rot1).T
130+
fr_wheel = (fr_wheel.T.dot(Rot1)).T
131+
fl_wheel = (fl_wheel.T.dot(Rot1)).T
132132

133-
outline = (outline.T * Rot1).T
134-
rr_wheel = (rr_wheel.T * Rot1).T
135-
rl_wheel = (rl_wheel.T * Rot1).T
133+
outline = (outline.T.dot(Rot1)).T
134+
rr_wheel = (rr_wheel.T.dot(Rot1)).T
135+
rl_wheel = (rl_wheel.T.dot(Rot1)).T
136136

137137
outline[0, :] += x
138138
outline[1, :] += y

0 commit comments

Comments
 (0)