Updated 9Dof with working compass