Actually fixes the heading values!

This accounts for pitch and yaw which makes our readings far more
accurate, and updates how the GPS updates