To cope with the possibility of imprecise navigation, the trajectory
should be executed in a closed loop. Before segment i is executed,
the actual position of the robot is compared with position qi,
which was predicted by the planner. If the distance between these
is larger than a given threshold, it is not safe to execute the actual
plan, and replanning from the actual position is invoked