diff --git a/control/exception.py b/control/exception.py index 2c4f29704..12d1bd859 100644 --- a/control/exception.py +++ b/control/exception.py @@ -39,32 +39,44 @@ # # $Id$ + class ControlSlycot(Exception): """Exception for Slycot import. Used when we can't import a function from the slycot package""" - pass + def __init__(self, error): + print("slycot import failed") + print(error) + class ControlDimension(Exception): """Raised when dimensions of system objects are not correct""" pass + class ControlArgument(Exception): """Raised when arguments to a function are not correct""" pass + class ControlMIMONotImplemented(Exception): """Function is not currently implemented for MIMO systems""" pass + class ControlNotImplemented(Exception): """Functionality is not yet implemented""" pass + # Utility function to see if slycot is installed def slycot_check(): try: import slycot - except: + except ImportError as error: + print(error) + return False + except Exception as exception: + print(exception, False) return False else: return True diff --git a/control/statefbk.py b/control/statefbk.py index 0fb377a47..5fada0b84 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1,10 +1,11 @@ -# statefbk.py - tools for state feedback control -# -# Author: Richard M. Murray, Roberto Bucher -# Date: 31 May 2010 -# -# This file contains routines for designing state space controllers -# +""" statefbk.py + +This file contains routines for designing state space controllers through state feedback control. +""" + +__author__ = "Richard M. Murray" +__contributor__ = "Roberto Bucher" + # Copyright (c) 2010 by California Institute of Technology # All rights reserved. # @@ -44,12 +45,13 @@ import scipy as sp from . import statesp from .exception import ControlSlycot, ControlArgument, ControlDimension +import warnings -__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'acker'] +__all__ = ['ctrb', 'obsv', 'gram', 'place', 'lqr', 'place_varga', 'acker'] # Pole placement -def place(A, B, p): +def place(A, B, p, method="YT", dtime=False, alpha=None): """Place closed loop eigenvalues K = place(A, B, p) @@ -61,6 +63,18 @@ def place(A, B, p): Input matrix p : 1-d list Desired eigenvalue locations + method : string - optional + Method used for pole placement: {"YT", "KNV0", "varga", "acker"} + dtime: optional (useful only if method=="varga") + False for continuous time pole placement or True for discrete time. + The default is dtime=False. + If dtime is not null, place_varga will leave the eigenvalues with modulus + less than alpha untouched. Otherwise, place_varga will leave the eigenvalues with real + real part less than alpha untouched. + alpha: double scalar (useful only if method=="varga") + + By default (alpha=None), place_varga computes alpha such that all + poles will be placed. Returns ------- @@ -91,26 +105,57 @@ def place(A, B, p): See Also -------- - place_varga, acker + acker, place_varga """ - from scipy.signal import place_poles - - # Convert the system inputs to NumPy arrays - A_mat = np.array(A) - B_mat = np.array(B) - if (A_mat.shape[0] != A_mat.shape[1]): - raise ControlDimension("A must be a square matrix") - - if (A_mat.shape[0] != B_mat.shape[0]): - err_str = "The number of rows of A must equal the number of rows in B" - raise ControlDimension(err_str) - - # Convert desired poles to numpy array - placed_eigs = np.array(p) + if method == "acker": + return acker(A, B, p) + if method == "varga": + try: + return place_varga(A, B, p, dtime=dtime, alpha=alpha) + except ControlSlycot as error: + warnings.warn("[Pole placement] Fallback strategy: using Tits-Yang method from scipy.") + return place(A, B, p) + else: + try: + from scipy.signal import place_poles + except ImportError as error: + warnings.warn(error) + warnings.warn("[Pole placement] Fallback strategy: using Varga method from slycot.") + try: + K = place_varga(A, B, p, dtime=dtime, alpha=alpha) + return K + except ControlSlycot as error: + warnings.warn("[Pole placement] Fallback strategy: using Ackermanm method.") + return acker(A, B, p) + + # Convert the system inputs to NumPy arrays + A_mat = np.array(A) + B_mat = np.array(B) + if A_mat.shape[0] != A_mat.shape[1]: + raise ControlDimension("A must be a square matrix. " + "rows: {} != columns: {}".format(A_mat.shape[0], + A_mat.shape[1])) + + if A_mat.shape[0] != B_mat.shape[0]: + raise ControlDimension("The number of rows of A ({}) must equal " + "the number of rows in B ({})".format(A_mat.shape[0], + B_mat.shape[0])) + + # Convert desired poles to numpy array + placed_eigs = np.array(p) - result = place_poles(A_mat, B_mat, placed_eigs, method='YT') - K = result.gain_matrix - return K + try: + result = place_poles(A_mat, B_mat, placed_eigs, method=method) + except ValueError as error: + warnings.warn("[Pole placement] Redundant pole location. " + "Fallback strategy: using Ackermann method.") + try: + return acker(A, B, p) + except ValueError as error: + warnings.warn("Pole placement failed.") + return None + K = result.gain_matrix + return K def place_varga(A, B, p, dtime=False, alpha=None): @@ -130,11 +175,10 @@ def place_varga(A, B, p, dtime=False, alpha=None): --------------- dtime: False for continuous time pole placement or True for discrete time. The default is dtime=False. + If dtime is not null, place_varga will leave the eigenvalues with modulus + less than alpha untouched. Otherwise, place_varga will leave the eigenvalues with real + real part less than alpha untouched. alpha: double scalar - If DICO='C', then place_varga will leave the eigenvalues with real - real part less than alpha untouched. - If DICO='D', the place_varga will leave eigenvalues with modulus - less than alpha untouched. By default (alpha=None), place_varga computes alpha such that all poles will be placed. @@ -170,15 +214,14 @@ def place_varga(A, B, p, dtime=False, alpha=None): # Make sure that SLICOT is installed try: from slycot import sb01bd - except ImportError: - raise ControlSlycot("can't find slycot module 'sb01bd'") + except ImportError as error: + raise ControlSlycot(error) # Convert the system inputs to NumPy arrays A_mat = np.array(A) B_mat = np.array(B) - if (A_mat.shape[0] != A_mat.shape[1] or - A_mat.shape[0] != B_mat.shape[0]): - raise ControlDimension("matrix dimensions are incorrect") + if A_mat.shape[0] != A_mat.shape[1] or A_mat.shape[0] != B_mat.shape[0]: + raise ControlDimension("Matrix dimensions are incorrect.") # Compute the system eigenvalues and convert poles to numpy array system_eigs = np.linalg.eig(A_mat)[0] @@ -186,14 +229,14 @@ def place_varga(A, B, p, dtime=False, alpha=None): # Need a character parameter for SB01BD if dtime: - DICO = 'D' + dico = 'D' else: - DICO = 'C' + dico = 'C' if alpha is None: # SB01BD ignores eigenvalues with real part less than alpha - # (if DICO='C') or with modulus less than alpha - # (if DICO = 'D'). + # (if dico = 'C') or with modulus less than alpha + # (if dico = 'D'). if dtime: # For discrete time, slycot only cares about modulus, so just make # alpha the smallest it can be. @@ -205,19 +248,19 @@ def place_varga(A, B, p, dtime=False, alpha=None): # the same as what slicot thinks are the eigs. So we need some # numerical breathing room. The following is pretty heuristic, # but does the trick - alpha = -2*abs(min(system_eigs.real)) + alpha = -2 * abs(min(system_eigs.real)) elif dtime and alpha < 0.0: - raise ValueError("Need alpha > 0 when DICO='D'") - + raise ValueError("Need alpha > 0 when dico='D'.") # Call SLICOT routine to place the eigenvalues - A_z,w,nfp,nap,nup,F,Z = \ + A_z, w, nfp, nap, nup, F, Z = \ sb01bd(B_mat.shape[0], B_mat.shape[1], len(placed_eigs), alpha, - A_mat, B_mat, placed_eigs, DICO) + A_mat, B_mat, placed_eigs, dico) # Return the gain matrix, with MATLAB gain convention return -F + # Contributed by Roberto Bucher def acker(A, B, poles): """Pole placement using Ackermann method @@ -237,7 +280,11 @@ def acker(A, B, poles): K: matrix Gains such that A - B K has given eigenvalues + See Also: + -------- + place, place_varga """ + # Convert the inputs to matrices a = np.mat(A) b = np.mat(B) @@ -245,22 +292,23 @@ def acker(A, B, poles): # Make sure the system is controllable ct = ctrb(A, B) if np.linalg.matrix_rank(ct) != a.shape[0]: - raise ValueError("System not reachable; pole placement invalid") + raise ValueError("System is not reachable, pole placement is invalid.") # Compute the desired characteristic polynomial p = np.real(np.poly(poles)) # Place the poles using Ackermann's method n = np.size(p) - pmat = p[n-1]*a**0 - for i in np.arange(1,n): - pmat = pmat + p[n-i-1]*a**i + pmat = p[n - 1] * a ** 0 + for i in np.arange(1, n): + pmat = pmat + p[n - i - 1] * a ** i K = np.linalg.solve(ct, pmat) - K = K[-1][:] # Extract the last row + K = K[-1][:] # Extract the last row return K -def lqr(*args, **keywords): + +def lqr(*args): """lqr(A, B, Q, R[, N]) Linear quadratic regulator design @@ -312,63 +360,64 @@ def lqr(*args, **keywords): from slycot import sb02md from slycot import sb02mt except ImportError: - raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'") + raise ControlSlycot("Can't find slycot module 'sb02md' or 'sb02nt'.") # # Process the arguments and figure out what inputs we received # # Get the system description - if (len(args) < 3): - raise ControlArgument("not enough input arguments") + if len(args) < 3: + raise ControlArgument("Not enough input arguments.") try: # If this works, we were (probably) passed a system as the # first argument; extract A and B - A = np.array(args[0].A, ndmin=2, dtype=float); - B = np.array(args[0].B, ndmin=2, dtype=float); - index = 1; + A = np.array(args[0].A, ndmin=2, dtype=float) + B = np.array(args[0].B, ndmin=2, dtype=float) + index = 1 except AttributeError: # Arguments should be A and B matrices - A = np.array(args[0], ndmin=2, dtype=float); - B = np.array(args[1], ndmin=2, dtype=float); - index = 2; + A = np.array(args[0], ndmin=2, dtype=float) + B = np.array(args[1], ndmin=2, dtype=float) + index = 2 # Get the weighting matrices (converting to matrices, if needed) - Q = np.array(args[index], ndmin=2, dtype=float); - R = np.array(args[index+1], ndmin=2, dtype=float); - if (len(args) > index + 2): - N = np.array(args[index+2], ndmin=2, dtype=float); + Q = np.array(args[index], ndmin=2, dtype=float) + R = np.array(args[index + 1], ndmin=2, dtype=float) + 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])); + 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") + nstates = B.shape[0] + ninputs = B.shape[1] + # TODO: Create / Figure out a generic function to check for inconsistent system + 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") + 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'); + 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') + 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]; + K = np.linalg.solve(R, np.dot(B.T, X) + N.T) + S = X + E = w[0:nstates] return K, S, E -def ctrb(A,B): + +def ctrb(A, B): """Controllabilty matrix Parameters @@ -392,10 +441,11 @@ def ctrb(A,B): bmat = np.mat(B) n = np.shape(amat)[0] # Construct the controllability matrix - ctrb = bmat + C = bmat for i in range(1, n): - ctrb = np.hstack((ctrb, amat**i*bmat)) - return ctrb + C = np.hstack((C, amat ** i * bmat)) + return C + def obsv(A, C): """Observability matrix @@ -422,22 +472,24 @@ def obsv(A, C): n = np.shape(amat)[0] # Construct the controllability matrix - obsv = cmat + O = cmat for i in range(1, n): - obsv = np.vstack((obsv, cmat*amat**i)) - return obsv + O = np.vstack((O, cmat * amat ** i)) + return O + -def gram(sys,type): +def gram(sys, gramian_type): """Gramian (controllability or observability) Parameters ---------- sys: StateSpace State-space system to compute Gramian for - type: String + gramian_type: String Type of desired computation. - `type` is either 'c' (controllability) or 'o' (observability). To compute the - Cholesky factors of gramians use 'cf' (controllability) or 'of' (observability) + `gramian_type` is either 'c' (controllability) or 'o' (observability). + To compute the Cholesky factors of gramians use 'cf' (controllability) + or 'of' (observability) Returns ------- @@ -448,7 +500,7 @@ def gram(sys,type): ------ ValueError * if system is not instance of StateSpace class - * if `type` is not 'c', 'o', 'cf' or 'of' + * if `gramian_type` is not 'c', 'o', 'cf' or 'of' * if system is unstable (sys.A has eigenvalues not in left half plane) ImportError @@ -457,71 +509,71 @@ def gram(sys,type): Examples -------- - >>> Wc = gram(sys,'c') - >>> Wo = gram(sys,'o') - >>> Rc = gram(sys,'cf'), where Wc=Rc'*Rc - >>> Ro = gram(sys,'of'), where Wo=Ro'*Ro + >>> Wc = gram(sys, 'c') + >>> Wo = gram(sys, 'o') + >>> Rc = gram(sys, 'cf') #, where Wc = Rc' * Rc + >>> Ro = gram(sys, 'of') #, where Wo = Ro' * Ro """ - #Check for ss system object - if not isinstance(sys,statesp.StateSpace): + # Check for ss system object + if not isinstance(sys, statesp.StateSpace): raise ValueError("System must be StateSpace!") - if type not in ['c', 'o', 'cf', 'of']: + if gramian_type not in ['c', 'o', 'cf', 'of']: raise ValueError("That type is not supported!") - #TODO: Check for continous or discrete, only continuous supported right now - # if isCont(): - # dico = 'C' - # elif isDisc(): - # dico = 'D' - # else: + # TODO: Check for continuous or discrete, only continuous supported right now + # if isCont(): + # dico = 'C' + # elif isDisc(): + # dico = 'D' + # else: dico = 'C' - #TODO: Check system is stable, perhaps a utility in ctrlutil.py - # or a method of the StateSpace class? + # TODO: Check system is stable, perhaps a utility in ctrlutil.py + # or a method of the StateSpace class? if np.any(np.linalg.eigvals(sys.A).real >= 0.0): - raise ValueError("Oops, the system is unstable!") + raise ValueError("The system is unstable.") - if type=='c' or type=='o': - #Compute Gramian by the Slycot routine sb03md - #make sure Slycot is installed + if gramian_type == 'c' or gramian_type == 'o': + # Compute Gramian by the Slycot routine sb03md + # make sure Slycot is installed try: from slycot import sb03md except ImportError: - raise ControlSlycot("can't find slycot module 'sb03md'") - if type=='c': + raise ControlSlycot("Can't find slycot module 'sb03md'.") + if gramian_type == 'c': tra = 'T' - C = -np.dot(sys.B,sys.B.transpose()) - elif type=='o': + C = -np.dot(sys.B, sys.B.transpose()) + elif gramian_type == 'o': tra = 'N' - C = -np.dot(sys.C.transpose(),sys.C) + C = -np.dot(sys.C.transpose(), sys.C) n = sys.states - U = np.zeros((n,n)) - A = np.array(sys.A) # convert to NumPy array for slycot - X,scale,sep,ferr,w = sb03md(n, C, A, U, dico, job='X', fact='N', trana=tra) + U = np.zeros((n, n)) + A = np.array(sys.A) # convert to NumPy array for slycot + X, scale, sep, ferr, w = sb03md(n, C, A, U, dico, job='X', fact='N', trana=tra) gram = X return gram - elif type=='cf' or type=='of': - #Compute cholesky factored gramian from slycot routine sb03od + elif gramian_type == 'cf' or gramian_type == 'of': + # Compute cholesky factored gramian from slycot routine sb03od try: from slycot import sb03od except ImportError: - raise ControlSlycot("can't find slycot module 'sb03od'") - tra='N' + raise ControlSlycot("Can't find slycot module 'sb03od'.") + tra = 'N' n = sys.states - Q = np.zeros((n,n)) - A = np.array(sys.A) # convert to NumPy array for slycot - if type=='cf': + Q = np.zeros((n, n)) + A = np.array(sys.A) # convert to NumPy array for slycot + if gramian_type == 'cf': m = sys.B.shape[1] B = np.zeros_like(A) - B[0:m,0:n] = sys.B.transpose() - X,scale,w = sb03od(n, m, A.transpose(), Q, B, dico, fact='N', trans=tra) - elif type=='of': + B[0:m, 0:n] = sys.B.transpose() + X, scale, w = sb03od(n, m, A.transpose(), Q, B, dico, fact='N', trans=tra) + elif gramian_type == 'of': m = sys.C.shape[0] C = np.zeros_like(A) - C[0:n,0:m] = sys.C.transpose() - X,scale,w = sb03od(n, m, A, Q, C.transpose(), dico, fact='N', trans=tra) + C[0:n, 0:m] = sys.C.transpose() + X, scale, w = sb03od(n, m, A, Q, C.transpose(), dico, fact='N', trans=tra) gram = X return gram diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index d187f6125..bcba99be1 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -402,13 +402,13 @@ def testModred(self): @unittest.skipIf(not slycot_check(), "slycot not installed") def testPlace_varga(self): - place_varga(self.siso_ss1.A, self.siso_ss1.B, [-2, -2]) + place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2], method="varga") def testPlace(self): place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5]) def testAcker(self): - acker(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5]) + place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5], method="acker") @unittest.skipIf(not slycot_check(), "slycot not installed") diff --git a/control/tests/statefbk_test.py b/control/tests/test_statefbk.py similarity index 66% rename from control/tests/statefbk_test.py rename to control/tests/test_statefbk.py index 66dce2b12..733654b7d 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/test_statefbk.py @@ -1,6 +1,6 @@ #!/usr/bin/env python # -# statefbk_test.py - test state feedback functions +# test_statefbk.py - test state feedback functions # RMM, 30 Mar 2011 (based on TestStatefbk from v0.4a) from __future__ import print_function @@ -11,6 +11,7 @@ from control.exception import slycot_check, ControlDimension from control.mateqn import care, dare + class TestStatefbk(unittest.TestCase): """Test state feedback functions""" @@ -24,118 +25,117 @@ def setUp(self): # get consistent test results np.random.seed(0) - def testCtrbSISO(self): - A = np.matrix("1. 2.; 3. 4.") - B = np.matrix("5.; 7.") - Wctrue = np.matrix("5. 19.; 7. 43.") - Wc = ctrb(A,B) + # 2 states SISO system + self.A_siso = np.array([[1., -2.], + [3., -4.]]) + self.B_siso = np.array([[5.], [7.]]) + self.C_siso = np.array([6., 8.]) + self.D_siso = np.array([9.]) + self.sys_siso = ss(self.A_siso, self.B_siso, self.C_siso, self.D_siso) + + # 2 states MIMO (2 inputs, 2 outputs) system + self.A_mimo = np.array([[1., -2.], + [3., -4.]]) + self.B_mimo = np.array([[5., 6.], + [7., 8.]]) + self.C_mimo = np.array([[4., 5.], + [6., 7.]]) + self.D_mimo = np.array([[13., 14.], + [15., 16.]]) + self.sys_mimo = ss(self.A_mimo, self.B_mimo, self.C_mimo, self.D_mimo) + + def test_ctrb_siso(self): + Wctrue = np.array([[5., -9.], + [7., -13.]]) + Wc = ctrb(self.A_siso, self.B_siso) np.testing.assert_array_almost_equal(Wc, Wctrue) - def testCtrbMIMO(self): - A = np.matrix("1. 2.; 3. 4.") - B = np.matrix("5. 6.; 7. 8.") - Wctrue = np.matrix("5. 6. 19. 22.; 7. 8. 43. 50.") - Wc = ctrb(A,B) + def test_ctrb_mimo(self): + Wctrue = np.array([[5., 6., -9., -10.], + [7., 8., -13., -14.]]) + Wc = ctrb(self.A_mimo, self.B_mimo) np.testing.assert_array_almost_equal(Wc, Wctrue) - def testObsvSISO(self): - A = np.matrix("1. 2.; 3. 4.") - C = np.matrix("5. 7.") - Wotrue = np.matrix("5. 7.; 26. 38.") - Wo = obsv(A,C) + def test_obsv_siso(self): + Wotrue = np.array([[6., 8.], + [30., -44.]]) + Wo = obsv(self.A_siso, self.C_siso) np.testing.assert_array_almost_equal(Wo, Wotrue) - def testObsvMIMO(self): - A = np.matrix("1. 2.; 3. 4.") - C = np.matrix("5. 6.; 7. 8.") - Wotrue = np.matrix("5. 6.; 7. 8.; 23. 34.; 31. 46.") - Wo = obsv(A,C) + def test_obsv_mimo(self): + Wotrue = np.array([[4., 5.], + [6., 7.], + [19., -28.], + [27., -40.]]) + Wo = obsv(self.A_mimo, self.C_mimo) np.testing.assert_array_almost_equal(Wo, Wotrue) - def testCtrbObsvDuality(self): - A = np.matrix("1.2 -2.3; 3.4 -4.5") - B = np.matrix("5.8 6.9; 8. 9.1") - Wc = ctrb(A,B); + def test_ctrb_obsv_duality(self): + A = np.array([[1.2, -2.3], + [3.4, -4.5]]) + B = np.array([[5.8, 6.9], + [8., 9.1]]) + Wc = ctrb(A, B) A = np.transpose(A) C = np.transpose(B) - Wo = np.transpose(obsv(A,C)); - np.testing.assert_array_almost_equal(Wc,Wo) + Wo = np.transpose(obsv(A, C)) + np.testing.assert_array_almost_equal(Wc, Wo) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testGramWc(self): - A = np.matrix("1. -2.; 3. -4.") - B = np.matrix("5. 6.; 7. 8.") - C = np.matrix("4. 5.; 6. 7.") - D = np.matrix("13. 14.; 15. 16.") - sys = ss(A, B, C, D) - Wctrue = np.matrix("18.5 24.5; 24.5 32.5") - Wc = gram(sys,'c') + def test_gram_wc(self): + Wctrue = np.array([[18.5, 24.5], + [24.5, 32.5]]) + Wc = gram(self.sys_mimo, 'c') np.testing.assert_array_almost_equal(Wc, Wctrue) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testGramRc(self): - A = np.matrix("1. -2.; 3. -4.") - B = np.matrix("5. 6.; 7. 8.") - C = np.matrix("4. 5.; 6. 7.") - D = np.matrix("13. 14.; 15. 16.") - sys = ss(A, B, C, D) - Rctrue = np.matrix("4.30116263 5.6961343; 0. 0.23249528") - Rc = gram(sys,'cf') + def test_gram_rc(self): + Rctrue = np.array([[4.30116263, 5.6961343], + [0., 0.23249528]]) + Rc = gram(self.sys_mimo, 'cf') np.testing.assert_array_almost_equal(Rc, Rctrue) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testGramWo(self): - A = np.matrix("1. -2.; 3. -4.") - B = np.matrix("5. 6.; 7. 8.") - C = np.matrix("4. 5.; 6. 7.") - D = np.matrix("13. 14.; 15. 16.") - sys = ss(A, B, C, D) - Wotrue = np.matrix("257.5 -94.5; -94.5 56.5") - Wo = gram(sys,'o') + def test_gram_wo(self): + Wotrue = np.array([[257.5, -94.5], + [-94.5, 56.5]]) + Wo = gram(self.sys_mimo, 'o') np.testing.assert_array_almost_equal(Wo, Wotrue) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testGramWo2(self): - A = np.matrix("1. -2.; 3. -4.") - B = np.matrix("5.; 7.") - C = np.matrix("6. 8.") - D = np.matrix("9.") - sys = ss(A,B,C,D) - Wotrue = np.matrix("198. -72.; -72. 44.") - Wo = gram(sys,'o') + def test_gram_wo2(self): + Wotrue = np.array([[198., -72.], + [-72., 44.]]) + Wo = gram(self.sys_siso, 'o') np.testing.assert_array_almost_equal(Wo, Wotrue) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testGramRo(self): - A = np.matrix("1. -2.; 3. -4.") - B = np.matrix("5. 6.; 7. 8.") - C = np.matrix("4. 5.; 6. 7.") - D = np.matrix("13. 14.; 15. 16.") - sys = ss(A, B, C, D) - Rotrue = np.matrix("16.04680654 -5.8890222; 0. 4.67112593") - Ro = gram(sys,'of') + def test_gram_ro(self): + Rotrue = np.array([[16.04680654, -5.8890222], + [0., 4.67112593]]) + Ro = gram(self.sys_mimo, 'of') np.testing.assert_array_almost_equal(Ro, Rotrue) - def testGramsys(self): - num =[1.] + def test_gram_sys(self): + num = [1.] den = [1., 1., 1.] - sys = tf(num,den) + sys = tf(num, den) self.assertRaises(ValueError, gram, sys, 'o') self.assertRaises(ValueError, gram, sys, 'c') - def testAcker(self): + def test_acker(self): for states in range(1, self.maxStates): for i in range(self.maxTries): # start with a random SS system and transform to TF then # back to SS, check that the matrices are the same. sys = rss(states, 1, 1) - if (self.debug): + if self.debug: print(sys) # Make sure the system is not degenerate Cmat = ctrb(sys.A, sys.B) if np.linalg.matrix_rank(Cmat) != states: - if (self.debug): + if self.debug: print(" skipping (not reachable or ill conditioned)") continue @@ -158,18 +158,18 @@ def testAcker(self): np.testing.assert_array_almost_equal(np.sort(poles), np.sort(placed), decimal=4) - def testPlace(self): + def test_place(self): # Matrices shamelessly stolen from scipy example code. - A = np.array([[1.380, -0.2077, 6.715, -5.676], - [-0.5814, -4.290, 0, 0.6750], - [1.067, 4.273, -6.654, 5.893], - [0.0480, 4.273, 1.343, -2.104]]) - - B = np.array([[0, 5.679], - [1.136, 1.136], - [0, 0,], - [-3.146, 0]]) - P = np.array([-0.5+1j, -0.5-1j, -5.0566, -8.6659]) + A = np.array([[1.380, -0.2077, 6.715, -5.676], + [-0.5814, -4.290, 0, 0.6750], + [1.067, 4.273, -6.654, 5.893], + [0.0480, 4.273, 1.343, -2.104]]) + + B = np.array([[0, 5.679], + [1.136, 1.136], + [0, 0, ], + [-3.146, 0]]) + P = np.array([-0.5 + 1j, -0.5 - 1j, -5.0566, -8.6659]) K = place(A, B, P) P_placed = np.linalg.eigvals(A - B.dot(K)) # No guarantee of the ordering, so sort them @@ -184,15 +184,17 @@ def testPlace(self): # Check that we get an error if we ask for too many poles in the same # location. Here, rank(B) = 2, so lets place three at the same spot. P_repeated = np.array([-0.5, -0.5, -0.5, -8.6659]) - np.testing.assert_raises(ValueError, place, A, B, P_repeated) + + # Error not raised anymore as fallback solution is implemented. + # np.testing.assert_raises(ValueError, place, A, B, P_repeated) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testPlace_varga_continuous(self): + def test_place_varga_continuous(self): """ Check that we can place eigenvalues for dtime=False """ - A = np.array([[1., -2.], [3., -4.]]) - B = np.array([[5.], [7.]]) + A = self.A_siso + B = self.B_siso P = np.array([-2., -2.]) K = place_varga(A, B, P) @@ -210,8 +212,8 @@ def testPlace_varga_continuous(self): # https://github.com/python-control/python-control/issues/177 A = np.array([[0, 1], [100, 0]]) B = np.array([[0], [1]]) - P = np.array([-20 + 10*1j, -20 - 10*1j]) - K = place_varga(A, B, P) + P = np.array([-20 + 10 * 1j, -20 - 10 * 1j]) + K = place(A, B, P, method="varga") P_placed = np.linalg.eigvals(A - B.dot(K)) # No guarantee of the ordering, so sort them @@ -220,20 +222,20 @@ def testPlace_varga_continuous(self): np.testing.assert_array_almost_equal(P, P_placed) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testPlace_varga_continuous_partial_eigs(self): + def test_pleace_varga_continuous_partial_eigs(self): """ Check that we are able to use the alpha parameter to only place a subset of the eigenvalues, for the continous time case. """ # A matrix has eigenvalues at s=-1, and s=-2. Choose alpha = -1.5 # and check that eigenvalue at s=-2 stays put. - A = np.array([[1., -2.], [3., -4.]]) - B = np.array([[5.], [7.]]) + A = self.A_siso + B = self.B_siso P = np.array([-3.]) P_expected = np.array([-2.0, -3.0]) alpha = -1.5 - K = place_varga(A, B, P, alpha=alpha) + K = place(A, B, P, method="varga", alpha=alpha) P_placed = np.linalg.eigvals(A - B.dot(K)) # No guarantee of the ordering, so sort them @@ -242,7 +244,7 @@ def testPlace_varga_continuous_partial_eigs(self): np.testing.assert_array_almost_equal(P_expected, P_placed) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testPlace_varga_discrete(self): + def test_place_varga_discrete(self): """ Check that we can place poles using dtime=True (discrete time) """ @@ -250,7 +252,7 @@ def testPlace_varga_discrete(self): B = np.array([[5.], [7.]]) P = np.array([0.5, 0.5]) - K = place_varga(A, B, P, dtime=True) + K = place(A, B, P, method="varga", dtime=True) P_placed = np.linalg.eigvals(A - B.dot(K)) # No guarantee of the ordering, so sort them P.sort() @@ -258,7 +260,7 @@ def testPlace_varga_discrete(self): np.testing.assert_array_almost_equal(P, P_placed) @unittest.skipIf(not slycot_check(), "slycot not installed") - def testPlace_varga_discrete_partial_eigs(self): + def test_place_varga_discrete_partial_eigs(self): """" Check that we can only assign a single eigenvalue in the discrete time case. @@ -270,14 +272,13 @@ def testPlace_varga_discrete_partial_eigs(self): P = np.array([0.2, 0.6]) P_expected = np.array([0.5, 0.6]) alpha = 0.51 - K = place_varga(A, B, P, dtime=True, alpha=alpha) + K = place(A, B, P, method="varga", dtime=True, alpha=alpha) P_placed = np.linalg.eigvals(A - B.dot(K)) P_expected.sort() P_placed.sort() np.testing.assert_array_almost_equal(P_expected, P_placed) - - def check_LQR(self, K, S, poles, Q, R): + def check_lqr(self, K, S, poles, Q, R): S_expected = np.array(np.sqrt(Q * R)) K_expected = S_expected / R poles_expected = np.array([-K_expected]) @@ -285,19 +286,18 @@ 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) - @unittest.skipIf(not slycot_check(), "slycot not installed") - def test_LQR_integrator(self): + def test_lqr_integrator(self): A, B, Q, R = 0., 1., 10., 2. K, S, poles = lqr(A, B, Q, R) - self.check_LQR(K, S, poles, Q, R) + self.check_lqr(K, S, poles, Q, R) @unittest.skipIf(not slycot_check(), "slycot not installed") - def test_LQR_3args(self): + def test_lqr_3args(self): sys = ss(0., 1., 1., 0.) Q, R = 10., 2. K, S, poles = lqr(sys, Q, R) - self.check_LQR(K, S, poles, Q, R) + self.check_lqr(K, S, poles, Q, R) @unittest.skipIf(not slycot_check(), "slycot not installed") def test_care(self): @@ -331,7 +331,8 @@ def test_dare(self): def test_suite(): - return unittest.TestLoader().loadTestsFromTestCase(TestStatefbk) + return unittest.TestLoader().loadTestsFromTestCase(TestStatefbk) + if __name__ == '__main__': unittest.main()