End of year

BDEvan5 · Dec 1, 2023 · 5b674ce · 5b674ce
1 parent 0ee0227
commit 5b674ce
Show file tree

Hide file tree

Showing 4 changed files with 660 additions and 658 deletions.
diff --git a/media/Particle Filter localisation.svg b/media/Particle Filter localisation.svg
diff --git a/sensor_fusion/RacingTests.py b/sensor_fusion/RacingTests.py
@@ -30,10 +30,10 @@ def simulate_car_pf():
 
     true_states, noisy_states = robot.get_states()
     estimates = particle_filter.get_estimated_states()
-    plot_racing_localistion(estimates, true_states, noisy_states, robot)
-
     mae = np.mean(np.abs(true_states - estimates))
     print(f"MAE: {mae:.4f} cm")
+    plot_racing_localistion(estimates, true_states, noisy_states, robot)
+
 
 
 def profile_particle_filter(number_of_particles):

diff --git a/sensor_fusion/estimators/ParticleFilter.py b/sensor_fusion/estimators/ParticleFilter.py
@@ -21,14 +21,23 @@ def control_update(self, control):
         random_samples = np.random.multivariate_normal(np.zeros(3), self.Q, self.NP)
         self.particles = next_states + random_samples
 
-    def particle_control_update(self, control):
-        next_states = self.f(self.particles, control)
-        random_samples = np.random.multivariate_normal(np.zeros(3), self.Q, self.NP)
-        self.particles = next_states + random_samples
+    # def particle_control_update(self, control):
+    #     next_states = self.f(self.particles, control)
+    #     random_samples = np.random.multivariate_normal(np.zeros(3), self.Q, self.NP)
+    #     self.particles = next_states + random_samples
 
     def measurement_update(self, measurement):
         particle_measurements = self.h(self.particles)
         z = particle_measurements - measurement
+        # ssd = np.power(z, 2)
+        # ssd = np.sum(ssd, axis=1)
+        # sigma = np.sqrt(np.average(ssd, axis=0))
+        # self.weights = 1.0 / np.sqrt(2.0 * np.pi * sigma ** 2) * np.exp(-ssd / (2 * sigma ** 2))
+
+
+        # ssd = np.power(z, 2)
+        # ssd = np.sum(ssd, axis=1)
+        # self.weights = np.exp(-0.5 * ssd)
         sigma = np.sqrt(np.average(z**2, axis=0))
         weights = 1.0 / np.sqrt(2.0 * np.pi * sigma ** 2) * np.exp(-z ** 2 / (2 * sigma ** 2))
         self.weights = np.prod(weights, axis=1) 

diff --git a/sensor_fusion/robots/AutonomousRacer.py b/sensor_fusion/robots/AutonomousRacer.py
@@ -8,7 +8,8 @@
 RANGE = 100
 
 MAP_NAME = "aut_wide"
-NUM_BEAMS = 10
+NUM_BEAMS = 200
+# NUM_BEAMS = 45
 
 
 class AutonomousRacer: