AP_Math: refactor quaternion library

This is in preparation for my work on both AC_AttitudeControl work and compass EMI learning