Changed sim to accurately calculate rotor moments

The sim was previously using some weird global coordinates that I think
weren't working with how Simulink samples stuff. Now I need to make the
simulink model look pretty again
Finally got the sim working reasonably well.

My problem was that I was using vectors for the control gains and not
Matrices :/