LQR Controller

See this example for now.