Skip to content

Commit 014fd86

Browse files
AtsushiSakaiAtsushiSakai
committed
first release PF localization
1 parent 52d7596 commit 014fd86

1 file changed

Lines changed: 57 additions & 100 deletions

File tree

‎Localization/particle_filter/particle_filter.py‎

Lines changed: 57 additions & 100 deletions
Original file line numberDiff line numberDiff line change
@@ -6,92 +6,16 @@
66
77
"""
88

9-
# pw=Normalize(pw,NP);%正規化
10-
# [px,pw]=Resampling(px,pw,NTh,NP);%リサンプリング
11-
# xEst=px*pw';%最終推定値は期待値
12-
13-
# %Animation (remove some flames)
14-
# if rem(i,5)==0
15-
# hold off;
16-
# arrow=0.5;
17-
# %パーティクル表示
18-
# for ip=1:NP
19-
# quiver(px(1,ip),px(2,ip),arrow*cos(px(3,ip)),arrow*sin(px(3,ip)),'ok');hold on;
20-
# end
21-
# plot(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on;
22-
# plot(RFID(:,1),RFID(:,2),'pk','MarkerSize',10);hold on;
23-
# %観測線の表示
24-
# if~isempty(z)
25-
# for iz=1:length(z(:,1))
26-
# ray=[xTrue(1:2)';z(iz,2:3)];
27-
# plot(ray(:,1),ray(:,2),'-r');hold on;
28-
# end
29-
# end
30-
# plot(result.xd(:,1),result.xd(:,2),'.k');hold on;
31-
# plot(result.xEst(:,1),result.xEst(:,2),'.r');hold on;
32-
# axis equal;
33-
# grid on;
34-
# drawnow;
35-
#
36-
# function [px,pw]=Resampling(px,pw,NTh,NP)
37-
# %リサンプリングを実施する関数
38-
# %アルゴリズムはLow Variance Sampling
39-
# Neff=1.0/(pw*pw');
40-
# if Neff<NTh %リサンプリング
41-
# wcum=cumsum(pw);
42-
# base=cumsum(pw*0+1/NP)-1/NP;%乱数を加える前のbase
43-
# resampleID=base+rand/NP;%ルーレットを乱数分増やす
44-
# ppx=px;%データ格納用
45-
# ind=1;%新しいID
46-
# for ip=1:NP
47-
# while(resampleID(ip)>wcum(ind))
48-
# ind=ind+1;
49-
# end
50-
# px(:,ip)=ppx(:,ind);%LVSで選ばれたパーティクルに置き換え
51-
# pw(ip)=1/NP;%尤度は初期化
52-
# end
53-
# end
54-
55-
# function pw=Normalize(pw,NP)
56-
# %重みベクトルを正規化する関数
57-
# sumw=sum(pw);
58-
# if sumw~=0
59-
# pw=pw/sum(pw);%正規化
60-
# else
61-
# pw=zeros(1,NP)+1/NP;
62-
# end
63-
64-
65-
# function p=Gauss(x,u,sigma)
66-
# %ガウス分布の確率密度を計算する関数
67-
# p=1/sqrt(2*pi*sigma^2)*exp(-(x-u)^2/(2*sigma^2));
68-
69-
# %Calc Observation from noise prameter
70-
# function [z, x, xd, u] = Observation(x, xd, u, RFID,MAX_RANGE)
71-
# global Qsigma;
72-
# global Rsigma;
73-
74-
# x=f(x, u);% Ground Truth
75-
# u=u+sqrt(Qsigma)*randn(2,1);%add Process Noise
76-
# xd=f(xd, u);% Dead Reckoning
77-
# %Simulate Observation
78-
# z=[];
79-
# for iz=1:length(RFID(:,1))
80-
# d=norm(RFID(iz,:)-x(1:2)');
81-
# if d<MAX_RANGE %観測範囲内
82-
# z=[z;[d+sqrt(Rsigma)*randn(1,1) RFID(iz,:)]];
83-
# end
84-
859
import numpy as np
8610
import math
8711
import matplotlib.pyplot as plt
8812

8913
# Estimation parameter of EKF
90-
Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2
14+
Q = np.diag([0.1])**2 # range error
9115
R = np.diag([1.0, math.radians(40.0)])**2
9216

9317
# Simulation parameter
94-
Qsim = np.diag([0.5, 0.5])**2
18+
Qsim = np.diag([0.2])**2
9519
Rsim = np.diag([1.0, math.radians(30.0)])**2
9620

9721
DT = 0.1 # time tick [s]
@@ -125,7 +49,8 @@ def observation(xTrue, xd, u, RFID):
12549
dy = xTrue[1, 0] - RFID[i, 1]
12650
d = math.sqrt(dx**2 + dy**2)
12751
if d <= MAX_RANGE:
128-
zi = np.matrix([d, RFID[i, 0], RFID[i, 1]])
52+
dn = d + np.random.randn() * Qsim[0, 0] # add noise
53+
zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
12954
z = np.vstack((z, zi))
13055

13156
# add noise to input
@@ -155,50 +80,83 @@ def motion_model(x, u):
15580
return x
15681

15782

158-
def observation_model(x):
159-
# Observation Model
160-
H = np.matrix([
161-
[1, 0, 0, 0],
162-
[0, 1, 0, 0]
163-
])
83+
def gauss_likelihood(x, sigma):
84+
p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
85+
math.exp(-x ** 2 / (2 * sigma ** 2))
86+
87+
return p
88+
16489

165-
z = H * x
90+
def calc_covariance(xEst, px, pw):
91+
cov = np.matrix(np.zeros((3, 3)))
16692

167-
return z
93+
for i in range(px.shape[1]):
94+
dx = (px[:, i] - xEst)[0:3]
95+
cov += pw[0, i] * dx * dx.T
16896

97+
return cov
16998

170-
def pf_estimation(px, pw, xEst, PEst, z, u):
17199

172-
# Predict
100+
def pf_localization(px, pw, xEst, PEst, z, u):
101+
"""
102+
Localization with Particle filter
103+
"""
104+
173105
for ip in range(NP):
174106
x = px[:, ip]
175-
# w = pw[ip]
107+
w = pw[0, ip]
176108

109+
# Predict with ramdom input sampling
177110
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
178111
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
179112
ud = np.matrix([ud1, ud2]).T
180-
181113
x = motion_model(x, ud)
182114

183-
px[:, ip] = x
184-
185115
# Calc Inportance Weight
186116
for i in range(len(z[:, 0])):
187117
dx = x[0, 0] - z[i, 1]
188118
dy = x[1, 0] - z[i, 2]
189119
prez = math.sqrt(dx**2 + dy**2)
190120
dz = prez - z[i, 0]
191-
# w=w*Gauss(dz,0,sqrt(R));
192-
# end
193-
# px(:,ip)=x;%格納
194-
# pw(ip)=w;
195-
# end
121+
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
122+
123+
px[:, ip] = x
124+
pw[0, ip] = w
125+
126+
pw = pw / pw.sum() # normalize
196127

197128
xEst = px * pw.T
129+
PEst = calc_covariance(xEst, px, pw)
130+
131+
px, pw = resampling(px, pw)
198132

199133
return xEst, PEst, px, pw
200134

201135

136+
def resampling(px, pw):
137+
"""
138+
low variance re-sampling
139+
"""
140+
141+
Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number
142+
if Neff < NTh:
143+
wcum = np.cumsum(pw)
144+
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
145+
resampleid = base + np.random.rand(base.shape[1]) / NP
146+
147+
inds = []
148+
ind = 0
149+
for ip in range(NP):
150+
while resampleid[0, ip] > wcum[0, ind]:
151+
ind += 1
152+
inds.append(ind)
153+
154+
px = px[:, inds]
155+
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight
156+
157+
return px, pw
158+
159+
202160
def plot_covariance_ellipse(xEst, PEst):
203161
Pxy = PEst[0:2, 0:2]
204162
eigval, eigvec = np.linalg.eig(Pxy)
@@ -242,7 +200,6 @@ def main():
242200

243201
px = np.matrix(np.zeros((4, NP))) # Particle store
244202
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight
245-
246203
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
247204

248205
# history
@@ -256,7 +213,7 @@ def main():
256213

257214
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
258215

259-
xEst, PEst, px, pw = pf_estimation(px, pw, xEst, PEst, z, ud)
216+
xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
260217

261218
# store data history
262219
hxEst = np.hstack((hxEst, xEst))

0 commit comments

Comments
 (0)