6DOF attitude calculation