PID control implemented
Used LQR to weight P-terms
Merge branch 'master' of github.com:davetcoleman/matlab_simulations