Initial version of the heading estimator, v2