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