Skip to content

Commit d4cb0f9

Browse files
AtsushiSakaiAtsushiSakai
committed
fix cal_covariance func and clean up code and added jupyter doc
1 parent 763ca4f commit d4cb0f9

File tree

2 files changed

+114
-48
lines changed

2 files changed

+114
-48
lines changed

‎Localization/particle_filter/particle_filter.ipynb‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "markdown",
5+
"metadata": {
6+
"collapsed": true,
7+
"pycharm": {
8+
"name": "#%% md\n"
9+
}
10+
},
11+
"source": [
12+
"# Particle Filter\n",
13+
"\n"
14+
]
15+
},
16+
{
17+
"cell_type": "markdown",
18+
"source": [
19+
"## How to calculate covariance matrix from particles\n",
20+
"\n",
21+
"$\\Xi_{j,k}=\\frac{1}{1-\\sum^N_{i=1}(w^i)^2}\\sum^N_{i=1}w^i(x^i_j-\\mu_j)(x^i_k-\\mu_k)$\n",
22+
"\n",
23+
"Ref:\n",
24+
"\n",
25+
"- [Improving the particle filter in high dimensions using conjugate artificial process noise](https://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/)\n"
26+
],
27+
"metadata": {
28+
"collapsed": false
29+
}
30+
}
31+
],
32+
"metadata": {
33+
"kernelspec": {
34+
"display_name": "Python 3",
35+
"language": "python",
36+
"name": "python3"
37+
},
38+
"language_info": {
39+
"codemirror_mode": {
40+
"name": "ipython",
41+
"version": 2
42+
},
43+
"file_extension": ".py",
44+
"mimetype": "text/x-python",
45+
"name": "python",
46+
"nbconvert_exporter": "python",
47+
"pygments_lexer": "ipython2",
48+
"version": "2.7.6"
49+
},
50+
"pycharm": {
51+
"stem_cell": {
52+
"cell_type": "raw",
53+
"source": [],
54+
"metadata": {
55+
"collapsed": false
56+
}
57+
}
58+
}
59+
},
60+
"nbformat": 4,
61+
"nbformat_minor": 0
62+
}

‎Localization/particle_filter/particle_filter.py‎

Lines changed: 52 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -37,20 +37,20 @@ def calc_input():
3737
return u
3838

3939

40-
def observation(xTrue, xd, u, RF_ID):
41-
xTrue = motion_model(xTrue, u)
40+
def observation(x_true, xd, u, rf_id):
41+
x_true = motion_model(x_true, u)
4242

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

46-
for i in range(len(RF_ID[:, 0])):
46+
for i in range(len(rf_id[:, 0])):
4747

48-
dx = xTrue[0, 0] - RF_ID[i, 0]
49-
dy = xTrue[1, 0] - RF_ID[i, 1]
48+
dx = x_true[0, 0] - rf_id[i, 0]
49+
dy = x_true[1, 0] - rf_id[i, 1]
5050
d = math.hypot(dx, dy)
5151
if d <= MAX_RANGE:
5252
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
53-
zi = np.array([[dn, RF_ID[i, 0], RF_ID[i, 1]]])
53+
zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
5454
z = np.vstack((z, zi))
5555

5656
# add noise to input
@@ -60,7 +60,7 @@ def observation(xTrue, xd, u, RF_ID):
6060

6161
xd = motion_model(xd, ud)
6262

63-
return xTrue, z, xd, ud
63+
return x_true, z, xd, ud
6464

6565

6666
def motion_model(x, u):
@@ -86,13 +86,17 @@ def gauss_likelihood(x, sigma):
8686
return p
8787

8888

89-
def calc_covariance(xEst, px, pw):
89+
def calc_covariance(x_est, px, pw):
90+
"""
91+
calculate covariance matrix
92+
see ipynb doc
93+
"""
9094
cov = np.zeros((3, 3))
91-
92-
for i in range(px.shape[1]):
93-
dx = (px[:, i] - xEst)[0:3]
94-
cov += pw[0, i] * dx.dot(dx.T)
95-
cov /= NP
95+
n_particle = px.shape[1]
96+
for i in range(n_particle):
97+
dx = (px[:, i:i + 1] - x_est)[0:3]
98+
cov += pw[0, i] * dx @ dx.T
99+
cov *= 1.0 / (1.0 - pw @ pw.T)
96100

97101
return cov
98102

@@ -125,13 +129,13 @@ def pf_localization(px, pw, z, u):
125129

126130
pw = pw / pw.sum() # normalize
127131

128-
xEst = px.dot(pw.T)
129-
PEst = calc_covariance(xEst, px, pw)
132+
x_est = px.dot(pw.T)
133+
p_est = calc_covariance(x_est, px, pw)
130134

131135
N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
132136
if N_eff < NTh:
133137
px, pw = re_sampling(px, pw)
134-
return xEst, PEst, px, pw
138+
return x_est, p_est, px, pw
135139

136140

137141
def re_sampling(px, pw):
@@ -140,8 +144,8 @@ def re_sampling(px, pw):
140144
"""
141145

142146
w_cum = np.cumsum(pw)
143-
base = np.arange(0.0, 1.0, 1/NP)
144-
re_sample_id = base + np.random.uniform(0, 1/NP)
147+
base = np.arange(0.0, 1.0, 1 / NP)
148+
re_sample_id = base + np.random.uniform(0, 1 / NP)
145149
indexes = []
146150
ind = 0
147151
for ip in range(NP):
@@ -155,9 +159,9 @@ def re_sampling(px, pw):
155159
return px, pw
156160

157161

158-
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
159-
Pxy = PEst[0:2, 0:2]
160-
eig_val, eig_vec = np.linalg.eig(Pxy)
162+
def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
163+
p_xy = p_est[0:2, 0:2]
164+
eig_val, eig_vec = np.linalg.eig(p_xy)
161165

162166
if eig_val[0] >= eig_val[1]:
163167
big_ind = 0
@@ -186,8 +190,8 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
186190
Rot = np.array([[math.cos(angle), -math.sin(angle)],
187191
[math.sin(angle), math.cos(angle)]])
188192
fx = Rot.dot(np.array([[x, y]]))
189-
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
190-
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
193+
px = np.array(fx[0, :] + x_est[0, 0]).flatten()
194+
py = np.array(fx[1, :] + x_est[1, 0]).flatten()
191195
plt.plot(px, py, "--r")
192196

193197

@@ -197,54 +201,54 @@ def main():
197201
time = 0.0
198202

199203
# RF_ID positions [x, y]
200-
RFi_ID = np.array([[10.0, 0.0],
201-
[10.0, 10.0],
202-
[0.0, 15.0],
203-
[-5.0, 20.0]])
204+
rf_id = np.array([[10.0, 0.0],
205+
[10.0, 10.0],
206+
[0.0, 15.0],
207+
[-5.0, 20.0]])
204208

205209
# State Vector [x y yaw v]'
206-
xEst = np.zeros((4, 1))
207-
xTrue = np.zeros((4, 1))
210+
x_est = np.zeros((4, 1))
211+
x_true = np.zeros((4, 1))
208212

209213
px = np.zeros((4, NP)) # Particle store
210214
pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
211-
xDR = np.zeros((4, 1)) # Dead reckoning
215+
x_dr = np.zeros((4, 1)) # Dead reckoning
212216

213217
# history
214-
hxEst = xEst
215-
hxTrue = xTrue
216-
hxDR = xTrue
218+
h_x_est = x_est
219+
h_x_true = x_true
220+
h_x_dr = x_true
217221

218222
while SIM_TIME >= time:
219223
time += DT
220224
u = calc_input()
221225

222-
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFi_ID)
226+
x_true, z, x_dr, ud = observation(x_true, x_dr, u, rf_id)
223227

224-
xEst, PEst, px, pw = pf_localization(px, pw, z, ud)
228+
x_est, PEst, px, pw = pf_localization(px, pw, z, ud)
225229

226230
# store data history
227-
hxEst = np.hstack((hxEst, xEst))
228-
hxDR = np.hstack((hxDR, xDR))
229-
hxTrue = np.hstack((hxTrue, xTrue))
231+
h_x_est = np.hstack((h_x_est, x_est))
232+
h_x_dr = np.hstack((h_x_dr, x_dr))
233+
h_x_true = np.hstack((h_x_true, x_true))
230234

231235
if show_animation:
232236
plt.cla()
233237
# for stopping simulation with the esc key.
234238
plt.gcf().canvas.mpl_connect('key_release_event',
235-
lambda event: [exit(0) if event.key == 'escape' else None])
239+
lambda event: [exit(0) if event.key == 'escape' else None])
236240

237241
for i in range(len(z[:, 0])):
238-
plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
239-
plt.plot(RFi_ID[:, 0], RFi_ID[:, 1], "*k")
242+
plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")
243+
plt.plot(rf_id[:, 0], rf_id[:, 1], "*k")
240244
plt.plot(px[0, :], px[1, :], ".r")
241-
plt.plot(np.array(hxTrue[0, :]).flatten(),
242-
np.array(hxTrue[1, :]).flatten(), "-b")
243-
plt.plot(np.array(hxDR[0, :]).flatten(),
244-
np.array(hxDR[1, :]).flatten(), "-k")
245-
plt.plot(np.array(hxEst[0, :]).flatten(),
246-
np.array(hxEst[1, :]).flatten(), "-r")
247-
plot_covariance_ellipse(xEst, PEst)
245+
plt.plot(np.array(h_x_true[0, :]).flatten(),
246+
np.array(h_x_true[1, :]).flatten(), "-b")
247+
plt.plot(np.array(h_x_dr[0, :]).flatten(),
248+
np.array(h_x_dr[1, :]).flatten(), "-k")
249+
plt.plot(np.array(h_x_est[0, :]).flatten(),
250+
np.array(h_x_est[1, :]).flatten(), "-r")
251+
plot_covariance_ellipse(x_est, PEst)
248252
plt.axis("equal")
249253
plt.grid(True)
250254
plt.pause(0.001)

0 commit comments

Comments
 (0)