Fixed various compilation and code integration problems, as well as implementing the Quaternion-based control algorithms.