Added rotation matrix based vector controller

This is a rotation matrix based vector controller implementing this:

http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf

Its test-flown using Matlab/Simulink Coder, but not in this formulation, which is a direct port. Note that the rate controllers in here would need to be improved (preferably with a C++ PID library), plain P controllers are not so nice (probably needs PI).

Still, this gets us a state of the art controller even suitable for acrobatic flight.