Write out yaw into position topics
Merge branch 'paul_estimator' into paul_estimator_mavlink2