from .exception import ControlSlycot, ControlArgument, ControlDimension, \

slycot_check

from .statesp import _ssmatrix

# Make sure we have access to the right slycot routines

try:

m = Q.shape[0]

# Check to make sure input matrices are the right shape and type

_check_shape("A", A, n, n, square=True)

_check_shape(A, n, n, square=True, name="A")

# Solve standard Lyapunov equation

if C is None and E is None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, n, n, square=True, symmetric=True)

_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

if method == 'scipy':

# Solve the Lyapunov equation using SciPy

# Solve the Sylvester equation

elif C is not None and E is None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, m, m, square=True)

_check_shape("C", C, n, m)

_check_shape(Q, m, m, square=True, name="Q")

_check_shape(C, n, m, name="C")

if method == 'scipy':

# Solve the Sylvester equation using SciPy

# Solve the generalized Lyapunov equation

elif C is None and E is not None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, n, n, square=True, symmetric=True)

_check_shape("E", E, n, n, square=True)

_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

_check_shape(E, n, n, square=True, name="E")

if method == 'scipy':

raise ControlArgument(

else:

raise ControlArgument("Invalid set of input parameters")

return _ssmatrix(X)

return X

def dlyap(A, Q, C=None, E=None, method=None):

m = Q.shape[0]

# Check to make sure input matrices are the right shape and type

_check_shape("A", A, n, n, square=True)

_check_shape(A, n, n, square=True, name="A")

# Solve standard Lyapunov equation

if C is None and E is None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, n, n, square=True, symmetric=True)

_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

if method == 'scipy':

# Solve the Lyapunov equation using SciPy

# Solve the Sylvester equation

elif C is not None and E is None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, m, m, square=True)

_check_shape("C", C, n, m)

_check_shape(Q, m, m, square=True, name="Q")

_check_shape(C, n, m, name="C")

if method == 'scipy':

raise ControlArgument(

# Solve the generalized Lyapunov equation

elif C is None and E is not None:

# Check to make sure input matrices are the right shape and type

_check_shape("Q", Q, n, n, square=True, symmetric=True)

_check_shape("E", E, n, n, square=True)

_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

_check_shape(E, n, n, square=True, name="E")

if method == 'scipy':

raise ControlArgument(

else:

raise ControlArgument("Invalid set of input parameters")

return _ssmatrix(X)

return X

#

m = B.shape[1]

# Check to make sure input matrices are the right shape and type

_check_shape(_As, A, n, n, square=True)

_check_shape(_Bs, B, n, m)

_check_shape(_Qs, Q, n, n, square=True, symmetric=True)

_check_shape(_Rs, R, m, m, square=True, symmetric=True)

_check_shape(A, n, n, square=True, name=_As)

_check_shape(B, n, m, name=_Bs)

_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)

_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)

# Solve the standard algebraic Riccati equation

if S is None and E is None:

X = sp.linalg.solve_continuous_are(A, B, Q, R)

K = np.linalg.solve(R, B.T @ X)

E, _ = np.linalg.eig(A - B @ K)

return _ssmatrix(X), E, _ssmatrix(K)

return X, E, K

# Make sure we can import required slycot routines

try:

# Return the solution X, the closed-loop eigenvalues L and

# the gain matrix G

return _ssmatrix(X), w[:n], _ssmatrix(G)

return X, w[:n], G

# Solve the generalized algebraic Riccati equation

else:

E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2)

# Check to make sure input matrices are the right shape and type

_check_shape(_Es, E, n, n, square=True)

_check_shape(_Ss, S, n, m)

_check_shape(E, n, n, square=True, name=_Es)

_check_shape(S, n, m, name=_Ss)

# See if we should solve this using SciPy

if method == 'scipy':

X = sp.linalg.solve_continuous_are(A, B, Q, R, s=S, e=E)

K = np.linalg.solve(R, B.T @ X @ E + S.T)

eigs, _ = sp.linalg.eig(A - B @ K, E)

return _ssmatrix(X), eigs, _ssmatrix(K)

return X, eigs, K

# Make sure we can find the required slycot routine

try:

# Return the solution X, the closed-loop eigenvalues L and

# the gain matrix G

return _ssmatrix(X), L, _ssmatrix(G)

return X, L, G

def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,

_As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"):

m = B.shape[1]

# Check to make sure input matrices are the right shape and type

_check_shape(_As, A, n, n, square=True)

_check_shape(_Bs, B, n, m)

_check_shape(_Qs, Q, n, n, square=True, symmetric=True)

_check_shape(_Rs, R, m, m, square=True, symmetric=True)

_check_shape(A, n, n, square=True, name=_As)

_check_shape(B, n, m, name=_Bs)

_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)

_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)

if E is not None:

_check_shape(_Es, E, n, n, square=True)

_check_shape(E, n, n, square=True, name=_Es)

if S is not None:

_check_shape(_Ss, S, n, m)

_check_shape(S, n, m, name=_Ss)

# Figure out how to solve the problem

if method == 'scipy':

else:

L, _ = sp.linalg.eig(A - B @ G, E)

return _ssmatrix(X), L, _ssmatrix(G)

return X, L, G

# Make sure we can import required slycot routine

try:

# Return the solution X, the closed-loop eigenvalues L and

# the gain matrix G

return _ssmatrix(X), L, _ssmatrix(G)

return X, L, G

# Utility function to decide on method to use

# Utility function to check matrix dimensions

def _check_shape(name, M, n, m, square=False, symmetric=False):

def _check_shape(M, n, m, square=False, symmetric=False, name="??"):

if square and M.shape[0] != M.shape[1]:

raise ControlDimension("%s must be a square matrix" % name)

if symmetric and not _is_symmetric(M):

raise ControlArgument("%s must be a symmetric matrix" % name)

if M.shape[0] != n or M.shape[1] != m:

raise ControlDimension("Incompatible dimensions of %s matrix" % name)

raise ControlDimension(

f"Incompatible dimensions of {name} matrix; "

f"expected ({n}, {m}) but found {M.shape}")

# Utility function to check if a matrix is symmetric