diff --git a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp index eb21fdf15b88dc012433161ee29dadd8051021f9..e48102a4074dc4e1f5282e85182b76b14c8e85c2 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||std::abs(odo_position[0]-new_pose[0])>1.5||std::abs(odo_position[1]-new_pose[1]>1.5)) { + 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);