LQR

Implemented more LQR code