Changes to controller, uses LQR instead of pole placement, but still linearized. Full simulation trajectory following now
Changes to controller, uses LQR instead of pole placement, but still linearized. Full simulation trajectory following now