def sb02md_example():
A = array([[0, 1], [0, 0]])
Q = array([[1, 0], [0, 2]])
G = array([[0, 0], [0, 1]])
out = slycot.sb02md(2, A, G, Q, 'C')
print('--- Example for sb02md ---')
print('The solution X is')
print(out[0])
print('rcond =', out[1])
def sb02md_example():
A = array([ [0, 1],
[0, 0]])
Q = array([ [1, 0],
[0, 2]])
G = array([ [0, 0],
[0, 1]])
out = slycot.sb02md(2,A,G,Q,'C')
print('--- Example for sb02md ---')
print('The solution X is')
print(out[0])
print('rcond =', out[1])
def __init__(self, q, qd, tunings=None):
super(LQR_State, self).__init__(q, qd, tunings)
self.x = numpy.hstack([q, qd])
q_cost = 1. / numpy.pi
qd_cost = 1. / tunings.Vmax
torque_lim = array(tunings.torque_limits)
Q = diag([q_cost] * 2 + [qd_cost] * 2)
R_inv = diag(torque_lim) / tunings.lqr_torque_weight
df = pendulum.df(q, qd, tau=numpy.array([0, 0]))
A = zeros((4, 4)) # d(f_state)/d(q, qd)
A[0:2, 2:4] = eye(2)
A[2:4, 0:4] = df[:, 0:4]
B = zeros((4, 2)) # d(f_state)/d(tau)
B[2:4, 0:2] = df[:, 4:]
dico = 'C' # continuous Riccati equation
n = 4 # order of A, Q, G and (the resulting) S
G = dot(B, dot(R_inv, B.transpose()))
try:
self.S, _, _, _, _, _ = sb02md(n, A, G, Q, dico)
self.minus_K = - dot(R_inv, dot(B.transpose(), self.S))
except:
print "Ooops! Debug info:"
print "q =", q
print "qd =", qd
print "A ="
print A
print "G ="
print G
print "Q ="
print Q
raise
if False:
print "S ="
print self.S
print "K ="
print self.K
def dlqr(*args, **keywords):
"""Linear quadratic regulator design
The lqr() function computes the optimal state feedback controller
that minimizes the quadratic cost
.. math:: J = \int_0^\infty x' Q x + u' R u + 2 x' N u
The function can be called with either 3, 4, or 5 arguments:
* ``lqr(sys, Q, R)``
* ``lqr(sys, Q, R, N)``
* ``lqr(A, B, Q, R)``
* ``lqr(A, B, Q, R, N)``
Parameters
----------
A, B: 2-d array
Dynamics and input matrices
sys: Lti (StateSpace or TransferFunction)
Linear I/O system
Q, R: 2-d array
State and input weight matrices
N: 2-d array, optional
Cross weight matrix
Returns
-------
K: 2-d array
State feedback gains
S: 2-d array
Solution to Riccati equation
E: 1-d array
Eigenvalues of the closed loop system
Examples
--------
>>> K, S, E = lqr(sys, Q, R, [N])
>>> K, S, E = lqr(A, B, Q, R, [N])
"""
# Make sure that SLICOT is installed
try:
from slycot import sb02md
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")
#
# Process the arguments and figure out what inputs we received
#
# Get the system description
if (len(args) < 4):
raise ControlArgument("not enough input arguments")
elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
#! TODO: really just need to check for A and B attributes
A = np.array(args[0].A, ndmin=2, dtype=float)
B = np.array(args[0].B, ndmin=2, dtype=float)
index = 1
else:
# Arguments should be A and B matrices
A = np.array(args[0], ndmin=2, dtype=float)
B = np.array(args[1], ndmin=2, dtype=float)
index = 2
# Get the weighting matrices (converting to matrices, if needed)
Q = np.array(args[index], ndmin=2, dtype=float)
R = np.array(args[index + 1], ndmin=2, dtype=float)
if (len(args) > index + 2):
N = np.array(args[index + 2], ndmin=2, dtype=float)
else:
N = np.zeros((Q.shape[0], R.shape[1]))
# Check dimensions for consistency
nstates = B.shape[0]
ninputs = B.shape[1]
if (A.shape[0] != nstates or A.shape[1] != nstates):
raise ControlDimension("inconsistent system dimensions")
elif (Q.shape[0] != nstates or Q.shape[1] != nstates
or R.shape[0] != ninputs or R.shape[1] != ninputs
or N.shape[0] != nstates or N.shape[1] != ninputs):
raise ControlDimension("incorrect weighting matrix dimensions")
# Compute the G matrix required by SB02MD
A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = \
sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')
# Call the SLICOT function
X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'D')
# Now compute the return value
K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T))
S = X
E = w[0:nstates]
return K, S, E
def lqr(*args, **keywords):
"""lqr(A, B, Q, R[, N])
Linear quadratic regulator design
The lqr() function computes the optimal state feedback controller
that minimizes the quadratic cost
.. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt
The function can be called with either 3, 4, or 5 arguments:
* ``lqr(sys, Q, R)``
* ``lqr(sys, Q, R, N)``
* ``lqr(A, B, Q, R)``
* ``lqr(A, B, Q, R, N)``
where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
2d arrays or matrices of appropriate dimension.
Parameters
----------
A, B : 2D array
Dynamics and input matrices
sys : LTI (StateSpace or TransferFunction)
Linear I/O system
Q, R : 2D array
State and input weight matrices
N : 2D array, optional
Cross weight matrix
Returns
-------
K : 2D array (or matrix)
State feedback gains
S : 2D array (or matrix)
Solution to Riccati equation
E : 1D array
Eigenvalues of the closed loop system
See Also
--------
lqe
Notes
-----
The return type for 2D arrays depends on the default class set for
state space operations. See :func:`~control.use_numpy_matrix`.
Examples
--------
>>> K, S, E = lqr(sys, Q, R, [N])
>>> K, S, E = lqr(A, B, Q, R, [N])
"""
# Make sure that SLICOT is installed
try:
from slycot import sb02md
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")
#
# Process the arguments and figure out what inputs we received
#
# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")
try:
# If this works, we were (probably) passed a system as the
# first argument; extract A and B
A = np.array(args[0].A, ndmin=2, dtype=float)
B = np.array(args[0].B, ndmin=2, dtype=float)
index = 1
except AttributeError:
# Arguments should be A and B matrices
A = np.array(args[0], ndmin=2, dtype=float)
B = np.array(args[1], ndmin=2, dtype=float)
index = 2
# Get the weighting matrices (converting to matrices, if needed)
Q = np.array(args[index], ndmin=2, dtype=float)
R = np.array(args[index + 1], ndmin=2, dtype=float)
if (len(args) > index + 2):
N = np.array(args[index + 2], ndmin=2, dtype=float)
else:
N = np.zeros((Q.shape[0], R.shape[1]))
# Check dimensions for consistency
nstates = B.shape[0]
ninputs = B.shape[1]
if (A.shape[0] != nstates or A.shape[1] != nstates):
raise ControlDimension("inconsistent system dimensions")
elif (Q.shape[0] != nstates or Q.shape[1] != nstates
or R.shape[0] != ninputs or R.shape[1] != ninputs
or N.shape[0] != nstates or N.shape[1] != ninputs):
raise ControlDimension("incorrect weighting matrix dimensions")
# Compute the G matrix required by SB02MD
A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = \
sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')
# Call the SLICOT function
X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'C')
# Now compute the return value
# We assume that R is positive definite and, hence, invertible
K = np.linalg.solve(R, np.dot(B.T, X) + N.T)
S = X
E = w[0:nstates]
return _ssmatrix(K), _ssmatrix(S), E
def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
"""Compute an approximate low-rank solution of a Riccati equation.
See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a
general description.
This function uses `slycot.sb02md` (if E and S are `None`),
`slycot.sb02od` (if E is `None` and S is not `None`) and
`slycot.sg03ad` (if E is not `None`), which are dense solvers.
Therefore, we assume all |Operators| and |VectorArrays| can be
converted to |NumPy arrays| using
:func:`~pymor.algorithms.to_matrix.to_matrix` and
:func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.
Parameters
----------
A
The |Operator| A.
E
The |Operator| E or `None`.
B
The operator B as a |VectorArray| from `A.source`.
C
The operator C as a |VectorArray| from `A.source`.
R
The operator R as a 2D |NumPy array| or `None`.
S
The operator S as a |VectorArray| from `A.source` or `None`.
trans
Whether the first |Operator| in the Riccati equation is
transposed.
options
The solver options to use (see
:func:`ricc_lrcf_solver_options`).
Returns
-------
Z
Low-rank Cholesky factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, C, R, S, trans)
options = _parse_options(options, ricc_lrcf_solver_options(), 'slycot',
None, False)
if options['type'] != 'slycot':
raise ValueError(
f"Unexpected Riccati equation solver ({options['type']}).")
A_source = A.source
A = to_matrix(A, format='dense')
E = to_matrix(E, format='dense') if E else None
B = B.to_numpy().T
C = C.to_numpy()
S = S.to_numpy().T if S else None
n = A.shape[0]
dico = 'C'
if E is None:
if S is None:
if not trans:
A = A.T
G = C.T.dot(C) if R is None else slycot.sb02mt(
n, C.shape[0], C.T, R)[-1]
else:
G = B.dot(B.T) if R is None else slycot.sb02mt(
n, B.shape[1], B, R)[-1]
Q = B.dot(B.T) if not trans else C.T.dot(C)
X, rcond = slycot.sb02md(n, A, G, Q, dico)[:2]
_ricc_rcond_check('slycot.sb02md', rcond)
else:
m = C.shape[0] if not trans else B.shape[1]
p = B.shape[1] if not trans else C.shape[0]
if R is None:
R = np.eye(m)
if not trans:
A = A.T
B, C = C.T, B.T
X, rcond = slycot.sb02od(n,
m,
A,
B,
C,
R,
dico,
p=p,
L=S,
fact='C')[:2]
_ricc_rcond_check('slycot.sb02od', rcond)
else:
jobb = 'B'
fact = 'C'
uplo = 'U'
jobl = 'Z' if S is None else 'N'
scal = 'N'
sort = 'S'
acc = 'R'
m = C.shape[0] if not trans else B.shape[1]
p = B.shape[1] if not trans else C.shape[0]
if R is None:
R = np.eye(m)
if S is None:
S = np.empty((n, m))
if not trans:
A = A.T
E = E.T
B, C = C.T, B.T
out = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc,
n, m, p, A, E, B, C, R, S)
X = out[1]
rcond = out[0]
_ricc_rcond_check('slycot.sg02ad', rcond)
return A_source.from_numpy(_chol(X).T)
def dare(A,B,Q,R,S=None,E=None):
""" (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati
equation
A^T X A - X - A^T X B (B^T X B + R)^-1 B^T X A + Q = 0
where A and Q are square matrices of the same dimension. Further, Q
is a symmetric matrix. The function returns the solution X, the gain
matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L,
i.e., the eigenvalues of A - B G.
(X,L,G) = dare(A,B,Q,R,S,E) solves the generalized discrete-time algebraic
Riccati equation
A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^-1 (B^T X A + S^T) +
+ Q = 0
where A, Q and E are square matrices of the same dimension. Further, Q and
R are symmetric matrices. The function returns the solution X, the gain
matrix G = (B^T X B + R)^-1 (B^T X A + S^T) and the closed loop
eigenvalues L, i.e., the eigenvalues of A - B G , E. """
# Make sure we can import required slycot routine
try:
from slycot import sb02md
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md'")
try:
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02mt'")
# Make sure we can find the required slycot routine
try:
from slycot import sg02ad
except ImportError:
raise ControlSlycot("can't find slycot module 'sg02ad'")
# Reshape 1-d arrays
if len(shape(A)) == 1:
A = A.reshape(1,A.size)
if len(shape(B)) == 1:
B = B.reshape(1,B.size)
if len(shape(Q)) == 1:
Q = Q.reshape(1,Q.size)
if R != None and len(shape(R)) == 1:
R = R.reshape(1,R.size)
if S != None and len(shape(S)) == 1:
S = S.reshape(1,S.size)
if E != None and len(shape(E)) == 1:
E = E.reshape(1,E.size)
# Determine main dimensions
if size(A) == 1:
n = 1
else:
n = size(A,0)
if size(B) == 1:
m = 1
else:
m = size(B,1)
# Solve the standard algebraic Riccati equation
if S==None and E==None:
# Check input data for consistency
if size(A) > 1 and shape(A)[0] != shape(A)[1]:
raise ControlArgument("A must be a quadratic matrix.")
if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
(size(Q) > 1 and shape(Q)[0] != n) or \
size(Q) == 1 and n > 1:
raise ControlArgument("Q must be a quadratic matrix of the same \
dimension as A.")
if (size(B) > 1 and shape(B)[0] != n) or \
size(B) == 1 and n > 1:
raise ControlArgument("Incompatible dimensions of B matrix.")
if not (asarray(Q) == asarray(Q).T).all():
raise ControlArgument("Q must be a symmetric matrix.")
if not (asarray(R) == asarray(R).T).all():
raise ControlArgument("R must be a symmetric matrix.")
# Create back-up of arrays needed for later computations
A_ba = copy(A)
R_ba = copy(R)
B_ba = copy(B)
# Solve the standard algebraic Riccati equation by calling Slycot
# functions sb02mt and sb02md
try:
A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = sb02mt(n,m,B,R)
except ValueError(ve):
if ve.info < 0:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == m+1:
e = ValueError("The matrix R is numerically singular.")
e.info = ve.info
else:
e = ValueError("The %i-th element of d in the UdU (LdL) \
factorization is zero." % ve.info)
e.info = ve.info
raise e
try:
X,rcond,w,S,U,A_inv = sb02md(n,A,G,Q,'D')
except ValueError(ve):
if ve.info < 0 or ve.info > 5:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == 1:
e = ValueError("The matrix A is (numerically) singular in \
discrete-time case.")
e.info = ve.info
elif ve.info == 2:
e = ValueError("The Hamiltonian or symplectic matrix H cannot \
be reduced to real Schur form.")
e.info = ve.info
elif ve.info == 3:
e = ValueError("The real Schur form of the Hamiltonian or \
symplectic matrix H cannot be appropriately ordered.")
e.info = ve.info
elif ve.info == 4:
e = ValueError("The Hamiltonian or symplectic matrix H has \
less than n stable eigenvalues.")
e.info = ve.info
elif ve.info == 5:
e = ValueError("The N-th order system of linear algebraic \
equations is singular to working precision.")
e.info = ve.info
raise e
# Calculate the gain matrix G
if size(R_b) == 1:
G = dot( 1/(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \
dot(asarray(B_ba).T,dot(X,A_ba)) )
else:
G = dot( inv(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \
dot(asarray(B_ba).T,dot(X,A_ba)) )
# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return (X , w[:n] , G)
# Solve the generalized algebraic Riccati equation
elif S != None and E != None:
# Check input data for consistency
if size(A) > 1 and shape(A)[0] != shape(A)[1]:
raise ControlArgument("A must be a quadratic matrix.")
if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
(size(Q) > 1 and shape(Q)[0] != n) or \
size(Q) == 1 and n > 1:
raise ControlArgument("Q must be a quadratic matrix of the same \
dimension as A.")
if (size(B) > 1 and shape(B)[0] != n) or \
size(B) == 1 and n > 1:
raise ControlArgument("Incompatible dimensions of B matrix.")
if (size(E) > 1 and shape(E)[0] != shape(E)[1]) or \
(size(E) > 1 and shape(E)[0] != n) or \
size(E) == 1 and n > 1:
raise ControlArgument("E must be a quadratic matrix of the same \
dimension as A.")
if (size(R) > 1 and shape(R)[0] != shape(R)[1]) or \
(size(R) > 1 and shape(R)[0] != m) or \
size(R) == 1 and m > 1:
raise ControlArgument("R must be a quadratic matrix of the same \
dimension as the number of columns in the B matrix.")
if (size(S) > 1 and shape(S)[0] != n) or \
(size(S) > 1 and shape(S)[1] != m) or \
size(S) == 1 and n > 1 or \
size(S) == 1 and m > 1:
raise ControlArgument("Incompatible dimensions of S matrix.")
if not (asarray(Q) == asarray(Q).T).all():
raise ControlArgument("Q must be a symmetric matrix.")
if not (asarray(R) == asarray(R).T).all():
raise ControlArgument("R must be a symmetric matrix.")
# Create back-up of arrays needed for later computations
A_b = copy(A)
R_b = copy(R)
B_b = copy(B)
E_b = copy(E)
S_b = copy(S)
# Solve the generalized algebraic Riccati equation by calling the
# Slycot function sg02ad
try:
rcondu,X,alfar,alfai,beta,S_o,T,U,iwarn = \
sg02ad('D','B','N','U','N','N','S','R',n,m,0,A,E,B,Q,R,S)
except ValueError(ve):
if ve.info < 0 or ve.info > 7:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == 1:
e = ValueError("The computed extended matrix pencil is \
singular, possibly due to rounding errors.")
e.info = ve.info
elif ve.info == 2:
e = ValueError("The QZ algorithm failed.")
e.info = ve.info
elif ve.info == 3:
e = ValueError("Reordering of the generalized eigenvalues \
failed.")
e.info = ve.info
elif ve.info == 4:
e = ValueError("After reordering, roundoff changed values of \
some complex eigenvalues so that leading \
eigenvalues in the generalized Schur form no \
longer satisfy the stability condition; this \
could also be caused due to scaling.")
e.info = ve.info
elif ve.info == 5:
e = ValueError("The computed dimension of the solution does \
not equal N.")
e.info = ve.info
elif ve.info == 6:
e = ValueError("The spectrum is too close to the boundary of \
the stability domain.")
e.info = ve.info
elif ve.info == 7:
e = ValueError("A singular matrix was encountered during the \
computation of the solution matrix X.")
e.info = ve.info
raise e
L = zeros((n,1))
L.dtype = 'complex64'
for i in range(n):
L[i] = (alfar[i] + alfai[i]*1j)/beta[i]
# Calculate the gain matrix G
if size(R_b) == 1:
G = dot( 1/(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \
dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T)
else:
G = dot( inv(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \
dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T)
# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return (X , L , G)
# Invalid set of input parameters
else:
raise ControlArgument("Invalid set of input parameters.")
def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
"""Compute an approximate low-rank solution of a Riccati equation.
See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a
general description.
This function uses `slycot.sb02md` (if E and S are `None`),
`slycot.sb02od` (if E is `None` and S is not `None`) and
`slycot.sg03ad` (if E is not `None`), which are dense solvers.
Therefore, we assume all |Operators| and |VectorArrays| can be
converted to |NumPy arrays| using
:func:`~pymor.algorithms.to_matrix.to_matrix` and
:func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.
Parameters
----------
A
The |Operator| A.
E
The |Operator| E or `None`.
B
The operator B as a |VectorArray| from `A.source`.
C
The operator C as a |VectorArray| from `A.source`.
R
The operator R as a 2D |NumPy array| or `None`.
S
The operator S as a |VectorArray| from `A.source` or `None`.
trans
Whether the first |Operator| in the Riccati equation is
transposed.
options
The solver options to use (see
:func:`ricc_lrcf_solver_options`).
Returns
-------
Z
Low-rank Cholesky factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, C, R, S, trans)
options = _parse_options(options, ricc_lrcf_solver_options(), 'slycot', None, False)
if options['type'] != 'slycot':
raise ValueError(f"Unexpected Riccati equation solver ({options['type']}).")
A_source = A.source
A = to_matrix(A, format='dense')
E = to_matrix(E, format='dense') if E else None
B = B.to_numpy().T
C = C.to_numpy()
S = S.to_numpy().T if S else None
n = A.shape[0]
dico = 'C'
if E is None:
if S is None:
if not trans:
A = A.T
G = C.T.dot(C) if R is None else slycot.sb02mt(n, C.shape[0], C.T, R)[-1]
else:
G = B.dot(B.T) if R is None else slycot.sb02mt(n, B.shape[1], B, R)[-1]
Q = B.dot(B.T) if not trans else C.T.dot(C)
X, rcond = slycot.sb02md(n, A, G, Q, dico)[:2]
_ricc_rcond_check('slycot.sb02md', rcond)
else:
m = C.shape[0] if not trans else B.shape[1]
p = B.shape[1] if not trans else C.shape[0]
if R is None:
R = np.eye(m)
if not trans:
A = A.T
B, C = C.T, B.T
X, rcond = slycot.sb02od(n, m, A, B, C, R, dico, p=p, L=S, fact='C')[:2]
_ricc_rcond_check('slycot.sb02od', rcond)
else:
jobb = 'B'
fact = 'C'
uplo = 'U'
jobl = 'Z' if S is None else 'N'
scal = 'N'
sort = 'S'
acc = 'R'
m = C.shape[0] if not trans else B.shape[1]
p = B.shape[1] if not trans else C.shape[0]
if R is None:
R = np.eye(m)
if S is None:
S = np.empty((n, m))
if not trans:
A = A.T
E = E.T
B, C = C.T, B.T
out = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc,
n, m, p,
A, E, B, C, R, S)
X = out[1]
rcond = out[0]
_ricc_rcond_check('slycot.sg02ad', rcond)
return A_source.from_numpy(_chol(X).T)
def care(A, B, Q, R=None, S=None, E=None, stabilizing=True):
"""(X, L, G) = care(A, B, Q, R=None) solves the continuous-time
algebraic Riccati equation
:math:`A^T X + X A - X B R^{-1} B^T X + Q = 0`
where A and Q are square matrices of the same dimension. Further,
Q and R are a symmetric matrices. If R is None, it is set to the
identity matrix. The function returns the solution X, the gain
matrix G = B^T X and the closed loop eigenvalues L, i.e., the
eigenvalues of A - B G.
(X, L, G) = care(A, B, Q, R, S, E) solves the generalized
continuous-time algebraic Riccati equation
:math:`A^T X E + E^T X A - (E^T X B + S) R^{-1} (B^T X E + S^T) + Q = 0`
where A, Q and E are square matrices of the same dimension. Further, Q
and R are symmetric matrices. If R is None, it is set to the identity
matrix. The function returns the solution X, the gain matrix G = R^-1
(B^T X E + S^T) and the closed loop eigenvalues L, i.e., the eigenvalues
of A - B G , E.
Parameters
----------
A, B, Q : 2D arrays
Input matrices for the Riccati equation
R, S, E : 2D arrays, optional
Input matrices for generalized Riccati equation
Returns
-------
X : 2D array (or matrix)
Solution to the Ricatti equation
L : 1D array
Closed loop eigenvalues
G : 2D array (or matrix)
Gain matrix
Notes
-----
The return type for 2D arrays depends on the default class set for
state space operations. See :func:`~control.use_numpy_matrix`.
"""
# Make sure we can import required slycot routine
try:
from slycot import sb02md
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md'")
try:
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02mt'")
# Make sure we can find the required slycot routine
try:
from slycot import sg02ad
except ImportError:
raise ControlSlycot("can't find slycot module 'sg02ad'")
# Reshape 1-d arrays
if len(shape(A)) == 1:
A = A.reshape(1, A.size)
if len(shape(B)) == 1:
B = B.reshape(1, B.size)
if len(shape(Q)) == 1:
Q = Q.reshape(1, Q.size)
if R is not None and len(shape(R)) == 1:
R = R.reshape(1, R.size)
if S is not None and len(shape(S)) == 1:
S = S.reshape(1, S.size)
if E is not None and len(shape(E)) == 1:
E = E.reshape(1, E.size)
# Determine main dimensions
if size(A) == 1:
n = 1
else:
n = size(A, 0)
if size(B) == 1:
m = 1
else:
m = size(B, 1)
if R is None:
R = eye(m, m)
# Solve the standard algebraic Riccati equation
if S is None and E is None:
# Check input data for consistency
if size(A) > 1 and shape(A)[0] != shape(A)[1]:
raise ControlArgument("A must be a quadratic matrix.")
if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
(size(Q) > 1 and shape(Q)[0] != n) or \
size(Q) == 1 and n > 1:
raise ControlArgument("Q must be a quadratic matrix of the same \
dimension as A.")
if (size(B) > 1 and shape(B)[0] != n) or \
size(B) == 1 and n > 1:
raise ControlArgument("Incompatible dimensions of B matrix.")
if not _is_symmetric(Q):
raise ControlArgument("Q must be a symmetric matrix.")
if not _is_symmetric(R):
raise ControlArgument("R must be a symmetric matrix.")
# Create back-up of arrays needed for later computations
R_ba = copy(R)
B_ba = copy(B)
# Solve the standard algebraic Riccati equation by calling Slycot
# functions sb02mt and sb02md
try:
A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = sb02mt(n, m, B, R)
except ValueError as ve:
if ve.info < 0:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == m + 1:
e = ValueError("The matrix R is numerically singular.")
e.info = ve.info
else:
e = ValueError("The %i-th element of d in the UdU (LdL) \
factorization is zero." % ve.info)
e.info = ve.info
raise e
try:
if stabilizing:
sort = 'S'
else:
sort = 'U'
X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort)
except ValueError as ve:
if ve.info < 0 or ve.info > 5:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == 1:
e = ValueError("The matrix A is (numerically) singular in \
continuous-time case.")
e.info = ve.info
elif ve.info == 2:
e = ValueError("The Hamiltonian or symplectic matrix H cannot \
be reduced to real Schur form.")
e.info = ve.info
elif ve.info == 3:
e = ValueError("The real Schur form of the Hamiltonian or \
symplectic matrix H cannot be appropriately ordered.")
e.info = ve.info
elif ve.info == 4:
e = ValueError("The Hamiltonian or symplectic matrix H has \
less than n stable eigenvalues.")
e.info = ve.info
elif ve.info == 5:
e = ValueError("The N-th order system of linear algebraic \
equations is singular to working precision.")
e.info = ve.info
raise e
# Calculate the gain matrix G
if size(R_b) == 1:
G = dot(dot(1 / (R_ba), asarray(B_ba).T), X)
else:
G = dot(solve(R_ba, asarray(B_ba).T), X)
# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return (_ssmatrix(X), w[:n], _ssmatrix(G))
# Solve the generalized algebraic Riccati equation
elif S is not None and E is not None:
# Check input data for consistency
if size(A) > 1 and shape(A)[0] != shape(A)[1]:
raise ControlArgument("A must be a quadratic matrix.")
if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
(size(Q) > 1 and shape(Q)[0] != n) or \
size(Q) == 1 and n > 1:
raise ControlArgument("Q must be a quadratic matrix of the same \
dimension as A.")
if (size(B) > 1 and shape(B)[0] != n) or \
size(B) == 1 and n > 1:
raise ControlArgument("Incompatible dimensions of B matrix.")
if (size(E) > 1 and shape(E)[0] != shape(E)[1]) or \
(size(E) > 1 and shape(E)[0] != n) or \
size(E) == 1 and n > 1:
raise ControlArgument("E must be a quadratic matrix of the same \
dimension as A.")
if (size(R) > 1 and shape(R)[0] != shape(R)[1]) or \
(size(R) > 1 and shape(R)[0] != m) or \
size(R) == 1 and m > 1:
raise ControlArgument("R must be a quadratic matrix of the same \
dimension as the number of columns in the B matrix.")
if (size(S) > 1 and shape(S)[0] != n) or \
(size(S) > 1 and shape(S)[1] != m) or \
size(S) == 1 and n > 1 or \
size(S) == 1 and m > 1:
raise ControlArgument("Incompatible dimensions of S matrix.")
if not _is_symmetric(Q):
raise ControlArgument("Q must be a symmetric matrix.")
if not _is_symmetric(R):
raise ControlArgument("R must be a symmetric matrix.")
# Create back-up of arrays needed for later computations
R_b = copy(R)
B_b = copy(B)
E_b = copy(E)
S_b = copy(S)
# Solve the generalized algebraic Riccati equation by calling the
# Slycot function sg02ad
try:
if stabilizing:
sort = 'S'
else:
sort = 'U'
rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \
sg02ad('C', 'B', 'N', 'U', 'N', 'N', sort,
'R', n, m, 0, A, E, B, Q, R, S)
except ValueError as ve:
if ve.info < 0 or ve.info > 7:
e = ValueError(ve.message)
e.info = ve.info
elif ve.info == 1:
e = ValueError("The computed extended matrix pencil is \
singular, possibly due to rounding errors.")
e.info = ve.info
elif ve.info == 2:
e = ValueError("The QZ algorithm failed.")
e.info = ve.info
elif ve.info == 3:
e = ValueError("Reordering of the generalized eigenvalues \
failed.")
e.info = ve.info
elif ve.info == 4:
e = ValueError("After reordering, roundoff changed values of \
some complex eigenvalues so that leading \
eigenvalues in the generalized Schur form no \
longer satisfy the stability condition; this \
could also be caused due to scaling.")
e.info = ve.info
elif ve.info == 5:
e = ValueError("The computed dimension of the solution does \
not equal N.")
e.info = ve.info
elif ve.info == 6:
e = ValueError("The spectrum is too close to the boundary of \
the stability domain.")
e.info = ve.info
elif ve.info == 7:
e = ValueError("A singular matrix was encountered during the \
computation of the solution matrix X.")
e.info = ve.info
raise e
# Calculate the closed-loop eigenvalues L
L = zeros((n, 1))
L.dtype = 'complex64'
for i in range(n):
L[i] = (alfar[i] + alfai[i] * 1j) / beta[i]
# Calculate the gain matrix G
if size(R_b) == 1:
G = dot(1 / (R_b),
dot(asarray(B_b).T, dot(X, E_b)) + asarray(S_b).T)
else:
G = solve(R_b, dot(asarray(B_b).T, dot(X, E_b)) + asarray(S_b).T)
# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return (_ssmatrix(X), L, _ssmatrix(G))
# Invalid set of input parameters
else:
raise ControlArgument("Invalid set of input parameters.")
def dlqr(*args, **keywords):
"""Linear quadratic regulator design
The lqr() function computes the optimal state feedback controller
that minimizes the quadratic cost
.. math:: J = \int_0^\infty x' Q x + u' R u + 2 x' N u
The function can be called with either 3, 4, or 5 arguments:
* ``lqr(sys, Q, R)``
* ``lqr(sys, Q, R, N)``
* ``lqr(A, B, Q, R)``
* ``lqr(A, B, Q, R, N)``
Parameters
----------
A, B: 2-d array
Dynamics and input matrices
sys: Lti (StateSpace or TransferFunction)
Linear I/O system
Q, R: 2-d array
State and input weight matrices
N: 2-d array, optional
Cross weight matrix
Returns
-------
K: 2-d array
State feedback gains
S: 2-d array
Solution to Riccati equation
E: 1-d array
Eigenvalues of the closed loop system
Examples
--------
>>> K, S, E = lqr(sys, Q, R, [N])
>>> K, S, E = lqr(A, B, Q, R, [N])
"""
# Make sure that SLICOT is installed
try:
from slycot import sb02md
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")
#
# Process the arguments and figure out what inputs we received
#
# Get the system description
if (len(args) < 4):
raise ControlArgument("not enough input arguments")
elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
#! TODO: really just need to check for A and B attributes
A = np.array(args[0].A, ndmin=2, dtype=float);
B = np.array(args[0].B, ndmin=2, dtype=float);
index = 1;
else:
# Arguments should be A and B matrices
A = np.array(args[0], ndmin=2, dtype=float);
B = np.array(args[1], ndmin=2, dtype=float);
index = 2;
# Get the weighting matrices (converting to matrices, if needed)
Q = np.array(args[index], ndmin=2, dtype=float);
R = np.array(args[index+1], ndmin=2, dtype=float);
if (len(args) > index + 2):
N = np.array(args[index+2], ndmin=2, dtype=float);
else:
N = np.zeros((Q.shape[0], R.shape[1]));
# Check dimensions for consistency
nstates = B.shape[0];
ninputs = B.shape[1];
if (A.shape[0] != nstates or A.shape[1] != nstates):
raise ControlDimension("inconsistent system dimensions")
elif (Q.shape[0] != nstates or Q.shape[1] != nstates or
R.shape[0] != ninputs or R.shape[1] != ninputs or
N.shape[0] != nstates or N.shape[1] != ninputs):
raise ControlDimension("incorrect weighting matrix dimensions")
# Compute the G matrix required by SB02MD
A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = \
sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N');
# Call the SLICOT function
X,rcond,w,S,U,A_inv = sb02md(nstates, A_b, G, Q_b, 'D')
# Now compute the return value
K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T));
S = X;
E = w[0:nstates];
return K, S, E
def solve_ricc(A, E=None, B=None, Q=None, C=None, R=None, G=None, trans=False, options=None):
"""Find a factor of the solution of a Riccati equation
Returns factor :math:`Z` such that :math:`Z Z^T` is
approximately the solution :math:`X` of a Riccati equation
.. math::
A^T X E + E^T X A - E^T X B R^{-1} B^T X E + Q = 0.
If E in `None`, it is taken to be the identity matrix.
Q can instead be given as C^T * C. In this case, Q needs to be
`None`, and C not `None`.
B * R^{-1} B^T can instead be given by G. In this case, B and R
need to be `None`, and G not `None`.
If R and G are `None`, then R is taken to be the identity
matrix.
If trans is `True`, then the dual Riccati equation is solved
.. math::
A X E^T + E X A^T - E X C^T R^{-1} C X E^T + Q = 0,
where Q can be replaced by B * B^T and C^T * R^{-1} * C by G.
This uses the `slycot` package, in particular its interfaces to
SLICOT functions `SB02MD` (for the standard Riccati equations)
and `SG02AD` (for the generalized Riccati equations).
These methods are only applicable to medium-sized dense
problems and need access to the matrix data of all operators.
Parameters
----------
A
The |Operator| A.
B
The |Operator| B or `None`.
E
The |Operator| E or `None`.
Q
The |Operator| Q or `None`.
C
The |Operator| C or `None`.
R
The |Operator| R or `None`.
G
The |Operator| G or `None`.
trans
If the dual equation needs to be solved.
options
The |solver_options| to use (see
:func:`ricc_solver_options`).
Returns
-------
Z
Low-rank factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, Q, C, R, G, trans)
options = _parse_options(options, ricc_solver_options(), 'slycot', None, False)
assert options['type'] == 'slycot'
import slycot
A_mat = to_matrix(A, format='dense')
B_mat = to_matrix(B, format='dense') if B else None
C_mat = to_matrix(C, format='dense') if C else None
R_mat = to_matrix(R, format='dense') if R else None
G_mat = to_matrix(G, format='dense') if G else None
Q_mat = to_matrix(Q, format='dense') if Q else None
n = A_mat.shape[0]
dico = 'C'
if E is None:
if not trans:
if G is None:
if R is None:
G_mat = B_mat.dot(B_mat.T)
else:
G_mat = slycot.sb02mt(n, B_mat.shape[1], B_mat, R_mat)[-1]
if C is not None:
Q_mat = C_mat.T.dot(C_mat)
X = slycot.sb02md(n, A_mat, G_mat, Q_mat, dico)[0]
else:
if G is None:
if R is None:
G_mat = C_mat.T.dot(C_mat)
else:
G_mat = slycot.sb02mt(n, C_mat.shape[0], C_mat.T, R_mat)[-1]
if B is not None:
Q_mat = B_mat.dot(B_mat.T)
X = slycot.sb02md(n, A_mat.T, G_mat, Q_mat, dico)[0]
else:
E_mat = to_matrix(E, format='dense') if E else None
jobb = 'B' if G is None else 'B'
fact = 'C' if Q is None else 'N'
uplo = 'U'
jobl = 'Z'
scal = 'N'
sort = 'S'
acc = 'R'
if not trans:
m = 0 if B is None else B_mat.shape[1]
p = 0 if C is None else C_mat.shape[0]
if G is not None:
B_mat = G_mat
R_mat = np.empty((1, 1))
elif R is None:
R_mat = np.eye(m)
if Q is None:
Q_mat = C_mat
L_mat = np.empty((n, m))
ret = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc, n, m, p,
A_mat, E_mat, B_mat, Q_mat, R_mat, L_mat)
else:
m = 0 if C is None else C_mat.shape[0]
p = 0 if B is None else B_mat.shape[1]
if G is not None:
C_mat = G_mat
R_mat = np.empty((1, 1))
elif R is None:
C_mat = C_mat.T
R_mat = np.eye(m)
else:
C_mat = C_mat.T
if Q is None:
Q_mat = B_mat.T
L_mat = np.empty((n, m))
ret = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc, n, m, p,
A_mat.T, E_mat.T, C_mat, Q_mat, R_mat, L_mat)
X = ret[1]
iwarn = ret[-1]
if iwarn == 1:
print('slycot.sg02ad warning: solution may be inaccurate.')
from pymor.bindings.scipy import chol
Z = chol(X, copy=False)
Z = A.source.from_numpy(np.array(Z).T)
return Z
