diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index a55b9ae..659c97e 100644
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@@ -258,6 +258,8 @@ try:
         print("est_weight:", est_weight)
         print("certainty:", certainty)
         if est_weight > 1:
+            draw_world(est_pose, particles, world)
+            cv2.imwrite("test.png", world)
             break
         else:
             arlo.go_diff(POWER, POWER, 0, 1)