Fixed movement control bug and tolerance

Changed objective direction to use yaw instead of pitch
Increased tolerance for position
Changed objective positions