heading wraps to avoid discontinuity when flying with changing headings

[wynn4]

I added wrapping to the heading measurement just like we do in the UAV book and ROSPlane. Without this change, the estimator will fail when you try to fly the copter around in a circle (heading changing as you go) because of the discontinuity when suddenly going from PI to -PI.