Small refactoring
Added task to calibrate compass and update yaw estimator
Make offset update reset the filter