AP_Math: added quaternion normalization
AP_NavEKF: use quaternion normalize and cleanup sqrt calls