Implementation of a SLAM method. We compute the possible trajectory of the robot after dt time and we contract only this box to find solution
Adjusted parameter for iteration_step=100
last change before new branch