diff --git a/control/exception.py b/control/exception.py index 9dde243af..e28ba8609 100644 --- a/control/exception.py +++ b/control/exception.py @@ -61,10 +61,13 @@ class ControlNotImplemented(NotImplementedError): pass # Utility function to see if slycot is installed +slycot_installed = None def slycot_check(): - try: - import slycot - except: - return False - else: - return True + global slycot_installed + if slycot_installed is None: + try: + import slycot + slycot_installed = True + except: + slycot_installed = False + return slycot_installed diff --git a/control/mateqn.py b/control/mateqn.py index a58194811..3a723591f 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -35,13 +35,15 @@ # OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF # SUCH DAMAGE. - import warnings +import numpy as np +from numpy import copy, eye, dot, finfo, inexact, atleast_2d + +import scipy as sp +from scipy.linalg import eigvals, solve -from numpy import shape, size, asarray, copy, zeros, eye, \ - finfo, inexact, atleast_2d -from scipy.linalg import eigvals, solve_discrete_are, solve -from .exception import ControlSlycot, ControlArgument +from .exception import ControlSlycot, ControlArgument, ControlDimension, \ + slycot_check from .statesp import _ssmatrix # Make sure we have access to the right slycot routines @@ -52,8 +54,9 @@ try: from slycot import sb03md57 + # wrap without the deprecation warning - def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): + def sb03md(n, C, A, U, dico, job='X', fact='N', trana='N', ldwork=None): ret = sb03md57(A, U, C, dico, job, fact, trana, ldwork) return ret[2:] except ImportError: @@ -84,13 +87,13 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): # -def lyap(A, Q, C=None, E=None): +def lyap(A, Q, C=None, E=None, method=None): """X = lyap(A, Q) solves the continuous-time Lyapunov equation :math:`A X + X A^T + Q = 0` - where A and Q are square matrices of the same dimension. - Further, Q must be symmetric. + where A and Q are square matrices of the same dimension. Q must be + symmetric. X = lyap(A, Q, C) solves the Sylvester equation @@ -103,74 +106,63 @@ def lyap(A, Q, C=None, E=None): :math:`A X E^T + E X A^T + Q = 0` - where Q is a symmetric matrix and A, Q and E are square matrices - of the same dimension. + where Q is a symmetric matrix and A, Q and E are square matrices of the + same dimension. Parameters ---------- - A : 2D array - Dynamics matrix - C : 2D array, optional - If present, solve the Slyvester equation - E : 2D array, optional - If present, solve the generalized Laypunov equation + A, Q : 2D array_like + Input matrices for the Lyapunov or Sylvestor equation + C : 2D array_like, optional + If present, solve the Sylvester equation + E : 2D array_like, optional + If present, solve the generalized Lyapunov equation + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- - Q : 2D array (or matrix) + X : 2D array (or matrix) Solution to the Lyapunov or Sylvester equation Notes ----- The return type for 2D arrays depends on the default class set for state space operations. See :func:`~control.use_numpy_matrix`. - """ - - if sb03md is None: - raise ControlSlycot("can't find slycot module 'sb03md'") - if sb04md is None: - raise ControlSlycot("can't find slycot module 'sb04md'") - - - # Reshape 1-d arrays - if len(shape(A)) == 1: - A = A.reshape(1, A.size) - - if len(shape(Q)) == 1: - Q = Q.reshape(1, Q.size) - if C is not None and len(shape(C)) == 1: - C = C.reshape(1, C.size) - - if E is not None and len(shape(E)) == 1: - E = E.reshape(1, E.size) + """ + # Decide what method to use + method = _slycot_or_scipy(method) + if method == 'slycot': + if sb03md is None: + raise ControlSlycot("Can't find slycot module 'sb03md'") + if sb04md is None: + raise ControlSlycot("Can't find slycot module 'sb04md'") + + # Reshape input arrays + A = np.array(A, ndmin=2) + Q = np.array(Q, ndmin=2) + if C is not None: + C = np.array(C, ndmin=2) + if E is not None: + E = np.array(E, ndmin=2) # Determine main dimensions - if size(A) == 1: - n = 1 - else: - n = size(A, 0) + n = A.shape[0] + m = Q.shape[0] - if size(Q) == 1: - m = 1 - else: - m = size(Q, 0) + # Check to make sure input matrices are the right shape and type + _check_shape("A", A, n, n, square=True) # Solve standard Lyapunov equation if C is None and E is None: - # Check input data for consistency - if shape(A) != shape(Q): - raise ControlArgument("A and Q must be matrices of identical \ - sizes.") - - if size(A) > 1 and shape(A)[0] != shape(A)[1]: - raise ControlArgument("A must be a quadratic matrix.") + # Check to make sure input matrices are the right shape and type + _check_shape("Q", Q, n, n, square=True, symmetric=True) - if size(Q) > 1 and shape(Q)[0] != shape(Q)[1]: - raise ControlArgument("Q must be a quadratic matrix.") - - if not _is_symmetric(Q): - raise ControlArgument("Q must be a symmetric matrix.") + if method == 'scipy': + return sp.linalg.solve_continuous_lyapunov(A, -Q) # Solve the Lyapunov equation by calling Slycot function sb03md with warnings.catch_warnings(): @@ -178,49 +170,34 @@ def lyap(A, Q, C=None, E=None): X, scale, sep, ferr, w = \ sb03md(n, -Q, A, eye(n, n), 'C', trana='T') - # Solve the Sylvester equation elif C is not 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.") + # 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) - if size(Q) > 1 and shape(Q)[0] != shape(Q)[1]: - raise ControlArgument("Q must be a quadratic matrix.") - - if (size(C) > 1 and shape(C)[0] != n) or \ - (size(C) > 1 and shape(C)[1] != m) or \ - (size(C) == 1 and size(A) != 1) or \ - (size(C) == 1 and size(Q) != 1): - raise ControlArgument("C matrix has incompatible dimensions.") + if method == 'scipy': + return sp.linalg.solve_sylvester(A, Q, -C) # Solve the Sylvester equation by calling the Slycot function sb04md X = sb04md(n, m, A, Q, -C) - # Solve the generalized Lyapunov equation elif C is None and E is not None: - # Check input data for consistency - 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 square matrix with the same \ - dimension as A.") - - 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 square matrix with the same \ - dimension as A.") - - if not _is_symmetric(Q): - raise ControlArgument("Q must be a symmetric matrix.") + # 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) + + if method == 'scipy': + raise ControlArgument( + "method='scipy' not valid for generalized Lyapunov equation") # Make sure we have access to the write slicot routine try: from slycot import sg03ad + except ImportError: - raise ControlSlycot("can't find slycot module 'sg03ad'") + raise ControlSlycot("Can't find slycot module 'sg03ad'") # Solve the generalized Lyapunov equation by calling Slycot # function sg03ad @@ -229,82 +206,94 @@ def lyap(A, Q, C=None, E=None): A, E, Q, Z, X, scale, sep, ferr, alphar, alphai, beta = \ sg03ad('C', 'B', 'N', 'T', 'L', n, A, E, eye(n, n), eye(n, n), -Q) - # Invalid set of input parameters + + # Invalid set of input parameters (C and E specified) else: raise ControlArgument("Invalid set of input parameters") return _ssmatrix(X) -def dlyap(A, Q, C=None, E=None): - """ dlyap(A,Q) solves the discrete-time Lyapunov equation +def dlyap(A, Q, C=None, E=None, method=None): + """dlyap(A, Q) solves the discrete-time Lyapunov equation :math:`A X A^T - X + Q = 0` where A and Q are square matrices of the same dimension. Further Q must be symmetric. - dlyap(A,Q,C) solves the Sylvester equation + dlyap(A, Q, C) solves the Sylvester equation :math:`A X Q^T - X + C = 0` where A and Q are square matrices. - dlyap(A,Q,None,E) solves the generalized discrete-time Lyapunov + dlyap(A, Q, None, E) solves the generalized discrete-time Lyapunov equation :math:`A X A^T - E X E^T + Q = 0` - where Q is a symmetric matrix and A, Q and E are square matrices - of the same dimension. """ + where Q is a symmetric matrix and A, Q and E are square matrices of the + same dimension. - # Make sure we have access to the right slycot routines - if sb03md is None: - raise ControlSlycot("can't find slycot module 'sb03md'") - if sb04qd is None: - raise ControlSlycot("can't find slycot module 'sb04qd'") - if sg03ad is None: - raise ControlSlycot("can't find slycot module 'sg03ad'") - - # Reshape 1-d arrays - if len(shape(A)) == 1: - A = A.reshape(1, A.size) + Parameters + ---------- + A, Q : 2D array_like + Input matrices for the Lyapunov or Sylvestor equation + C : 2D array_like, optional + If present, solve the Sylvester equation + E : 2D array_like, optional + If present, solve the generalized Lyapunov equation + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. - if len(shape(Q)) == 1: - Q = Q.reshape(1, Q.size) + Returns + ------- + X : 2D array (or matrix) + Solution to the Lyapunov or Sylvester equation - if C is not None and len(shape(C)) == 1: - C = C.reshape(1, C.size) + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. - if E is not None and len(shape(E)) == 1: - E = E.reshape(1, E.size) + """ + # Decide what method to use + method = _slycot_or_scipy(method) + + if method == 'slycot': + # Make sure we have access to the right slycot routines + if sb03md is None: + raise ControlSlycot("Can't find slycot module 'sb03md'") + if sb04qd is None: + raise ControlSlycot("Can't find slycot module 'sb04qd'") + if sg03ad is None: + raise ControlSlycot("Can't find slycot module 'sg03ad'") + + # Reshape input arrays + A = np.array(A, ndmin=2) + Q = np.array(Q, ndmin=2) + if C is not None: + C = np.array(C, ndmin=2) + if E is not None: + E = np.array(E, ndmin=2) # Determine main dimensions - if size(A) == 1: - n = 1 - else: - n = size(A, 0) + n = A.shape[0] + m = Q.shape[0] - if size(Q) == 1: - m = 1 - else: - m = size(Q, 0) + # Check to make sure input matrices are the right shape and type + _check_shape("A", A, n, n, square=True) # Solve standard Lyapunov equation if C is None and E is None: - # Check input data for consistency - if shape(A) != shape(Q): - raise ControlArgument("A and Q must be matrices of identical \ - sizes.") - - if size(A) > 1 and shape(A)[0] != shape(A)[1]: - raise ControlArgument("A must be a quadratic matrix.") + # Check to make sure input matrices are the right shape and type + _check_shape("Q", Q, n, n, square=True, symmetric=True) - if size(Q) > 1 and shape(Q)[0] != shape(Q)[1]: - raise ControlArgument("Q must be a quadratic matrix.") - - if not _is_symmetric(Q): - raise ControlArgument("Q must be a symmetric matrix.") + if method == 'scipy': + return sp.linalg.solve_discrete_lyapunov(A, Q) # Solve the Lyapunov equation by calling the Slycot function sb03md with warnings.catch_warnings(): @@ -314,38 +303,26 @@ def dlyap(A, Q, C=None, E=None): # Solve the Sylvester equation elif C is not 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]: - raise ControlArgument("Q must be a quadratic matrix") + # 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) - if (size(C) > 1 and shape(C)[0] != n) or \ - (size(C) > 1 and shape(C)[1] != m) or \ - (size(C) == 1 and size(A) != 1) or (size(C) == 1 and size(Q) != 1): - raise ControlArgument("C matrix has incompatible dimensions") + if method == 'scipy': + raise ControlArgument( + "method='scipy' not valid for Sylvester equation") # Solve the Sylvester equation by calling Slycot function sb04qd - X = sb04qd(n, m, -A, asarray(Q).T, C) + X = sb04qd(n, m, -A, Q.T, C) # Solve the generalized Lyapunov equation elif C is None and E is not None: - # Check input data for consistency - 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 square matrix with the same \ - dimension as A.") - - 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 square matrix with the same \ - dimension as A.") - - if not _is_symmetric(Q): - raise ControlArgument("Q must be a symmetric matrix.") + # 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) + + if method == 'scipy': + raise ControlArgument( + "method='scipy' not valid for generalized Lyapunov equation") # Solve the generalized Lyapunov equation by calling Slycot # function sg03ad @@ -354,7 +331,8 @@ def dlyap(A, Q, C=None, E=None): A, E, Q, Z, X, scale, sep, ferr, alphar, alphai, beta = \ sg03ad('D', 'B', 'N', 'T', 'L', n, A, E, eye(n, n), eye(n, n), -Q) - # Invalid set of input parameters + + # Invalid set of input parameters (C and E specified) else: raise ControlArgument("Invalid set of input parameters") @@ -365,9 +343,8 @@ def dlyap(A, Q, C=None, E=None): # Riccati equation solvers care and dare # - -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 +def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): + """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` @@ -378,7 +355,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): 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 + 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` @@ -391,10 +368,14 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): Parameters ---------- - A, B, Q : 2D arrays + A, B, Q : 2D array_like Input matrices for the Riccati equation - R, S, E : 2D arrays, optional + R, S, E : 2D array_like, optional Input matrices for generalized Riccati equation + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- @@ -411,77 +392,59 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): state space operations. See :func:`~control.use_numpy_matrix`. """ + # Decide what method to use + method = _slycot_or_scipy(method) - # 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 method == 'slycot': + # Make sure we can import required slycot routines + try: + from slycot import sb02md + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02md'") - if S is not None and len(shape(S)) == 1: - S = S.reshape(1, S.size) + try: + from slycot import sb02mt + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02mt'") - if E is not None and len(shape(E)) == 1: - E = E.reshape(1, E.size) + # 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 input arrays + A = np.array(A, ndmin=2) + B = np.array(B, ndmin=2) + Q = np.array(Q, ndmin=2) + R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2) + if S is not None: + S = np.array(S, ndmin=2) + if E is not None: + E = np.array(E, ndmin=2) # Determine main dimensions - if size(A) == 1: - n = 1 - else: - n = size(A, 0) + n = A.shape[0] + m = B.shape[1] - if size(B) == 1: - m = 1 - else: - m = size(B, 1) - if R is None: - R = eye(m, m) + # Check to make sure input matrices are the right shape and type + _check_shape("A", A, n, n, square=True) + _check_shape("B", B, n, m) + _check_shape("Q", Q, n, n, square=True, symmetric=True) + _check_shape("R", R, m, m, square=True, symmetric=True) # 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.") + # See if we should solve this using SciPy + if method == 'scipy': + if not stabilizing: + raise ControlArgument( + "method='scipy' not valid when stabilizing is not True") - if not _is_symmetric(R): - raise ControlArgument("R must be a symmetric matrix.") + 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) # Create back-up of arrays needed for later computations R_ba = copy(R) @@ -495,54 +458,32 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort) # Calculate the gain matrix G - if size(R_b) == 1: - G = 1/(R_ba) * asarray(B_ba).T @ X - else: - G = solve(R_ba, asarray(B_ba).T) @ X + G = solve(R_ba, 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)) + 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.") + else: + # Initialize optional matrices + S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2) + 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("E", E, n, n, square=True) + _check_shape("S", S, n, m) + + # See if we should solve this using SciPy + if method == 'scipy': + if not stabilizing: + raise ControlArgument( + "method='scipy' not valid when stabilizing is not True") + + 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) # Create back-up of arrays needed for later computations R_b = copy(R) @@ -560,27 +501,16 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): 'R', n, m, 0, A, E, B, Q, R, S) # 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] + L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) # Calculate the gain matrix G - if size(R_b) == 1: - G = 1/(R_b) * (asarray(B_b).T @ X @ E_b + asarray(S_b).T) - else: - G = solve(R_b, asarray(B_b).T @ X @ E_b + asarray(S_b).T) + G = solve(R_b, B_b.T @ X @ E_b + 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.") - + return _ssmatrix(X), L, _ssmatrix(G) -def dare(A, B, Q, R, S=None, E=None, stabilizing=True): +def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): """(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati equation @@ -607,6 +537,10 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True): Input matrices for the Riccati equation R, S, E : 2D arrays, optional Input matrices for generalized Riccati equation + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- @@ -623,191 +557,150 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True): state space operations. See :func:`~control.use_numpy_matrix`. """ - if S is not None or E is not None or not stabilizing: - return dare_old(A, B, Q, R, S, E, stabilizing) + # Decide what method to use + method = _slycot_or_scipy(method) + + # Reshape input arrays + A = np.array(A, ndmin=2) + B = np.array(B, ndmin=2) + Q = np.array(Q, ndmin=2) + R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2) + if S is not None: + S = np.array(S, ndmin=2) + if E is not None: + E = np.array(E, ndmin=2) + + # Determine main dimensions + n = A.shape[0] + m = B.shape[1] + + # Check to make sure input matrices are the right shape and type + _check_shape("A", A, n, n, square=True) + + # Figure out how to solve the problem + if method == 'scipy' and not stabilizing: + raise ControlArgument( + "method='scipy' not valid when stabilizing is not True") + + elif method == 'slycot': + return _dare_slycot(A, B, Q, R, S, E, stabilizing) + else: + _check_shape("B", B, n, m) + _check_shape("Q", Q, n, n, square=True, symmetric=True) + _check_shape("R", R, m, m, square=True, symmetric=True) + if E is not None: + _check_shape("E", E, n, n, square=True) + if S is not None: + _check_shape("S", S, n, m) + Rmat = _ssmatrix(R) Qmat = _ssmatrix(Q) - X = solve_discrete_are(A, B, Qmat, Rmat) - G = solve(B.T @ X @ B + Rmat, B.T @ X @ A) - L = eigvals(A - B @ G) + X = sp.linalg.solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S) + if S is None: + G = solve(B.T @ X @ B + Rmat, B.T @ X @ A) + else: + G = solve(B.T @ X @ B + Rmat, B.T @ X @ A + S.T) + if E is None: + L = eigvals(A - B @ G) + else: + L, _ = sp.linalg.eig(A - B @ G, E) + return _ssmatrix(X), L, _ssmatrix(G) -def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): +def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): # Make sure we can import required slycot routine try: from slycot import sb02md except ImportError: - raise ControlSlycot("can't find slycot module 'sb02md'") + raise ControlSlycot("Can't find slycot module 'sb02md'") try: from slycot import sb02mt except ImportError: - raise ControlSlycot("can't find slycot module 'sb02mt'") + 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) + raise ControlSlycot("Can't find slycot module 'sg02ad'") - 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) + # Reshape input arrays + A = np.array(A, ndmin=2) + B = np.array(B, ndmin=2) + Q = np.array(Q, ndmin=2) + R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2) # Determine main dimensions - if size(A) == 1: - n = 1 - else: - n = size(A, 0) - - if size(B) == 1: - m = 1 + n = A.shape[0] + m = B.shape[1] + + # Initialize optional matrices + S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2) + 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("A", A, n, n, square=True) + _check_shape("B", B, n, m) + _check_shape("Q", Q, n, n, square=True, symmetric=True) + _check_shape("R", R, m, m, square=True, symmetric=True) + _check_shape("E", E, n, n, square=True) + _check_shape("S", S, n, m) + + # 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 + sort = 'S' if stabilizing else 'U' + with warnings.catch_warnings(): + warnings.simplefilter("error", category=SlycotResultWarning) + rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \ + sg02ad('D', 'B', 'N', 'U', 'N', 'N', sort, + 'R', n, m, 0, A, E, B, Q, R, S) + + # Calculate the closed-loop eigenvalues L + L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) + + # Calculate the gain matrix G + G = solve(B_b.T @ X @ B_b + R_b, B_b.T @ X @ A_b + S_b.T) + + # Return the solution X, the closed-loop eigenvalues L and + # the gain matrix G + return _ssmatrix(X), L, _ssmatrix(G) + + +# Utility function to decide on method to use +def _slycot_or_scipy(method): + if method == 'slycot' or (method is None and slycot_check()): + return 'slycot' + elif method == 'scipy' or (method is None and not slycot_check()): + return 'scipy' else: - m = size(B, 1) + raise ControlArgument("Unknown method %s" % method) - # 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.") +# Utility function to check matrix dimensions +def _check_shape(name, M, n, m, square=False, symmetric=False): + if square and M.shape[0] != M.shape[1]: + raise ControlDimension("%s must be a square matrix" % name) - if not _is_symmetric(Q): - raise ControlArgument("Q must be a symmetric matrix.") + if symmetric and not _is_symmetric(M): + raise ControlArgument("%s must be a symmetric matrix" % name) - if not _is_symmetric(R): - 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 - A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = sb02mt(n, m, B, R) - - sort = 'S' if stabilizing else 'U' - X, rcond, w, S, U, A_inv = sb02md(n, A, G, Q, 'D', sort=sort) - - # Calculate the gain matrix G - if size(R_b) == 1: - G = (1/(asarray(B_ba).T @ X @ B_ba + R_ba) * - asarray(B_ba).T @ X @ A_ba) - else: - G = solve(asarray(B_ba).T @ X @ B_ba + R_ba, - asarray(B_ba).T @ X @ A_ba) - - # 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 - 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 - sort = 'S' if stabilizing else 'U' - with warnings.catch_warnings(): - warnings.simplefilter("error", category=SlycotResultWarning) - rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \ - sg02ad('D', 'B', 'N', 'U', 'N', 'N', sort, - 'R', n, m, 0, A, E, B, Q, R, S) - - 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 = (1/(asarray(B_b).T @ X @ B_b + R_b) * - (asarray(B_b).T @ X @ A_b + asarray(S_b).T)) - else: - G = solve(asarray(B_b).T @ X @ B_b + R_b, - asarray(B_b).T @ X @ A_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.") + if M.shape[0] != n or M.shape[1] != m: + raise ControlDimension("Incompatible dimensions of %s matrix" % name) +# Utility function to check if a matrix is symmetric def _is_symmetric(M): - M = atleast_2d(M) + M = np.atleast_2d(M) if isinstance(M[0, 0], inexact): eps = finfo(M.dtype).eps return ((M - M.T) < eps).all() diff --git a/control/statefbk.py b/control/statefbk.py index e82923bb4..e710f6b13 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -304,6 +304,10 @@ def lqe(*args, **keywords): Process and sensor noise covariance matrices N : 2D array, optional Cross covariance matrix. Not currently implemented. + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- @@ -326,8 +330,8 @@ def lqe(*args, **keywords): Examples -------- - >>> L, P, E = lqe(A, G, C, QN, RN) - >>> L, P, E = lqe(A, G, C, QN, RN, NN) + >>> L, P, E = lqe(A, G, C, Q, R) + >>> L, P, E = lqe(A, G, C, Q, R, N) See Also -------- @@ -345,6 +349,9 @@ def lqe(*args, **keywords): # Process the arguments and figure out what inputs we received # + # Get the method to use (if specified as a keyword) + method = keywords.get('method', None) + # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") @@ -393,7 +400,7 @@ def lqe(*args, **keywords): N.shape[0] != ninputs or N.shape[1] != noutputs): raise ControlDimension("incorrect covariance matrix dimensions") - P, E, LT = care(A.T, C.T, G @ Q @ G.T, R) + P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method) return _ssmatrix(LT.T), _ssmatrix(P), E @@ -475,6 +482,10 @@ def lqr(*args, **keywords): State and input weight matrices N : 2D array, optional Cross weight matrix + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- @@ -498,19 +509,16 @@ def lqr(*args, **keywords): -------- >>> 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 method to use (if specified as a keyword) + method = keywords.get('method', None) + # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") @@ -533,33 +541,11 @@ def lqr(*args, **keywords): 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, B.T @ X + N.T) - S = X - E = w[0:nstates] + N = None - return _ssmatrix(K), _ssmatrix(S), E + # Solve continuous algebraic Riccati equation + X, L, G = care(A, B, Q, R, N, None, method=method) + return G, X, L def ctrb(A, B): diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 62fca6bd3..0ae5a7db2 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -33,57 +33,75 @@ Author: Bjorn Olofsson """ +import numpy as np from numpy import array, zeros from numpy.testing import assert_array_almost_equal, assert_array_less import pytest from scipy.linalg import eigvals, solve +import control as ct from control.mateqn import lyap, dlyap, care, dare -from control.exception import ControlArgument +from control.exception import ControlArgument, ControlDimension, slycot_check from control.tests.conftest import slycotonly -@slycotonly class TestMatrixEquations: """These are tests for the matrix equation solvers in mateqn.py""" def test_lyap(self): - A = array([[-1, 1],[-1, 0]]) - Q = array([[1,0],[0,1]]) - X = lyap(A,Q) + A = array([[-1, 1], [-1, 0]]) + Q = array([[1, 0], [0, 1]]) + X = lyap(A, Q) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X + X @ A.T + Q, zeros((2,2))) - A = array([[1, 2],[-3, -4]]) - Q = array([[3, 1],[1, 1]]) + A = array([[1, 2], [-3, -4]]) + Q = array([[3, 1], [1, 1]]) X = lyap(A,Q) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X + X @ A.T + Q, zeros((2,2))) + # Compare methods + if slycot_check(): + X_scipy = lyap(A, Q, method='scipy') + X_slycot = lyap(A, Q, method='slycot') + assert_array_almost_equal(X_scipy, X_slycot) + def test_lyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) C = array([2, 1]) - X = lyap(A,B,C) + X = lyap(A, B, C) # print("The solution obtained is ", X) assert_array_almost_equal(A * X + X @ B + C, zeros((1,2))) - A = array([[2,1],[1,2]]) - B = array([[1,2],[0.5,0.1]]) - C = array([[1,0],[0,1]]) - X = lyap(A,B,C) + A = array([[2, 1], [1, 2]]) + B = array([[1, 2], [0.5, 0.1]]) + C = array([[1, 0], [0, 1]]) + X = lyap(A, B, C) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X + X @ B + C, zeros((2,2))) + # Compare methods + if slycot_check(): + X_scipy = lyap(A, B, C, method='scipy') + X_slycot = lyap(A, B, C, method='slycot') + assert_array_almost_equal(X_scipy, X_slycot) + + @slycotonly def test_lyap_g(self): - A = array([[-1, 2],[-3, -4]]) - Q = array([[3, 1],[1, 1]]) - E = array([[1,2],[2,1]]) - X = lyap(A,Q,None,E) + A = array([[-1, 2], [-3, -4]]) + Q = array([[3, 1], [1, 1]]) + E = array([[1, 2], [2, 1]]) + X = lyap(A, Q, None, E) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X @ E.T + E @ X @ A.T + Q, zeros((2,2))) + # Make sure that trying to solve with SciPy generates an error + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X = lyap(A, Q, None, E, method='scipy') + def test_dlyap(self): A = array([[-0.6, 0],[-0.1, -0.4]]) Q = array([[1,0],[0,1]]) @@ -97,15 +115,21 @@ def test_dlyap(self): # print("The solution obtained is ", X) assert_array_almost_equal(A @ X @ A.T - X + Q, zeros((2,2))) + @slycotonly def test_dlyap_g(self): A = array([[-0.6, 0],[-0.1, -0.4]]) Q = array([[3, 1],[1, 1]]) E = array([[1, 1],[2, 1]]) - X = dlyap(A,Q,None,E) + X = dlyap(A, Q, None, E) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X @ A.T - E @ X @ E.T + Q, zeros((2,2))) + # Make sure that trying to solve with SciPy generates an error + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X = dlyap(A, Q, None, E, method='scipy') + + @slycotonly def test_dlyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) @@ -114,25 +138,37 @@ def test_dlyap_sylvester(self): # print("The solution obtained is ", X) assert_array_almost_equal(A * X @ B.T - X + C, zeros((1,2))) - A = array([[2,1],[1,2]]) - B = array([[1,2],[0.5,0.1]]) - C = array([[1,0],[0,1]]) - X = dlyap(A,B,C) + A = array([[2, 1], [1, 2]]) + B = array([[1, 2], [0.5, 0.1]]) + C = array([[1, 0], [0, 1]]) + X = dlyap(A, B, C) # print("The solution obtained is ", X) assert_array_almost_equal(A @ X @ B.T - X + C, zeros((2,2))) + # Make sure that trying to solve with SciPy generates an error + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X = dlyap(A, B, C, method='scipy') + def test_care(self): A = array([[-2, -1],[-1, -1]]) Q = array([[0, 0],[0, 1]]) B = array([[1, 0],[0, 4]]) - X,L,G = care(A,B,Q) + X, L, G = care(A, B, Q) # print("The solution obtained is", X) M = A.T @ X + X @ A - X @ B @ B.T @ X + Q assert_array_almost_equal(M, zeros((2,2))) assert_array_almost_equal(B.T @ X, G) + # Compare methods + if slycot_check(): + X_scipy, L_scipy, G_scipy = care(A, B, Q, method='scipy') + X_slycot, L_slycot, G_slycot = care(A, B, Q, method='slycot') + assert_array_almost_equal(X_scipy, X_slycot) + assert_array_almost_equal(np.sort(L_scipy), np.sort(L_slycot)) + assert_array_almost_equal(G_scipy, G_slycot) + def test_care_g(self): A = array([[-2, -1],[-1, -1]]) Q = array([[0, 0],[0, 1]]) @@ -150,6 +186,16 @@ def test_care_g(self): - (E.T @ X @ B + S) @ Gref + Q, zeros((2,2))) + # Compare methods + if slycot_check(): + X_scipy, L_scipy, G_scipy = care( + A, B, Q, R, S, E, method='scipy') + X_slycot, L_slycot, G_slycot = care( + A, B, Q, R, S, E, method='slycot') + assert_array_almost_equal(X_scipy, X_slycot) + assert_array_almost_equal(np.sort(L_scipy), np.sort(L_slycot)) + assert_array_almost_equal(G_scipy, G_slycot) + def test_care_g2(self): A = array([[-2, -1],[-1, -1]]) Q = array([[0, 0],[0, 1]]) @@ -167,13 +213,23 @@ def test_care_g2(self): zeros((2,2))) assert_array_almost_equal(Gref , G) + # Compare methods + if slycot_check(): + X_scipy, L_scipy, G_scipy = care( + A, B, Q, R, S, E, method='scipy') + X_slycot, L_slycot, G_slycot = care( + A, B, Q, R, S, E, method='slycot') + assert_array_almost_equal(X_scipy, X_slycot) + assert_array_almost_equal(L_scipy, L_slycot) + assert_array_almost_equal(G_scipy, G_slycot) + def test_dare(self): A = array([[-0.6, 0],[-0.1, -0.4]]) Q = array([[2, 1],[1, 0]]) B = array([[2, 1],[0, 1]]) R = array([[1, 0],[0, 1]]) - X,L,G = dare(A,B,Q,R) + X, L, G = dare(A, B, Q, R) # print("The solution obtained is", X) Gref = solve(B.T @ X @ B + R, B.T @ X @ A) assert_array_almost_equal(Gref, G) @@ -188,7 +244,7 @@ def test_dare(self): B = array([[1],[0]]) R = 2 - X,L,G = dare(A,B,Q,R) + X, L, G = dare(A, B, Q, R) # print("The solution obtained is", X) AtXA = A.T @ X @ A AtXB = A.T @ X @ B @@ -201,6 +257,25 @@ def test_dare(self): lam = eigvals(A - B @ G) assert_array_less(abs(lam), 1.0) + def test_dare_compare(self): + A = np.array([[-0.6, 0], [-0.1, -0.4]]) + Q = np.array([[2, 1], [1, 0]]) + B = np.array([[2, 1], [0, 1]]) + R = np.array([[1, 0], [0, 1]]) + S = np.zeros((A.shape[0], B.shape[1])) + E = np.eye(A.shape[0]) + + # Solve via scipy + X_scipy, L_scipy, G_scipy = dare(A, B, Q, R, method='scipy') + + # Solve via slycot + if ct.slycot_check(): + X_slicot, L_slicot, G_slicot = dare( + A, B, Q, R, S, E, method='scipy') + np.testing.assert_almost_equal(X_scipy, X_slicot) + np.testing.assert_almost_equal(L_scipy, L_slicot) + np.testing.assert_almost_equal(G_scipy, G_slicot) + def test_dare_g(self): A = array([[-0.6, 0],[-0.1, -0.4]]) Q = array([[2, 1],[1, 3]]) @@ -209,7 +284,7 @@ def test_dare_g(self): S = array([[1, 0],[2, 0]]) E = array([[2, 1],[1, 2]]) - X,L,G = dare(A,B,Q,R,S,E) + X, L, G = dare(A, B, Q, R, S, E) # print("The solution obtained is", X) Gref = solve(B.T @ X @ B + R, B.T @ X @ A + S.T) assert_array_almost_equal(Gref, G) @@ -259,21 +334,21 @@ def test_raise(self): Efq = array([[2, 1, 0], [1, 2, 0]]) for cdlyap in [lyap, dlyap]: - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(Afq, Q) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(A, Qfq) with pytest.raises(ControlArgument): cdlyap(A, Qfs) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(Afq, Q, C) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(A, Qfq, C) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(A, Q, Cfd) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(A, Qfq, None, E) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdlyap(A, Q, None, Efq) with pytest.raises(ControlArgument): cdlyap(A, Qfs, None, E) @@ -290,34 +365,32 @@ def test_raise(self): E = array([[2, 1], [1, 2]]) Ef = array([[2, 1], [1, 2], [1, 2]]) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): care(Afq, B, Q) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): care(A, B, Qfq) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): care(A, Bf, Q) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): care(1, B, 1) with pytest.raises(ControlArgument): care(A, B, Qfs) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): dare(A, B, Q, Rfs) for cdare in [care, dare]: - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(Afq, B, Q, R, S, E) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(A, B, Qfq, R, S, E) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(A, Bf, Q, R, S, E) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(A, B, Q, R, S, Ef) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(A, B, Q, Rfq, S, E) - with pytest.raises(ControlArgument): + with pytest.raises(ControlDimension): cdare(A, B, Q, R, Sf, E) with pytest.raises(ControlArgument): cdare(A, B, Qfs, R, S, E) with pytest.raises(ControlArgument): cdare(A, B, Q, Rfs, S, E) - with pytest.raises(ControlArgument): - cdare(A, B, Q, R, S) diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index 8b2a0951e..a379ce7f0 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -507,7 +507,6 @@ def testAcker(self, siso): """Call acker()""" acker(siso.ss1.A, siso.ss1.B, [-2, -2.5]) - @slycotonly def testLQR(self, siso): """Call lqr()""" (K, S, E) = lqr(siso.ss1.A, siso.ss1.B, np.eye(2), np.eye(1)) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 03e1ff344..fad848da2 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -8,7 +8,8 @@ import control as ct from control import lqe, pole, rss, ss, tf -from control.exception import ControlDimension +from control.exception import ControlDimension, ControlSlycot, \ + ControlArgument, slycot_check from control.mateqn import care, dare from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker from control.tests.conftest import (slycotonly, check_deprecated_matrix, @@ -305,21 +306,34 @@ def check_LQR(self, K, S, poles, Q, R): np.testing.assert_array_almost_equal(K, K_expected) np.testing.assert_array_almost_equal(poles, poles_expected) - - @slycotonly - def test_LQR_integrator(self, matarrayin, matarrayout): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_LQR_integrator(self, matarrayin, matarrayout, method): + if method == 'slycot' and not slycot_check(): + return A, B, Q, R = (matarrayin([[X]]) for X in [0., 1., 10., 2.]) - K, S, poles = lqr(A, B, Q, R) + K, S, poles = lqr(A, B, Q, R, method=method) self.check_LQR(K, S, poles, Q, R) - @slycotonly - def test_LQR_3args(self, matarrayin, matarrayout): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_LQR_3args(self, matarrayin, matarrayout, method): + if method == 'slycot' and not slycot_check(): + return sys = ss(0., 1., 1., 0.) Q, R = (matarrayin([[X]]) for X in [10., 2.]) - K, S, poles = lqr(sys, Q, R) + K, S, poles = lqr(sys, Q, R, method=method) self.check_LQR(K, S, poles, Q, R) - @slycotonly + def test_lqr_badmethod(self): + A, B, Q, R = 0, 1, 10, 2 + with pytest.raises(ControlArgument, match="Unknown method"): + K, S, poles = lqr(A, B, Q, R, method='nosuchmethod') + + def test_lqr_slycot_not_installed(self): + A, B, Q, R = 0, 1, 10, 2 + if not slycot_check(): + with pytest.raises(ControlSlycot, match="Can't find slycot"): + K, S, poles = lqr(A, B, Q, R, method='slycot') + @pytest.mark.xfail(reason="warning not implemented") def testLQR_warning(self): """Test lqr() @@ -365,11 +379,11 @@ def test_lqr_call_format(self): np.testing.assert_array_almost_equal(Eref, E) # Inconsistent system dimensions - with pytest.raises(ct.ControlDimension, match="inconsistent system"): + with pytest.raises(ct.ControlDimension, match="Incompatible dimen"): K, S, E = lqr(sys.A, sys.C, Q, R) # incorrect covariance matrix dimensions - with pytest.raises(ct.ControlDimension, match="incorrect weighting"): + with pytest.raises(ct.ControlDimension, match="Q must be a square"): K, S, E = lqr(sys.A, sys.B, sys.C, R, Q) def check_LQE(self, L, P, poles, G, QN, RN): @@ -380,13 +394,11 @@ def check_LQE(self, L, P, poles, G, QN, RN): np.testing.assert_array_almost_equal(L, L_expected) np.testing.assert_array_almost_equal(poles, poles_expected) - @slycotonly def test_LQE(self, matarrayin): A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) L, P, poles = lqe(A, G, C, QN, RN) self.check_LQE(L, P, poles, G, QN, RN) - @slycotonly def test_lqe_call_format(self): # Create a random state space system for testing sys = rss(4, 3, 2) @@ -409,7 +421,7 @@ def test_lqe_call_format(self): sys_siso = rss(4, 1, 1) L_ss, P_ss, E_ss = lqe(sys_siso, np.eye(1), np.eye(1)) L_tf, P_tf, E_tf = lqe(tf(sys_siso), np.eye(1), np.eye(1)) - np.testing.assert_array_almost_equal(E_ss, E_tf) + np.testing.assert_array_almost_equal(np.sort(E_ss), np.sort(E_tf)) # Make sure we get an error if we specify N with pytest.raises(ct.ControlNotImplemented): @@ -423,7 +435,6 @@ def test_lqe_call_format(self): with pytest.raises(ct.ControlDimension, match="incorrect covariance"): L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) - @slycotonly def test_care(self, matarrayin): """Test stabilizing and anti-stabilizing feedbacks, continuous""" A = matarrayin(np.diag([1, -1])) @@ -432,12 +443,17 @@ def test_care(self, matarrayin): R = matarrayin(np.identity(2)) S = matarrayin(np.zeros((2, 2))) E = matarrayin(np.identity(2)) + X, L, G = care(A, B, Q, R, S, E, stabilizing=True) assert np.all(np.real(L) < 0) - X, L, G = care(A, B, Q, R, S, E, stabilizing=False) - assert np.all(np.real(L) > 0) - @slycotonly + if slycot_check(): + X, L, G = care(A, B, Q, R, S, E, stabilizing=False) + assert np.all(np.real(L) > 0) + else: + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X, L, G = care(A, B, Q, R, S, E, stabilizing=False) + def test_dare(self, matarrayin): """Test stabilizing and anti-stabilizing feedbacks, discrete""" A = matarrayin(np.diag([0.5, 2])) @@ -446,7 +462,13 @@ def test_dare(self, matarrayin): R = matarrayin(np.identity(2)) S = matarrayin(np.zeros((2, 2))) E = matarrayin(np.identity(2)) + X, L, G = dare(A, B, Q, R, S, E, stabilizing=True) assert np.all(np.abs(L) < 1) - X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) - assert np.all(np.abs(L) > 1) + + if slycot_check(): + X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) + assert np.all(np.abs(L) > 1) + else: + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X, L, G = dare(A, B, Q, R, S, E, stabilizing=False)