Fix for robot_pose_ekf