Updates to the Lee Controller - Part 1

[jglane]

This is part 1 in my attempt to improve the Lee controller. These changes correspond to the first paragraph of my larger PR #1494 and address the following:

1. Implemented the true integral errors as seen in [2].
2. Fixed an issue in the torque calculation, now the implementation is exactly like [2] if the desired angular velocity was set to zero.
3. Some refactoring to hopefully make it more readable and easy to follow from the papers.

Both position and manual control were tested after each commit to make sure they still worked and had the intended effect. Next steps are to add approximation calculations as described in #1494 to estimate the desired angular velocity on the fly.

@whoenig @ToveRumar @KhWahba

[1] Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
Control of Complex Maneuvers for a Quadrotor UAV using Geometric Methods on SE(3)
CDC 2010, updated on arXiv 2011
https://arxiv.org/pdf/1003.2005

[2] Farhad Goodarzi, Daewon Lee, Taeyoung Lee
Geometric Nonlinear PID Control of a Quadrotor UAV on SE(3)
ECC 2013
https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=6669644

[jglane]

@gemenerik @whoenig any updates on this?

[whoenig]

Adding @KhWahba