From 7a1784ad4798b4febd82a9a276fdae79a116a57b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Thu, 24 Oct 2019 19:43:22 +0300 Subject: [PATCH] condition check is moved 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. --- .../particle_filter/particle_filter.py | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 8859b514..0ebb5fc3 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -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