From 91a0455bc86d0d954cfe67a0effa376bb9f6f12c Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Wed, 22 Dec 2021 22:13:47 -0800 Subject: [PATCH 1/7] add method='scipy' to lqr() --- control/statefbk.py | 55 ++++++++++++++++++++++++---------- control/tests/statefbk_test.py | 29 +++++++++++++----- 2 files changed, 61 insertions(+), 23 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index e82923bb4..ee3b10c1c 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -475,6 +475,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,14 +502,23 @@ 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'") + # Figure out what method to use + method = keywords.get('method', None) + if method == 'slycot' or method is None: + # Make sure that SLICOT is installed + try: + from slycot import sb02md + from slycot import sb02mt + method = 'slycot' + except ImportError: + if method == 'slycot': + raise ControlSlycot( + "can't find slycot module 'sb02md' or 'sb02nt'") + else: + method = 'scipy' # # Process the arguments and figure out what inputs we received @@ -546,18 +559,28 @@ def lqr(*args, **keywords): 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') + if method == 'slycot': + # 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') + # 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] + # 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] + + elif method == 'scipy': + import scipy as sp + S = sp.linalg.solve_continuous_are(A, B, Q, R, s=N) + K = np.linalg.solve(R, B.T @ S + N.T) + E, _ = np.linalg.eig(A - B @ K) + + else: + raise ValueError("unknown method: %s" % method) return _ssmatrix(K), _ssmatrix(S), E diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 03e1ff344..68e82c472 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -8,7 +8,7 @@ import control as ct from control import lqe, pole, rss, ss, tf -from control.exception import ControlDimension +from control.exception import ControlDimension, ControlSlycot, 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, @@ -306,19 +306,34 @@ def check_LQR(self, K, S, poles, Q, R): 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) + def test_lqr_badmethod(self): + A, B, Q, R = 0, 1, 10, 2 + with pytest.raises(ValueError, match="unknown"): + 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') + @slycotonly @pytest.mark.xfail(reason="warning not implemented") def testLQR_warning(self): From 88da72913fcc0f71767928e0108f1adf0eee79be Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 23 Dec 2021 08:36:33 -0800 Subject: [PATCH 2/7] cache status of slycot in slycot_check() --- control/exception.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) 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 From 69e9605c269936faa2af3e37930c0d7abaed6e83 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 23 Dec 2021 10:23:03 -0800 Subject: [PATCH 3/7] simplify mateqn argument processing/checking --- control/mateqn.py | 450 +++++++++++------------------------ control/tests/mateqn_test.py | 24 +- 2 files changed, 160 insertions(+), 314 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index a58194811..b05df0b96 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -35,9 +35,8 @@ # OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF # SUCH DAMAGE. - import warnings - +import numpy as np from numpy import shape, size, asarray, copy, zeros, eye, \ finfo, inexact, atleast_2d from scipy.linalg import eigvals, solve_discrete_are, solve @@ -52,8 +51,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: @@ -89,8 +89,8 @@ def lyap(A, Q, C=None, E=None): :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,17 +103,17 @@ 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 Returns ------- @@ -124,6 +124,7 @@ def lyap(A, Q, C=None, E=None): ----- 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: @@ -131,46 +132,25 @@ def lyap(A, Q, C=None, E=None): 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) + # 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.") - - 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.") + # Check to make sure input matrices are the right shape and type + _check_shape("Q", Q, n, n, square=True, symmetric=True) # Solve the Lyapunov equation by calling Slycot function sb03md with warnings.catch_warnings(): @@ -178,47 +158,25 @@ 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.") - - 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.") + # 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) # 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) # 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'") @@ -229,6 +187,7 @@ 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 else: raise ControlArgument("Invalid set of input parameters") @@ -237,20 +196,20 @@ def lyap(A, Q, C=None, E=None): def dlyap(A, Q, C=None, E=None): - """ dlyap(A,Q) solves the discrete-time Lyapunov equation + """ 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` @@ -266,45 +225,25 @@ def dlyap(A, Q, C=None, E=None): 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) - - 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) + # 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.") - - 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.") + # Check to make sure input matrices are the right shape and type + _check_shape("Q", Q, n, n, square=True, symmetric=True) # Solve the Lyapunov equation by calling the Slycot function sb03md with warnings.catch_warnings(): @@ -314,38 +253,18 @@ 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") - - 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") + # 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) # Solve the Sylvester equation by calling Slycot function sb04qd X = sb04qd(n, m, -A, asarray(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) # Solve the generalized Lyapunov equation by calling Slycot # function sg03ad @@ -354,6 +273,7 @@ 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 else: raise ControlArgument("Invalid set of input parameters") @@ -365,7 +285,6 @@ 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 algebraic Riccati equation @@ -391,9 +310,9 @@ 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 Returns @@ -412,7 +331,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): """ - # Make sure we can import required slycot routine + # Make sure we can import required slycot routines try: from slycot import sb02md except ImportError: @@ -429,60 +348,28 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): 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) + # 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.") - - 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) @@ -506,43 +393,9 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): # 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.") + # 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) # Create back-up of arrays needed for later computations R_b = copy(R) @@ -577,7 +430,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): # Invalid set of input parameters else: - raise ControlArgument("Invalid set of input parameters.") + raise ControlArgument("Invalid set of input parameters") def dare(A, B, Q, R, S=None, E=None, stabilizing=True): @@ -623,9 +476,31 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True): state space operations. See :func:`~control.use_numpy_matrix`. """ + # 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) + if S is not None or E is not None or not stabilizing: return dare_old(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) + Rmat = _ssmatrix(R) Qmat = _ssmatrix(Q) X = solve_discrete_are(A, B, Qmat, Rmat) @@ -652,58 +527,28 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): 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) + # 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) + # 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.") - - 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) @@ -730,43 +575,9 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): # 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.") + # 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) # Create back-up of arrays needed for later computations A_b = copy(A) @@ -803,11 +614,24 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): # Invalid set of input parameters else: - raise ControlArgument("Invalid set of input parameters.") + raise ControlArgument("Invalid set of input parameters") + + +# 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 ControlArgument("%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 ControlArgument("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/tests/mateqn_test.py b/control/tests/mateqn_test.py index 62fca6bd3..e10c42460 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -33,11 +33,13 @@ 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.tests.conftest import slycotonly @@ -201,6 +203,26 @@ 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) + + # Solve via slycot + if ct.slycot_check(): + X_slicot, L_slicot, G_slicot = dare(A, B, Q, R, S, E) + + np.testing.assert_almost_equal(X_scipy, X_slicot) + np.testing.assert_almost_equal(L_scipy.flatten(), + L_slicot.flatten()) + 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]]) @@ -300,7 +322,7 @@ def test_raise(self): 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): From 4baea13685f8f230e43a55a998b9053da98f7e97 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 23 Dec 2021 15:56:32 -0800 Subject: [PATCH 4/7] add method='scipy' to mateqn functions --- control/mateqn.py | 338 +++++++++++++++++++++-------------- control/tests/mateqn_test.py | 113 ++++++++---- 2 files changed, 290 insertions(+), 161 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index b05df0b96..031455b02 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -37,10 +37,12 @@ import warnings import numpy as np -from numpy import shape, size, asarray, copy, zeros, eye, \ - finfo, inexact, atleast_2d +from numpy import shape, size, copy, zeros, eye, finfo, inexact, atleast_2d + +import scipy as sp from scipy.linalg import eigvals, solve_discrete_are, solve -from .exception import ControlSlycot, ControlArgument + +from .exception import ControlSlycot, ControlArgument, slycot_check from .statesp import _ssmatrix # Make sure we have access to the right slycot routines @@ -84,7 +86,7 @@ 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` @@ -114,10 +116,14 @@ def lyap(A, Q, C=None, E=None): 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 @@ -126,11 +132,13 @@ def lyap(A, Q, C=None, E=None): 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'") + # 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) @@ -152,6 +160,9 @@ def lyap(A, Q, C=None, E=None): # Check to make sure input matrices are the right shape and type _check_shape("Q", Q, n, n, square=True, symmetric=True) + if method == 'scipy': + return sp.linalg.solve_continuous_lyapunov(A, -Q) + # Solve the Lyapunov equation by calling Slycot function sb03md with warnings.catch_warnings(): warnings.simplefilter("error", category=SlycotResultWarning) @@ -164,6 +175,9 @@ def lyap(A, Q, C=None, E=None): _check_shape("Q", Q, m, m, square=True) _check_shape("C", C, n, m) + 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) @@ -173,6 +187,10 @@ def lyap(A, Q, C=None, E=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) _check_shape("E", E, n, n, square=True) + if method == 'scipy': + raise ValueError( + "method='scipy' not valid for generalized Lyapunov equation") + # Make sure we have access to the write slicot routine try: from slycot import sg03ad @@ -195,8 +213,8 @@ def lyap(A, Q, C=None, E=None): 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` @@ -214,16 +232,44 @@ def dlyap(A, Q, C=None, E=None): :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. + + 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'. + + Returns + ------- + X : 2D array (or matrix) + Solution to the Lyapunov or Sylvester equation - # 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'") + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. + + """ + # 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) @@ -245,6 +291,9 @@ def dlyap(A, Q, C=None, E=None): # Check to make sure input matrices are the right shape and type _check_shape("Q", Q, n, n, square=True, symmetric=True) + 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(): warnings.simplefilter("error", category=SlycotResultWarning) @@ -257,8 +306,12 @@ def dlyap(A, Q, C=None, E=None): _check_shape("Q", Q, m, m, square=True) _check_shape("C", C, n, m) + if method == 'scipy': + raise ValueError( + "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: @@ -266,6 +319,10 @@ def dlyap(A, Q, C=None, E=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) _check_shape("E", E, n, n, square=True) + if method == 'scipy': + raise ValueError( + "method='scipy' not valid for generalized Lyapunov equation") + # Solve the generalized Lyapunov equation by calling Slycot # function sg03ad with warnings.catch_warnings(): @@ -285,8 +342,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` @@ -297,7 +354,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` @@ -314,6 +371,10 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): Input matrices for the Riccati equation 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 ------- @@ -330,23 +391,26 @@ 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 routines - try: - from slycot import sb02md - except ImportError: - raise ControlSlycot("can't find slycot module 'sb02md'") + 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'") - try: - from slycot import sb02mt - except ImportError: - raise ControlSlycot("can't find slycot module 'sb02mt'") + 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'") + # 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) @@ -370,6 +434,17 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): # Solve the standard algebraic Riccati equation if S is None and E is None: + # See if we should solve this using SciPy + if method == 'scipy': + if not stabilizing: + raise ValueError( + "method='scipy' not valid when stabilizing is not True") + + 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) B_ba = copy(B) @@ -382,14 +457,11 @@ 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: @@ -397,6 +469,17 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): _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 ValueError( + "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) B_b = copy(B) @@ -413,27 +496,24 @@ 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 = zeros(n) L.dtype = 'complex64' for i in range(n): - L[i] = (alfar[i] + alfai[i]*1j)/beta[i] + L[i] = (alfar[i] + alfai[i]*1j) / beta[i] # 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)) + return _ssmatrix(X), L, _ssmatrix(G) # Invalid set of input parameters else: raise ControlArgument("Invalid set of input parameters") -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 @@ -460,6 +540,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 ------- @@ -476,6 +560,9 @@ def dare(A, B, Q, R, 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) + # Reshape input arrays A = np.array(A, ndmin=2) B = np.array(B, ndmin=2) @@ -493,23 +580,43 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True): # Check to make sure input matrices are the right shape and type _check_shape("A", A, n, n, square=True) - if S is not None or E is not None or not stabilizing: - return dare_old(A, B, Q, R, S, E, stabilizing) + # Figure out how to solve the problem + if method == 'scipy' and not stabilizing: + raise ValueError( + "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) + + # For consistency with dare_slycot(), don't allow just S or E + if (S is None and E is not None) or (E is None and S is not None): + raise ControlArgument("Invalid set of input parameters") 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 = 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 @@ -532,89 +639,60 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): 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] + # 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) - - # Solve the standard algebraic Riccati equation - if S is None and E is None: - # 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 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) - - # 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 + _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) + + L = zeros(n) + L.dtype = 'complex64' + for i in range(n): + L[i] = (alfar[i] + alfai[i]*1j)/beta[i] + + # 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 is None and slycot_check()) or method == 'slycot': + return 'slycot' + elif method == 'scipy' or not slycot_check(): + return 'scipy' else: - raise ControlArgument("Invalid set of input parameters") + raise ValueError("unknown method %s" % method) # Utility function to check matrix dimensions diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index e10c42460..5f3359b08 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -41,51 +41,67 @@ import control as ct from control.mateqn import lyap, dlyap, care, dare -from control.exception import ControlArgument +from control.exception import ControlArgument, 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(ValueError, 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]]) @@ -99,6 +115,7 @@ 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]]) @@ -108,6 +125,11 @@ def test_dlyap_g(self): 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(ValueError, 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]]) @@ -116,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(ValueError, 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]]) @@ -152,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]]) @@ -169,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) @@ -190,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 @@ -212,15 +266,14 @@ def test_dare_compare(self): E = np.eye(A.shape[0]) # Solve via scipy - X_scipy, L_scipy, G_scipy = dare(A, B, Q, R) + 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) - + 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.flatten(), - L_slicot.flatten()) + np.testing.assert_almost_equal(L_scipy, L_slicot) np.testing.assert_almost_equal(G_scipy, G_slicot) def test_dare_g(self): @@ -231,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) @@ -341,5 +394,3 @@ def test_raise(self): 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) From b284c47f743c94e8d8206c42722cb1eca85d4d94 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 23 Dec 2021 21:53:31 -0800 Subject: [PATCH 5/7] use care() for lqr() --- control/mateqn.py | 23 +++++------ control/statefbk.py | 71 ++++++++-------------------------- control/tests/mateqn_test.py | 2 +- control/tests/matlab_test.py | 1 - control/tests/statefbk_test.py | 26 ++++++++----- 5 files changed, 43 insertions(+), 80 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 031455b02..14747690b 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -206,7 +206,7 @@ def lyap(A, Q, C=None, E=None, method=None): 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") @@ -331,7 +331,7 @@ def dlyap(A, Q, C=None, E=None, method=None): 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") @@ -464,7 +464,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): return _ssmatrix(X), w[:n], _ssmatrix(G) # Solve the generalized algebraic Riccati equation - elif S is not None and E is not None: + 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) @@ -508,11 +512,6 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): # the gain matrix G return _ssmatrix(X), L, _ssmatrix(G) - # Invalid set of input parameters - else: - raise ControlArgument("Invalid set of input parameters") - - 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 @@ -597,10 +596,6 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): if S is not None: _check_shape("S", S, n, m) - # For consistency with dare_slycot(), don't allow just S or E - if (S is None and E is not None) or (E is None and S is not None): - raise ControlArgument("Invalid set of input parameters") - Rmat = _ssmatrix(R) Qmat = _ssmatrix(Q) X = solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S) @@ -687,9 +682,9 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): # Utility function to decide on method to use def _slycot_or_scipy(method): - if (method is None and slycot_check()) or method == 'slycot': + if method == 'slycot' or (method is None and slycot_check()): return 'slycot' - elif method == 'scipy' or not slycot_check(): + elif method == 'scipy' or (method is None and not slycot_check()): return 'scipy' else: raise ValueError("unknown method %s" % method) diff --git a/control/statefbk.py b/control/statefbk.py index ee3b10c1c..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 @@ -505,25 +512,13 @@ def lqr(*args, **keywords): """ - # Figure out what method to use - method = keywords.get('method', None) - if method == 'slycot' or method is None: - # Make sure that SLICOT is installed - try: - from slycot import sb02md - from slycot import sb02mt - method = 'slycot' - except ImportError: - if method == 'slycot': - raise ControlSlycot( - "can't find slycot module 'sb02md' or 'sb02nt'") - else: - method = 'scipy' - # # 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") @@ -546,43 +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") - - if method == 'slycot': - # 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] - - elif method == 'scipy': - import scipy as sp - S = sp.linalg.solve_continuous_are(A, B, Q, R, s=N) - K = np.linalg.solve(R, B.T @ S + N.T) - E, _ = np.linalg.eig(A - B @ K) - - else: - raise ValueError("unknown method: %s" % method) + 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 5f3359b08..81e3fffba 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -120,7 +120,7 @@ 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))) 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 68e82c472..9d2c54d14 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -305,7 +305,6 @@ 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) - @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) def test_LQR_integrator(self, matarrayin, matarrayout, method): if method == 'slycot' and not slycot_check(): @@ -334,7 +333,6 @@ def test_lqr_slycot_not_installed(self): with pytest.raises(ControlSlycot, match="can't find slycot"): K, S, poles = lqr(A, B, Q, R, method='slycot') - @slycotonly @pytest.mark.xfail(reason="warning not implemented") def testLQR_warning(self): """Test lqr() @@ -395,13 +393,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) @@ -438,7 +434,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])) @@ -447,12 +442,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(ValueError, 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])) @@ -461,7 +461,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 = care(A, B, Q, R, S, E, stabilizing=False) + assert np.all(np.real(L) > 0) + else: + with pytest.raises(ValueError, match="'scipy' not valid"): + X, L, G = care(A, B, Q, R, S, E, stabilizing=False) From 53f780807d1fcb123df85525303debe30f07a67d Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 23 Dec 2021 22:43:23 -0800 Subject: [PATCH 6/7] fix up rebase issues --- control/mateqn.py | 50 ++++++++++++++++------------------ control/tests/mateqn_test.py | 36 ++++++++++++------------ control/tests/statefbk_test.py | 10 +++---- 3 files changed, 46 insertions(+), 50 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 14747690b..3fed641bc 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -37,12 +37,13 @@ import warnings import numpy as np -from numpy import shape, size, copy, zeros, eye, finfo, inexact, atleast_2d +from numpy import copy, eye, dot, finfo, inexact, atleast_2d import scipy as sp -from scipy.linalg import eigvals, solve_discrete_are, solve +from scipy.linalg import eigvals, solve -from .exception import ControlSlycot, ControlArgument, slycot_check +from .exception import ControlSlycot, ControlArgument, ControlDimension, \ + slycot_check from .statesp import _ssmatrix # Make sure we have access to the right slycot routines @@ -136,9 +137,9 @@ def lyap(A, Q, C=None, E=None, method=None): method = _slycot_or_scipy(method) if method == 'slycot': if sb03md is None: - raise ControlSlycot("can't find slycot module 'sb03md'") + raise ControlSlycot("Can't find slycot module 'sb03md'") if sb04md is None: - raise ControlSlycot("can't find slycot module 'sb04md'") + raise ControlSlycot("Can't find slycot module 'sb04md'") # Reshape input arrays A = np.array(A, ndmin=2) @@ -196,7 +197,7 @@ def lyap(A, Q, C=None, E=None, method=None): 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 @@ -265,11 +266,11 @@ def dlyap(A, Q, C=None, E=None, method=None): 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'") + raise ControlSlycot("Can't find slycot module 'sb03md'") if sb04qd is None: - raise ControlSlycot("can't find slycot module 'sb04qd'") + raise ControlSlycot("Can't find slycot module 'sb04qd'") if sg03ad is None: - raise ControlSlycot("can't find slycot module 'sg03ad'") + raise ControlSlycot("Can't find slycot module 'sg03ad'") # Reshape input arrays A = np.array(A, ndmin=2) @@ -399,18 +400,18 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): 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'") + raise ControlSlycot("Can't find slycot module 'sg02ad'") # Reshape input arrays A = np.array(A, ndmin=2) @@ -500,10 +501,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): 'R', n, m, 0, A, E, B, Q, R, S) # Calculate the closed-loop eigenvalues L - L = zeros(n) - 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 G = solve(R_b, B_b.T @ X @ E_b + S_b.T) @@ -598,7 +596,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): Rmat = _ssmatrix(R) Qmat = _ssmatrix(Q) - X = solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S) + 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: @@ -616,18 +614,18 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): 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'") + raise ControlSlycot("Can't find slycot module 'sg02ad'") # Reshape input arrays A = np.array(A, ndmin=2) @@ -667,10 +665,8 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): sg02ad('D', 'B', 'N', 'U', 'N', 'N', sort, 'R', n, m, 0, A, E, B, Q, R, S) - L = zeros(n) - L.dtype = 'complex64' - for i in range(n): - L[i] = (alfar[i] + alfai[i]*1j)/beta[i] + # 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) @@ -687,19 +683,19 @@ def _slycot_or_scipy(method): elif method == 'scipy' or (method is None and not slycot_check()): return 'scipy' else: - raise ValueError("unknown method %s" % method) + raise ValueError("Unknown method %s" % method) # 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 ControlArgument("%s must be a square matrix" % name) + 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 ControlArgument("Incompatible dimensions of %s matrix" % name) + raise ControlDimension("Incompatible dimensions of %s matrix" % name) # Utility function to check if a matrix is symmetric diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 81e3fffba..aa7f7e5d0 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -41,7 +41,7 @@ import control as ct from control.mateqn import lyap, dlyap, care, dare -from control.exception import ControlArgument, slycot_check +from control.exception import ControlArgument, ControlDimension, slycot_check from control.tests.conftest import slycotonly @@ -334,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) @@ -365,30 +365,30 @@ 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(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) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 9d2c54d14..d6fcf30a0 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -324,13 +324,13 @@ def test_LQR_3args(self, matarrayin, matarrayout, method): def test_lqr_badmethod(self): A, B, Q, R = 0, 1, 10, 2 - with pytest.raises(ValueError, match="unknown"): + with pytest.raises(ValueError, 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"): + 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") @@ -378,11 +378,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): @@ -420,7 +420,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): From 801f282bfc951a818c497d65fd1c277ac36c7773 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 08:14:34 -0800 Subject: [PATCH 7/7] address @bnavigator comments --- control/mateqn.py | 14 +++++++------- control/tests/mateqn_test.py | 6 +++--- control/tests/statefbk_test.py | 15 ++++++++------- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 3fed641bc..3a723591f 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -189,7 +189,7 @@ def lyap(A, Q, C=None, E=None, method=None): _check_shape("E", E, n, n, square=True) if method == 'scipy': - raise ValueError( + raise ControlArgument( "method='scipy' not valid for generalized Lyapunov equation") # Make sure we have access to the write slicot routine @@ -308,7 +308,7 @@ def dlyap(A, Q, C=None, E=None, method=None): _check_shape("C", C, n, m) if method == 'scipy': - raise ValueError( + raise ControlArgument( "method='scipy' not valid for Sylvester equation") # Solve the Sylvester equation by calling Slycot function sb04qd @@ -321,7 +321,7 @@ def dlyap(A, Q, C=None, E=None, method=None): _check_shape("E", E, n, n, square=True) if method == 'scipy': - raise ValueError( + raise ControlArgument( "method='scipy' not valid for generalized Lyapunov equation") # Solve the generalized Lyapunov equation by calling Slycot @@ -438,7 +438,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): # See if we should solve this using SciPy if method == 'scipy': if not stabilizing: - raise ValueError( + raise ControlArgument( "method='scipy' not valid when stabilizing is not True") X = sp.linalg.solve_continuous_are(A, B, Q, R) @@ -477,7 +477,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): # See if we should solve this using SciPy if method == 'scipy': if not stabilizing: - raise ValueError( + 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) @@ -579,7 +579,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): # Figure out how to solve the problem if method == 'scipy' and not stabilizing: - raise ValueError( + raise ControlArgument( "method='scipy' not valid when stabilizing is not True") elif method == 'slycot': @@ -683,7 +683,7 @@ def _slycot_or_scipy(method): elif method == 'scipy' or (method is None and not slycot_check()): return 'scipy' else: - raise ValueError("Unknown method %s" % method) + raise ControlArgument("Unknown method %s" % method) # Utility function to check matrix dimensions diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index aa7f7e5d0..0ae5a7db2 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -99,7 +99,7 @@ def test_lyap_g(self): zeros((2,2))) # Make sure that trying to solve with SciPy generates an error - with pytest.raises(ValueError, match="'scipy' not valid"): + with pytest.raises(ControlArgument, match="'scipy' not valid"): X = lyap(A, Q, None, E, method='scipy') def test_dlyap(self): @@ -126,7 +126,7 @@ def test_dlyap_g(self): zeros((2,2))) # Make sure that trying to solve with SciPy generates an error - with pytest.raises(ValueError, match="'scipy' not valid"): + with pytest.raises(ControlArgument, match="'scipy' not valid"): X = dlyap(A, Q, None, E, method='scipy') @slycotonly @@ -146,7 +146,7 @@ def test_dlyap_sylvester(self): 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(ValueError, match="'scipy' not valid"): + with pytest.raises(ControlArgument, match="'scipy' not valid"): X = dlyap(A, B, C, method='scipy') def test_care(self): diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index d6fcf30a0..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, ControlSlycot, slycot_check +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, @@ -324,7 +325,7 @@ def test_LQR_3args(self, matarrayin, matarrayout, method): def test_lqr_badmethod(self): A, B, Q, R = 0, 1, 10, 2 - with pytest.raises(ValueError, match="Unknown method"): + with pytest.raises(ControlArgument, match="Unknown method"): K, S, poles = lqr(A, B, Q, R, method='nosuchmethod') def test_lqr_slycot_not_installed(self): @@ -450,7 +451,7 @@ def test_care(self, matarrayin): X, L, G = care(A, B, Q, R, S, E, stabilizing=False) assert np.all(np.real(L) > 0) else: - with pytest.raises(ValueError, match="'scipy' not valid"): + 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): @@ -466,8 +467,8 @@ def test_dare(self, matarrayin): assert np.all(np.abs(L) < 1) if slycot_check(): - X, L, G = care(A, B, Q, R, S, E, stabilizing=False) - assert np.all(np.real(L) > 0) + X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) + assert np.all(np.abs(L) > 1) else: - with pytest.raises(ValueError, match="'scipy' not valid"): - X, L, G = care(A, B, Q, R, S, E, stabilizing=False) + with pytest.raises(ControlArgument, match="'scipy' not valid"): + X, L, G = dare(A, B, Q, R, S, E, stabilizing=False)