Heading control is pure P-controller in order not to overshoot