Initial drawing of the trajectory
Trajectory is now shown normally
Now only the current position is showed
Fixed bug that made the trajectory be wrongly calculated