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) pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy