I have been using Python exclusively for my scientific computing for about half a year (having been frustrated by Matlab’s awkward syntax for complex programs, and annoying licensing). One area where I struggled was to compute steady state LQR controllers.
I’ve learnt that SciPy offers a Ricatti equation solver (`scipy.linalg.solve_continuous_are` and `scipy.linalg.solve_discrete_are`). Since solving the Ricatti equation is the hard part of solving for an LQR gain, this implies that one can compute infinite horizon LQR controllers straight-forwardly using only SciPy.linalg. Similarly, one can compute steady state Kalman filters.
Below are my wrapper functions for continuous and discrete time LQR controllers. The equations come from Bertsekas “Dynamic Programming and Optimal Control”. I’ve done some basic sanity checks, and it seems to work.
from __future__ import division, print_function
import numpy as np
import scipy.linalg
def lqr(A,B,Q,R):
"""Solve the continuous time lqr controller.
dx/dt = A x + B u
cost = integral x.T*Q*x + u.T*R*u
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R))
#compute the LQR gain
K = np.matrix(scipy.linalg.inv(R)*(B.T*X))
eigVals, eigVecs = scipy.linalg.eig(A-B*K)
return K, X, eigVals
def dlqr(A,B,Q,R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R))
#compute the LQR gain
K = np.matrix(scipy.linalg.inv(B.T*X*B+R)*(B.T*X*A))
eigVals, eigVecs = scipy.linalg.eig(A-B*K)
return K, X, eigVals
An alternative method is to use the Python Control toolbox. This offers a Matlab-like syntax for a variety of control methods. The downside is that it requires Slycot library, which is annoying to get running on Windows. It also does not appear to support discrete time systems.