LQR for trajectory tracking added