Changed Quaternion normalizations to a normalizing filter for speed. Also removed unnecessary heading computations.
Changed Quaternion normalizations to a normalizing filter for speed. Also removed unnecessary heading computations.

ctraabe/NaviCtrl,ctraabe/NaviCtrl