Implemented updateHeading for rudder control and velocity control