def bb_dare(A, B, Q, R):
"""Solve Riccati equation for discrete time systems
Usage
=====
[K, S, E] = care(A, B, Q, R)
Inputs
------
A, B: 2-d arrays with dynamics and input matrices
sys: linear I/O system
Q, R: 2-d array with state and input weight matrices
Outputs
-------
X: solution of the Riccati eq.
"""
# 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):
raise ControlDimension("incorrect weighting matrix dimensions")
X,rcond,w,S,T = \
sb02od(nstates, ninputs, A, B, Q, R, 'D')
return X
def kalman(A, B, C, Q, R):
"""Solves for the steady state kalman gain and covariance matricies.
Args:
A, B, C: SS matricies.
Q: The model uncertantity
R: The measurement uncertainty
Returns:
KalmanGain, Covariance.
"""
I = numpy.matrix(numpy.eye(Q.shape[0]))
Z = numpy.matrix(numpy.zeros(Q.shape[0]))
n = A.shape[0]
m = C.shape[0]
controllability_rank = numpy.linalg.matrix_rank(ctrb(A.T, C.T))
if controllability_rank != n:
glog.warning('Observability of %d != %d, unobservable state',
controllability_rank, n)
# Compute the steady state covariance matrix.
P_prior, rcond, w, S, T = slycot.sb02od(n=n,
m=m,
A=A.T,
B=C.T,
Q=Q,
R=R,
dico='D')
S = C * P_prior * C.T + R
K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
P = (I - K * C) * P_prior
return K, P
def dare(A,B,Q,R):
"""Solve Riccati equation for discrete time systems
Usage
=====
[K, S, E] = care(A, B, Q, R)
Inputs
------
A, B: 2-d arrays with dynamics and input matrices
sys: linear I/O system
Q, R: 2-d array with state and input weight matrices
Outputs
-------
X: solution of the Riccati eq.
"""
# 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) :
raise ControlDimension("incorrect weighting matrix dimensions")
X,rcond,w,S,T = \
sb02od(nstates, ninputs, A, B, Q, R, 'D');
return X
def kalman(A, B, C, Q, R):
"""Solves for the steady state kalman gain and covariance matricies.
Args:
A, B, C: SS matricies.
Q: The model uncertantity
R: The measurement uncertainty
Returns:
KalmanGain, Covariance.
"""
I = numpy.matrix(numpy.eye(Q.shape[0]))
Z = numpy.matrix(numpy.zeros(Q.shape[0]))
n = A.shape[0]
m = C.shape[0]
controllability_rank = numpy.linalg.matrix_rank(ctrb(A.T, C.T))
if controllability_rank != n:
glog.warning('Observability of %d != %d, unobservable state',
controllability_rank, n)
# Compute the steady state covariance matrix.
P_prior, rcond, w, S, T = slycot.sb02od(n=n, m=m, A=A.T, B=C.T, Q=Q, R=R, dico='D')
S = C * P_prior * C.T + R
K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
P = (I - K * C) * P_prior
return K, P
def sb02od_example():
from numpy import zeros, shape, dot, ones
A = array([[0, 1], [0, 0]])
B = array([[0], [1]])
C = array([[1, 0], [0, 1], [0, 0]])
Q = dot(C.T, C)
R = ones((1, 1))
out = slycot.sb02od(2, 1, A, B, Q, R, 'C')
print('--- Example for sb02od ...')
print('The solution X is')
print(out[0])
print('rcond =', out[1])
def dlqr(A, B, Q, R):
"""Solves for the optimal lqr controller.
x(n+1) = A * x(n) + B * u(n)
J = sum(0, inf, x.T * Q * x + u.T * R * u)
"""
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
P, rcond, w, S, T = slycot.sb02od(n=A.shape[0],m=B.shape[1],A=A,B=B,Q=Q,R=R,dico='D')
F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
return F
def dlqr(A, B, Q, R):
"""Solves for the optimal lqr controller.
x(n+1) = A * x(n) + B * u(n)
J = sum(0, inf, x.T * Q * x + u.T * R * u)
"""
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
P, rcond, w, S, T = slycot.sb02od(A.shape[0], B.shape[1], A, B, Q, R, 'D')
F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
return F
def sb02od_example():
from numpy import zeros, shape, dot, ones
A = array([ [0, 1],
[0, 0]])
B = array([ [0],
[1]])
C = array([ [1, 0],
[0, 1],
[0, 0]])
Q = dot(C.T,C)
R = ones((1,1))
out = slycot.sb02od(2,1,A,B,Q,R,'C')
print('--- Example for sb02od ...')
print('The solution X is')
print(out[0])
print('rcond =', out[1])
def solve_slycot(self):
"""Directly solves the DARE using the SLICOT library's SB02MD
implementation of a generalized Schur vectors method. If the Slycot
package is unavailable, a RuntimeError will be thrown. This solver
should be considerably more robust than the pure-Python implementation
in solve_direct(). Only minimal shape checking is performed.
More on SLICOT: http://www.slicot.org/
Python Interface (Slycot): https://github.com/avventi/Slycot
"""
if self.a.shape[0] != self.a.shape[1]:
raise ValueError('input "a" must be a square matrix')
try:
self.solution,rcond,w,s,t = slycot.sb02od(self.a.shape[1],self.b.shape[1],\
self.a, self.b, self.q, self.r,'D')
except NameError:
raise RuntimeError('SLICOT not available')
return self.solution
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 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)
#Prepare the inputs for calling Slicot
C = matrix( C )
Q = matrix( Q )
F = matrix( F )
D = matrix( D )
E = matrix( E )
QFT = array( Q*(F.T) )
#Solve the ARME
#Note: The definitions of n and m are reversed in this function
# Slycot solves: X = A'XA - (L + A'XB)(R + B'XB)^-1 (L+A'XB)' + Q
# While GRP solves M = M + C - ( MQF' - D )(E + FQ'MQF )^-1(MQF' - D)'
# Arguments (n, m, A, B, Q, R, dico, [p, L, fact, uplo, sort, tol, ldwork])
M, rcond, w, S, T = slycot.sb02od( m, n, eye(m), QFT, C, E, dico="D", L= -D )
# Compute the error to ensure that slycot solved the problem correctly
X = inv( E + F * ( Q.T ) * M * Q * ( F.T ) )
Y = M * Q * ( F.T ) - D
Mnew = M + C - Y * X * ( Y.T )
maxerror = 0.0
for i in range(0, m):
for j in range(0, m):
error = abs( ( M[i,j] - Mnew[i,j] ) / M[i,j] )
if error > maxerror:
maxerror = error
if maxerror > data["tolerance"]:
sys.stderr.write("Warning: ARME tolerance not met! Max rel error %s\n" % maxerror)
