From d1e6e1afa5a46a7151d4fee97bce9c02b722682c Mon Sep 17 00:00:00 2001
From: "SIDDARTH.D" <siddarth.dayasagar@gmail.com>
Date: Sat, 19 Oct 2024 03:18:23 -0400
Subject: [PATCH] Updated with low-variance resampling

Updated the algorithm with a low variance step for resampling, this is much better than multinomial resampling.
---
 .../Particle filter/particle_filter.py        | 30 ++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/Localization/Particle filter/particle_filter.py b/Localization/Particle filter/particle_filter.py
index ded5273..3ae27e6 100644
--- a/Localization/Particle filter/particle_filter.py	
+++ b/Localization/Particle filter/particle_filter.py	
@@ -135,6 +135,34 @@ def resample_particles(self, weights):
         
         self.particles = resampled_particles
 
+    def low_variance_resample(self, weights):
+        """Low variance resampling method."""
+        weights = np.array(weights)
+        weights_sum = np.sum(weights)
+        
+        if weights_sum == 0:
+            print("All weights are zero! Check measurement model.")
+            weights = np.ones(self.num_particles) / self.num_particles  
+        else:
+            weights /= weights_sum
+        
+        num_particles = self.num_particles
+        resampled_particles = []
+    
+        r = np.random.uniform(0, 1/num_particles)  
+        cumulative_sum = 0.0
+        index = 0
+        c = weights[0]
+    
+        for i in range(num_particles):
+            U = r + i / num_particles 
+            while U > c:
+                index += 1
+                c += weights[index]
+            resampled_particles.append(self.particles[index])
+    
+        self.particles = resampled_particles
+
     
     def update(self, measurement):
         """
@@ -232,4 +260,4 @@ def main():
     pf.e_plot_particles()
         
 if __name__=="__main__":
-    main()
\ No newline at end of file
+    main()