From b2d65f277ec9d7a122baaec7d2bf4a4b77c5d6a8 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: Sat, 26 Oct 2019 09:11:23 +0300
Subject: [PATCH] change resampling function to match with the book

- changed resampling function so that it chooses random number once for the starting point.
- 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        | 30 +++++++++----------
 1 file changed, 14 insertions(+), 16 deletions(-)

diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index 6643116f..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,21 +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.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
-        re_sample_id = base + np.random.rand(base.shape[0]) / NP
+    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)
 
-        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