diff --git a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
index cc1d6481c10136bae1dc968a0ea628c16ef6f209..eb21fdf15b88dc012433161ee29dadd8051021f9 100644
--- a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
@@ -149,7 +149,7 @@ void ImageAnswer(const asclinic_pkg::position& msg) {
 
     //ROS_INFO_STREAM("new_pose: " << new_pose);
 
-    if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4) {
+    if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4||std::abs(odo_position[0]-new_pose[0])>1.5||std::abs(odo_position[1]-new_pose[1]>1.5)) {
         x = odo_position(0);
         y = odo_position(1);
         phi = odo_position(2);
diff --git a/catkin_ws/src/asclinic_pkg/src/image.py b/catkin_ws/src/asclinic_pkg/src/image.py
index 3f688618f4835407339add6ce9708e20cea887fa..a695e4c89fd77b69a0a51e09db572aaba273e857 100755
--- a/catkin_ws/src/asclinic_pkg/src/image.py
+++ b/catkin_ws/src/asclinic_pkg/src/image.py
@@ -63,7 +63,7 @@ def answer_callback(msg, markers, publisher):
                 ry = fiducial_marker.rvec[1]
                 rz = fiducial_marker.rvec[2]
                 now_d = math.sqrt(tx**2 + tz**2)
-                if now_d > 4:
+                if now_d > 3.5:
                     continue
                 valid_number+=1
                 # Compute rotation matrix