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

condition check is moved

Browse Source 
condition check is moved from the inside of the resampling function
to the outside of the function, this way the purpose of the
function is much clear. Instead "resampling" every iteration,
resample if the condition holds.
This commit is contained in:
Göktuğ Karakaşlı
2019-10-24 19:43:22 +03:00
parent 9f4102378a
commit 7a1784ad47
1 changed files with 14 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
Localization/particle_filter/particle_filter.py
View File

@@ -128,8 +128,9 @@ def pf_localization(px, pw, z, u):
xEst = px.dot(pw.T)
PEst = calc_covariance(xEst, px, pw)
px, pw = re_sampling(px, pw)
N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if N_eff < NTh:
px, pw = re_sampling(px, pw)
return xEst, PEst, px, pw
@@ -138,20 +139,18 @@ def re_sampling(px, pw):
low variance re-sampling
"""
N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if N_eff < NTh:
w_cum = np.cumsum(pw)
base = np.arange(0.0, 1.0, 1/NP)
re_sample_id = base + np.random.uniform(0, 1/NP)
indexes = []
ind = 0
for ip in range(NP):
while re_sample_id[ip] > w_cum[ind]:
ind += 1
indexes.append(ind)
w_cum = np.cumsum(pw)
base = np.arange(0.0, 1.0, 1/NP)
re_sample_id = base + np.random.uniform(0, 1/NP)
indexes = []
ind = 0
for ip in range(NP):
while re_sample_id[ip] > w_cum[ind]:
ind += 1
indexes.append(ind)
px = px[:, indexes]
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
px = px[:, indexes]
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
return px, pw