DONE EKF update, almousto DONE quaternion class (replace extern quaternion module)