From a0eeeba7871bf85bc083361eba8eb83c4c16c0bf Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 10 Oct 2022 14:37:09 +0200
Subject: [PATCH] :sparkles:

---
 selflocalization/selflocalize.py | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index aa0f6e8..5900f5c 100644
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@@ -254,8 +254,16 @@ try:
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
         est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
         calc_weight(est_pose, landmark_values)
-        if est_pose.weight > 10**-23:
+        est_weight = est_pose.weight * 10**23
+        print(est_weight)
+        if est_weight > 1:
             break
+        else:
+            arlo.go_diff(POWER, POWER, 0, 1)
+            sleep((10 * TURN_T)/1000)
+            arlo.stop()
+            for p in particles:
+                p.setTheta(p.theta + np.deg2rad(10))
 
         if showGUI:
             # Draw map