updateHeading function implemented.

Compensator implemented.