Tuning LQR

Changed some things in the structure as well