From 0c1e9832f6bc6042fd11782de56ecbab1f91033b Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 15 Jun 2019 07:48:35 -0700 Subject: [PATCH 1/4] allow np.array or np.matrix for state space matrices + unit tests --- control/bdalg.py | 91 ++-- control/config.py | 13 + control/frdata.py | 17 +- control/mateqn.py | 43 +- control/modelsimp.py | 48 +- control/statefbk.py | 47 +- control/statesp.py | 134 ++++-- control/tests/config_test.py | 3 + control/tests/modelsimp_array_test.py | 177 +++++++ control/tests/modelsimp_test.py | 2 +- control/tests/robust_array_test.py | 392 ++++++++++++++++ control/tests/statefbk_array_test.py | 419 +++++++++++++++++ control/tests/statesp_array_test.py | 637 ++++++++++++++++++++++++++ 13 files changed, 1857 insertions(+), 166 deletions(-) create mode 100644 control/tests/modelsimp_array_test.py create mode 100644 control/tests/robust_array_test.py create mode 100644 control/tests/statefbk_array_test.py create mode 100644 control/tests/statesp_array_test.py diff --git a/control/bdalg.py b/control/bdalg.py index 6744413ce..57e8763ff 100644 --- a/control/bdalg.py +++ b/control/bdalg.py @@ -53,7 +53,6 @@ """ -import scipy as sp import numpy as np from . import xferfcn as tf from . import statesp as ss @@ -61,17 +60,18 @@ __all__ = ['series', 'parallel', 'negate', 'feedback', 'append', 'connect'] + def series(sys1, *sysn): """Return the series connection (... \* sys3 \*) sys2 \* sys1 Parameters ---------- - sys1: scalar, StateSpace, TransferFunction, or FRD - sysn: other scalars, StateSpaces, TransferFunctions, or FRDs + sys1 : scalar, StateSpace, TransferFunction, or FRD + sysn : other scalars, StateSpaces, TransferFunctions, or FRDs Returns ------- - out: scalar, StateSpace, or TransferFunction + out : scalar, StateSpace, or TransferFunction Raises ------ @@ -105,18 +105,19 @@ def series(sys1, *sysn): from functools import reduce return reduce(lambda x, y:y*x, sysn, sys1) + def parallel(sys1, *sysn): """ Return the parallel connection sys1 + sys2 (+ sys3 + ...) Parameters ---------- - sys1: scalar, StateSpace, TransferFunction, or FRD - *sysn: other scalars, StateSpaces, TransferFunctions, or FRDs + sys1 : scalar, StateSpace, TransferFunction, or FRD + *sysn : other scalars, StateSpaces, TransferFunctions, or FRDs Returns ------- - out: scalar, StateSpace, or TransferFunction + out : scalar, StateSpace, or TransferFunction Raises ------ @@ -150,17 +151,18 @@ def parallel(sys1, *sysn): from functools import reduce return reduce(lambda x, y:x+y, sysn, sys1) + def negate(sys): """ Return the negative of a system. Parameters ---------- - sys: StateSpace, TransferFunction or FRD + sys : StateSpace, TransferFunction or FRD Returns ------- - out: StateSpace or TransferFunction + out : StateSpace or TransferFunction Notes ----- @@ -177,7 +179,6 @@ def negate(sys): >>> sys2 = negate(sys1) # Same as sys2 = -sys1. """ - return -sys; #! TODO: expand to allow sys2 default to work in MIMO case? @@ -187,10 +188,10 @@ def feedback(sys1, sys2=1, sign=-1): Parameters ---------- - sys1: scalar, StateSpace, TransferFunction, FRD - The primary plant. - sys2: scalar, StateSpace, TransferFunction, FRD - The feedback plant (often a feedback controller). + sys1 : scalar, StateSpace, TransferFunction, FRD + The primary process. + sys2 : scalar, StateSpace, TransferFunction, FRD + The feedback process (often a feedback controller). sign: scalar The sign of feedback. `sign` = -1 indicates negative feedback, and `sign` = 1 indicates positive feedback. `sign` is an optional @@ -198,7 +199,7 @@ def feedback(sys1, sys2=1, sign=-1): Returns ------- - out: StateSpace or TransferFunction + out : StateSpace or TransferFunction Raises ------ @@ -256,7 +257,7 @@ def feedback(sys1, sys2=1, sign=-1): return sys1.feedback(sys2, sign) def append(*sys): - '''append(sys1, sys2, ..., sysn) + """append(sys1, sys2, ..., sysn) Group models by appending their inputs and outputs @@ -279,42 +280,40 @@ def append(*sys): Examples -------- - >>> sys1 = ss("1. -2; 3. -4", "5.; 7", "6. 8", "9.") - >>> sys2 = ss("-1.", "1.", "1.", "0.") + >>> sys1 = ss([[1., -2], [3., -4]], [[5.], [7]]", [[6., 8]], [[9.]]) + >>> sys2 = ss([[-1.]], [[1.]], [[1.]], [[0.]]) >>> sys = append(sys1, sys2) - .. todo:: - also implement for transfer function, zpk, etc. - ''' + """ s1 = sys[0] for s in sys[1:]: s1 = s1.append(s) return s1 def connect(sys, Q, inputv, outputv): - ''' - Index-base interconnection of system + """Index-based interconnection of an LTI system. - The system sys is a system typically constructed with append, with - multiple inputs and outputs. The inputs and outputs are connected - according to the interconnection matrix Q, and then the final - inputs and outputs are trimmed according to the inputs and outputs - listed in inputv and outputv. + The system `sys` is a system typically constructed with `append`, with + multiple inputs and outputs. The inputs and outputs are connected + according to the interconnection matrix `Q`, and then the final inputs and + outputs are trimmed according to the inputs and outputs listed in `inputv` + and `outputv`. - Note: to have this work, inputs and outputs start counting at 1!!!! + NOTE: Inputs and outputs are indexed starting at 1 and negative values + correspond to a negative feedback interconnection. Parameters ---------- - sys: StateSpace Transferfunction + sys : StateSpace Transferfunction System to be connected - Q: 2d array + Q : 2D array Interconnection matrix. First column gives the input to be connected - second column gives the output to be fed into this input. Negative + second column gives the output to be fed into this input. Negative values for the second column mean the feedback is negative, 0 means - no connection is made - inputv: 1d array + no connection is made. Inputs and outputs are indexed starting at 1. + inputv : 1D array list of final external inputs - outputv: 1d array + outputv : 1D array list of final external outputs Returns @@ -324,28 +323,30 @@ def connect(sys, Q, inputv, outputv): Examples -------- - >>> sys1 = ss("1. -2; 3. -4", "5.; 7", "6, 8", "9.") - >>> sys2 = ss("-1.", "1.", "1.", "0.") + >>> sys1 = ss([[1., -2], [3., -4]], [[5.], [7]], [[6, 8]], [[9.]]) + >>> sys2 = ss([[-1.]], [[1.]], [[1.]], [[0.]]) >>> sys = append(sys1, sys2) - >>> Q = sp.mat([ [ 1, 2], [2, -1] ]) # basically feedback, output 2 in 1 + >>> Q = [[1, 2], [2, -1]]) # negative feedback interconnection >>> sysc = connect(sys, Q, [2], [1, 2]) - ''' + + """ # first connect - K = sp.zeros( (sys.inputs, sys.outputs) ) - for r in sp.array(Q).astype(int): + K = np.zeros((sys.inputs, sys.outputs)) + for r in np.array(Q).astype(int): inp = r[0]-1 for outp in r[1:]: if outp > 0 and outp <= sys.outputs: K[inp,outp-1] = 1. elif outp < 0 and -outp >= -sys.outputs: K[inp,-outp-1] = -1. - sys = sys.feedback(sp.matrix(K), sign=1) + sys = sys.feedback(np.array(K), sign=1) # now trim - Ytrim = sp.zeros( (len(outputv), sys.outputs) ) - Utrim = sp.zeros( (sys.inputs, len(inputv)) ) + Ytrim = np.zeros((len(outputv), sys.outputs)) + Utrim = np.zeros((sys.inputs, len(inputv))) for i,u in enumerate(inputv): Utrim[u-1,i] = 1. for i,y in enumerate(outputv): Ytrim[i,y-1] = 1. - return sp.matrix(Ytrim)*sys*sp.matrix(Utrim) + + return Ytrim * sys * Utrim diff --git a/control/config.py b/control/config.py index c90b9e16b..efafd44fd 100644 --- a/control/config.py +++ b/control/config.py @@ -7,6 +7,8 @@ # files. For now, you can just choose between MATLAB and FBS default # values. +import warnings + # Bode plot defaults bode_dB = False # Bode plot magnitude units bode_deg = True # Bode Plot phase units @@ -14,6 +16,8 @@ bode_number_of_samples = None # Bode plot number of samples bode_feature_periphery_decade = 1.0 # Bode plot feature periphery in decades +# State space module variables +_use_numpy_matrix = True # Decide whether to use numpy.marix def reset_defaults(): """Reset configuration values to their default values.""" @@ -22,6 +26,7 @@ def reset_defaults(): global bode_Hz; bode_Hz = False global bode_number_of_samples; bode_number_of_samples = None global bode_feature_periphery_decade; bode_feature_periphery_decade = 1.0 + global _use_numpy_matrix; _use_numpy_matrix = True # Set defaults to match MATLAB @@ -36,6 +41,7 @@ def use_matlab_defaults(): global bode_dB; bode_dB = True global bode_deg; bode_deg = True global bode_Hz; bode_Hz = True + global _use_numpy_matrix; _use_numpy_matrix = True # Set defaults to match FBS (Astrom and Murray) @@ -52,3 +58,10 @@ def use_fbs_defaults(): global bode_deg; bode_deg = True global bode_Hz; bode_Hz = False + +# Decide whether to use numpy.matrix for state space operations +def use_numpy_matrix(flag=True, warn=True): + if flag and warn: + warnings.warn("Return type numpy.matrix is soon to be deprecated.", + stacklevel=2) + global _use_numpy_matrix; _use_numpy_matrix = flag diff --git a/control/frdata.py b/control/frdata.py index 82da8420e..1a29df0d8 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -52,7 +52,7 @@ from warnings import warn import numpy as np from numpy import angle, array, empty, ones, \ - real, imag, matrix, absolute, eye, linalg, where, dot + real, imag, absolute, eye, linalg, where, dot from scipy.interpolate import splprep, splev from .lti import LTI @@ -81,6 +81,10 @@ class FRD(LTI): """ + # Allow NDarray * StateSpace to give StateSpace._rmul_() priority + # https://docs.scipy.org/doc/numpy/reference/arrays.classes.html + __array_priority__ = 11 # override ndarray and matrix types + epsw = 1e-8 def __init__(self, *args, **kwargs): @@ -436,12 +440,13 @@ def feedback(self, other=1, sign=-1): # TODO: vectorize this # TODO: handle omega re-mapping for k, w in enumerate(other.omega): - fresp[:, :, k] = self.fresp[:, :, k].view(type=matrix)* \ + fresp[:, :, k] = np.dot( + self.fresp[:, :, k], linalg.solve( - eye(self.inputs) + - other.fresp[:, :, k].view(type=matrix) * - self.fresp[:, :, k].view(type=matrix), - eye(self.inputs)) + eye(self.inputs) + + np.dot(other.fresp[:, :, k], self.fresp[:, :, k]), + eye(self.inputs)) + ) return FRD(fresp, other.omega, smooth=(self.ifunc is not None)) diff --git a/control/mateqn.py b/control/mateqn.py index 7e842234a..19f9930a4 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -41,16 +41,17 @@ Author: Bjorn Olofsson """ -from scipy import shape, size, asarray, asmatrix, copy, zeros, eye, dot +from scipy import shape, size, array, asarray, copy, zeros, eye, dot from scipy.linalg import eigvals, solve_discrete_are, solve from .exception import ControlSlycot, ControlArgument +from .statesp import _ssmatrix __all__ = ['lyap', 'dlyap', 'dare', 'care'] #### Lyapunov equation solvers lyap and dlyap -def lyap(A,Q,C=None,E=None): - """ X = lyap(A,Q) solves the continuous-time Lyapunov equation +def lyap(A, Q, C=None, E=None): + """X = lyap(A, Q) solves the continuous-time Lyapunov equation :math:`A X + X A^T + Q = 0` @@ -69,7 +70,9 @@ 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. """ + of the same dimension. + + """ # Make sure we have access to the right slycot routines try: @@ -84,27 +87,27 @@ def lyap(A,Q,C=None,E=None): # Reshape 1-d arrays if len(shape(A)) == 1: - A = A.reshape(1,A.size) + A = A.reshape(1, A.size) if len(shape(Q)) == 1: - Q = Q.reshape(1,Q.size) + Q = Q.reshape(1, Q.size) if C is not None and len(shape(C)) == 1: - C = C.reshape(1,C.size) + C = C.reshape(1, C.size) if E is not None and len(shape(E)) == 1: - E = E.reshape(1,E.size) + E = E.reshape(1, E.size) # Determine main dimensions if size(A) == 1: n = 1 else: - n = size(A,0) + n = size(A, 0) if size(Q) == 1: m = 1 else: - m = size(Q,0) + m = size(Q, 0) # Solve standard Lyapunov equation if C is None and E is None: @@ -228,7 +231,7 @@ def lyap(A,Q,C=None,E=None): else: raise ControlArgument("Invalid set of input parameters") - return X + return _ssmatrix(X) def dlyap(A,Q,C=None,E=None): @@ -405,12 +408,10 @@ def dlyap(A,Q,C=None,E=None): else: raise ControlArgument("Invalid set of input parameters") - return X - + return _ssmatrix(X) #### 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 @@ -566,7 +567,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G - return (X , w[:n] , G ) + return (_ssmatrix(X) , w[:n] , _ssmatrix(G)) # Solve the generalized algebraic Riccati equation elif S is not None and E is not None: @@ -673,7 +674,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True): # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G - return (X , L , G) + return (_ssmatrix(X), L, _ssmatrix(G)) # Invalid set of input parameters else: @@ -703,12 +704,12 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=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: - Rmat = asmatrix(R) - Qmat = asmatrix(Q) + Rmat = _ssmatrix(R) + Qmat = _ssmatrix(Q) X = solve_discrete_are(A, B, Qmat, Rmat) G = solve(B.T.dot(X).dot(B) + Rmat, B.T.dot(X).dot(A)) L = eigvals(A - B.dot(G)) - return X, L, G + return _ssmatrix(X), L, _ssmatrix(G) def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): # Make sure we can import required slycot routine @@ -845,7 +846,7 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G - return (X , w[:n] , G) + return (_ssmatrix(X) , w[:n], _ssmatrix(G)) # Solve the generalized algebraic Riccati equation elif S is not None and E is not None: @@ -954,7 +955,7 @@ def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True): # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G - return (X , L , G) + return (_ssmatrix(X), L, _ssmatrix(G)) # Invalid set of input parameters else: diff --git a/control/modelsimp.py b/control/modelsimp.py index 087b6bfaf..9fd36923e 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -66,7 +66,7 @@ def hsvd(sys): Returns ------- - H : Matrix + H : array A list of Hankel singular values See Also @@ -96,11 +96,10 @@ def hsvd(sys): w, v = np.linalg.eig(WoWc) hsv = np.sqrt(w) - hsv = np.matrix(hsv) + hsv = np.array(hsv) hsv = np.sort(hsv) - hsv = np.fliplr(hsv) - # Return the Hankel singular values - return hsv + # Return the Hankel singular values, high to low + return hsv[::-1] def modred(sys, ELIM, method='matchdc'): """ @@ -159,15 +158,15 @@ def modred(sys, ELIM, method='matchdc'): # Create list of elements not to eliminate (NELIM) NELIM = [i for i in range(len(sys.A)) if i not in ELIM] # A1 is a matrix of all columns of sys.A not to eliminate - A1 = sys.A[:,NELIM[0]] + A1 = sys.A[:, NELIM[0]].reshape(-1, 1) for i in NELIM[1:]: - A1 = np.hstack((A1, sys.A[:,i])) + A1 = np.hstack((A1, sys.A[:,i].reshape(-1, 1))) A11 = A1[NELIM,:] A21 = A1[ELIM,:] # A2 is a matrix of all columns of sys.A to eliminate - A2 = sys.A[:,ELIM[0]] + A2 = sys.A[:, ELIM[0]].reshape(-1, 1) for i in ELIM[1:]: - A2 = np.hstack((A2, sys.A[:,i])) + A2 = np.hstack((A2, sys.A[:,i].reshape(-1, 1))) A12 = A2[NELIM,:] A22 = A2[ELIM,:] @@ -192,10 +191,10 @@ def modred(sys, ELIM, method='matchdc'): A22I_A21 = A22I_A21_B2[:, :A21.shape[1]] A22I_B2 = A22I_A21_B2[:, A21.shape[1]:] - Ar = A11 - A12*A22I_A21 - Br = B1 - A12*A22I_B2 - Cr = C1 - C2*A22I_A21 - Dr = sys.D - C2*A22I_B2 + Ar = A11 - np.dot(A12, A22I_A21) + Br = B1 - np.dot(A12, A22I_B2) + Cr = C1 - np.dot(C2, A22I_A21) + Dr = sys.D - np.dot(C2, A22I_B2) elif method=='truncate': # if truncate, simply discard state x2 Ar = A11 @@ -380,7 +379,7 @@ def era(YY, m, n, nin, nout, r): """ raise NotImplementedError('This function is not implemented yet.') -def markov(Y, U, M): +def markov(Y, U, m): """ Calculate the first `M` Markov parameters [D CB CAB ...] from input `U`, output `Y`. @@ -391,13 +390,13 @@ def markov(Y, U, M): Output data U: array_like Input data - M: integer + m: int Number of Markov parameters to output Returns ------- - H: matrix - First M Markov parameters + H: ndarray + First m Markov parameters Notes ----- @@ -405,21 +404,22 @@ def markov(Y, U, M): Examples -------- - >>> H = markov(Y, U, M) + >>> H = markov(Y, U, m) """ # Convert input parameters to matrices (if they aren't already) - Ymat = np.mat(Y) - Umat = np.mat(U) + Ymat = np.array(Y) + Umat = np.array(U) n = np.size(U) # Construct a matrix of control inputs to invert UU = Umat - for i in range(1, M-1): - newCol = np.vstack((0, UU[0:n-1,i-2])) + for i in range(1, m-1): + # TODO: second index on UU doesn't seem right; could be neg or pos?? + newCol = np.vstack((0, np.reshape(UU[0:n-1, i-2], (-1, 1)))) UU = np.hstack((UU, newCol)) - Ulast = np.vstack((0, UU[0:n-1,M-2])) - for i in range(n-1,0,-1): + Ulast = np.vstack((0, np.reshape(UU[0:n-1, m-2], (-1, 1)))) + for i in range(n-1, 0, -1): Ulast[i] = np.sum(Ulast[0:i-1]) UU = np.hstack((UU, Ulast)) diff --git a/control/statefbk.py b/control/statefbk.py index d6eef0e45..5f4d6e09d 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -43,6 +43,7 @@ import numpy as np import scipy as sp from . import statesp +from .statesp import _ssmatrix from .exception import ControlSlycot, ControlArgument, ControlDimension __all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'acker'] @@ -110,7 +111,7 @@ def place(A, B, p): result = place_poles(A_mat, B_mat, placed_eigs, method='YT') K = result.gain_matrix - return K + return _ssmatrix(K) def place_varga(A, B, p, dtime=False, alpha=None): @@ -141,7 +142,7 @@ def place_varga(A, B, p, dtime=False, alpha=None): Returns ------- - K : 2-d array + K : 2D array Gain such that A - B K has eigenvalues given in p. @@ -216,7 +217,7 @@ def place_varga(A, B, p, dtime=False, alpha=None): A_mat, B_mat, placed_eigs, DICO) # Return the gain matrix, with MATLAB gain convention - return -F + return _ssmatrix(-F) # Contributed by Roberto Bucher def acker(A, B, poles): @@ -239,8 +240,8 @@ def acker(A, B, poles): """ # Convert the inputs to matrices - a = np.mat(A) - b = np.mat(B) + a = _ssmatrix(A) + b = _ssmatrix(B) # Make sure the system is controllable ct = ctrb(A, B) @@ -252,13 +253,13 @@ def acker(A, B, poles): # Place the poles using Ackermann's method n = np.size(p) - pmat = p[n-1]*a**0 + pmat = p[n-1] * np.linalg.matrix_power(a, 0) for i in np.arange(1,n): - pmat = pmat + p[n-i-1]*a**i + pmat = pmat + np.dot(p[n-i-1], np.linalg.matrix_power(a, i)) K = np.linalg.solve(ct, pmat) K = K[-1][:] # Extract the last row - return K + return _ssmatrix(K) def lqr(*args, **keywords): """lqr(A, B, Q, R[, N]) @@ -293,11 +294,11 @@ def lqr(*args, **keywords): Returns ------- - K: 2-d array + K: 2D array State feedback gains - S: 2-d array + S: 2D array Solution to Riccati equation - E: 1-d array + E: 1D array Eigenvalues of the closed loop system Examples @@ -366,7 +367,7 @@ def lqr(*args, **keywords): S = X; E = w[0:nstates]; - return K, S, E + return _ssmatrix(K), _ssmatrix(S), E def ctrb(A, B): """Controllabilty matrix @@ -388,13 +389,14 @@ def ctrb(A, B): """ # Convert input parameters to matrices (if they aren't already) - amat = np.mat(A) - bmat = np.mat(B) + amat = _ssmatrix(A) + bmat = _ssmatrix(B) n = np.shape(amat)[0] # Construct the controllability matrix - ctrb = np.hstack([bmat] + [amat**i*bmat for i in range(1, n)]) - return ctrb + ctrb = np.hstack([bmat] + [np.dot(np.linalg.matrix_power(amat, i), bmat) + for i in range(1, n)]) + return _ssmatrix(ctrb) def obsv(A, C): """Observability matrix @@ -416,13 +418,14 @@ def obsv(A, C): """ # Convert input parameters to matrices (if they aren't already) - amat = np.mat(A) - cmat = np.mat(C) + amat = _ssmatrix(A) + cmat = _ssmatrix(C) n = np.shape(amat)[0] # Construct the observability matrix - obsv = np.vstack([cmat] + [cmat*amat**i for i in range(1, n)]) - return obsv + obsv = np.vstack([cmat] + [np.dot(cmat, np.linalg.matrix_power(amat, i)) + for i in range(1, n)]) + return _ssmatrix(obsv) def gram(sys,type): """Gramian (controllability or observability) @@ -498,7 +501,7 @@ def gram(sys,type): 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 + return _ssmatrix(gram) elif type=='cf' or type=='of': #Compute cholesky factored gramian from slycot routine sb03od @@ -521,4 +524,4 @@ def gram(sys,type): 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 + return _ssmatrix(gram) diff --git a/control/statesp.py b/control/statesp.py index d76701629..d401966d3 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -53,8 +53,8 @@ import math import numpy as np -from numpy import all, angle, any, array, asarray, concatenate, cos, delete, \ - dot, empty, exp, eye, isinf, matrix, ones, pad, shape, sin, zeros, squeeze +from numpy import any, array, asarray, concatenate, cos, delete, \ + dot, empty, exp, eye, isinf, ones, pad, sin, zeros, squeeze from numpy.random import rand, randn from numpy.linalg import solve, eigvals, matrix_rank from numpy.linalg.linalg import LinAlgError @@ -62,32 +62,56 @@ from scipy.signal import lti, cont2discrete from warnings import warn from .lti import LTI, timebase, timebaseEqual, isdtime -from .xferfcn import _convert_to_transfer_function +from . import config from copy import deepcopy __all__ = ['StateSpace', 'ss', 'rss', 'drss', 'tf2ss', 'ssdata'] -def _matrix(a): - """Wrapper around numpy.matrix that reshapes empty matrices to be 0x0 +def _ssmatrix(data, axis=1): + """Convert argument to a (possibly empty) state space matrix. Parameters ---------- - a: sequence passed to numpy.matrix + data : array, list, or string + Input data defining the contents of the 2D array + axis : 0 or 1 + If input data is 1D, which axis to use for return object. The default + is 1, corresponding to a row matrix. Returns ------- - am: result of numpy.matrix(a), except if a is empty, am will be 0x0. + arr : 2D array, with shape (0, 0) if a is empty - numpy.matrix([]) has size 1x0; for empty StateSpace objects, we - need 0x0 matrices, so use this instead of numpy.matrix in this - module. """ - from numpy import matrix - am = matrix(a, dtype=float) - if (1, 0) == am.shape: - am.shape = (0, 0) - return am + # Convert the data into an array or matrix, as configured + # If data is passed as a string, use (deprecated?) matrix constructor + if config._use_numpy_matrix or isinstance(data, str): + arr = np.matrix(data, dtype=float) + else: + arr = np.array(data, dtype=float) + ndim = arr.ndim + shape = arr.shape + + # Change the shape of the array into a 2D array + if (ndim > 2): + raise ValueError("state-space matrix must be 2-dimensional") + + elif (ndim == 2 and shape == (1, 0)) or \ + (ndim == 1 and shape == (0, )): + # Passed an empty matrix or empty vector; change shape to (0, 0) + shape = (0, 0) + + elif ndim == 1: + # Passed a row or column vector + shape = (1, shape[0]) if axis == 1 else (shape[0], 1) + + elif ndim == 0: + # Passed a constant; turn into a matrix + shape = (1, 1) + + # Create the actual object used to store the result + return arr.reshape(shape) class StateSpace(LTI): @@ -115,6 +139,10 @@ class StateSpace(LTI): sampling time. """ + # Allow ndarray * StateSpace to give StateSpace._rmul_() priority + __array_priority__ = 11 # override ndarray and matrix types + + def __init__(self, *args, **kw): """ StateSpace(A, B, C, D[, dt]) @@ -128,7 +156,6 @@ def __init__(self, *args, **kw): call StateSpace(sys), where sys is a StateSpace object. """ - if len(args) == 4: # The user provided A, B, C, and D matrices. (A, B, C, D) = args @@ -155,7 +182,10 @@ def __init__(self, *args, **kw): # Process keyword arguments remove_useless = kw.get('remove_useless', True) - A, B, C, D = [_matrix(M) for M in (A, B, C, D)] + A = _ssmatrix(A) + B = _ssmatrix(B, axis=0) + C = _ssmatrix(C, axis=1) + D = _ssmatrix(D) # TODO: use super here? LTI.__init__(self, inputs=D.shape[1], outputs=D.shape[0], dt=dt) @@ -197,12 +227,15 @@ def _remove_useless_states(self): """ - # Search for useless states and get the indices of these states - # as an array. + # Search for useless states and get indices of these states. + # + # Note: shape from np.where depends on whether we are storing state + # space objects as np.matrix or np.array. Code below will work + # correctly in either case. ax1_A = np.where(~self.A.any(axis=1))[0] ax1_B = np.where(~self.B.any(axis=1))[0] - ax0_A = np.where(~self.A.any(axis=0))[1] - ax0_C = np.where(~self.C.any(axis=0))[1] + ax0_A = np.where(~self.A.any(axis=0))[-1] + ax0_C = np.where(~self.C.any(axis=0))[-1] useless_1 = np.intersect1d(ax1_A, ax1_B, assume_unique=True) useless_2 = np.intersect1d(ax0_A, ax0_C, assume_unique=True) useless = np.union1d(useless_1, useless_2) @@ -327,12 +360,14 @@ def __mul__(self, other): # Concatenate the various arrays A = concatenate( - (concatenate((other.A, zeros((other.A.shape[0], self.A.shape[1]))), - axis=1), - concatenate((self.B * other.C, self.A), axis=1)), axis=0) - B = concatenate((other.B, self.B * other.D), axis=0) - C = concatenate((self.D * other.C, self.C),axis=1) - D = self.D * other.D + (concatenate((other.A, + zeros((other.A.shape[0], self.A.shape[1]))), + axis=1), + concatenate((np.dot(self.B, other.C), self.A), axis=1)), + axis=0) + B = concatenate((other.B, np.dot(self.B, other.D)), axis=0) + C = concatenate((np.dot(self.D, other.C), self.C),axis=1) + D = np.dot(self.D, other.D) return StateSpace(A, B, C, D, dt) @@ -356,9 +391,9 @@ def __rmul__(self, other): # try to treat this as a matrix try: - X = _matrix(other) - C = X * self.C - D = X * self.D + X = _ssmatrix(other) + C = np.dot(X, self.C) + D = np.dot(X, self.D) return StateSpace(self.A, self.B, C, D, self.dt) except Exception as e: @@ -407,8 +442,8 @@ def horner(self, s): Returns a matrix of values evaluated at complex variable s. """ - resp = self.C * solve(s * eye(self.states) - self.A, - self.B) + self.D + resp = np.dot(self.C, solve(s * eye(self.states) - self.A, + self.B)) + self.D return array(resp) # Method for generating the frequency response of the system @@ -582,7 +617,7 @@ def feedback(self, other=1, sign=-1): C2 = other.C D2 = other.D - F = eye(self.inputs) - sign * D2 * D1 + F = eye(self.inputs) - sign * np.dot(D2, D1) if matrix_rank(F) != self.inputs: raise ValueError("I - sign * D2 * D1 is singular to working precision.") @@ -595,15 +630,20 @@ def feedback(self, other=1, sign=-1): E_D2 = E_D2_C2[:, :other.inputs] E_C2 = E_D2_C2[:, other.inputs:] - T1 = eye(self.outputs) + sign * D1 * E_D2 - T2 = eye(self.inputs) + sign * E_D2 * D1 - - A = concatenate((concatenate((A1 + sign * B1 * E_D2 * C1, sign * B1 * E_C2), axis=1), - concatenate((B2 * T1 * C1, A2 + sign * B2 * D1 * E_C2), axis=1)), - axis=0) - B = concatenate((B1 * T2, B2 * D1 * T2), axis=0) - C = concatenate((T1 * C1, sign * D1 * E_C2), axis=1) - D = D1 * T2 + T1 = eye(self.outputs) + sign * np.dot(D1, E_D2) + T2 = eye(self.inputs) + sign * np.dot(E_D2, D1) + + A = concatenate( + (concatenate( + (A1 + sign * np.dot(np.dot(B1, E_D2), C1), + sign * np.dot(B1, E_C2)), axis=1), + concatenate( + (np.dot(B2, np.dot(T1, C1)), + A2 + sign * np.dot(np.dot(B2, D1), E_C2)), axis=1)), + axis=0) + B = concatenate((np.dot(B1, T2), np.dot(np.dot(B2, D1), T2)), axis=0) + C = concatenate((np.dot(T1, C1), sign * np.dot(D1, E_C2)), axis=1) + D = np.dot(D1, T2) return StateSpace(A, B, C, D, dt) @@ -671,7 +711,7 @@ def lft(self, other, nu=-1, ny=-1): F = np.block([[np.eye(ny), -D22], [-Dbar11, np.eye(nu)]]) if matrix_rank(F) != ny + nu: raise ValueError("lft not well-posed to working precision.") - + # solve for the resulting ss by solving for [y, u] using [x, # xbar] and [w1, w2]. TH = np.linalg.solve(F, np.block( @@ -686,7 +726,7 @@ def lft(self, other, nu=-1, ny=-1): H12 = TH[:ny, self.states + other.states + self.inputs - nu:] H21 = TH[ny:, self.states + other.states: self.states + other.states + self.inputs - nu] H22 = TH[ny:, self.states + other.states + self.inputs - nu:] - + Ares = np.block([ [A + B2.dot(T21), B2.dot(T22)], [Bbar1.dot(T11), Abar + Bbar1.dot(T12)] @@ -746,7 +786,7 @@ def returnScipySignalLTI(self): for i in range(self.outputs): for j in range(self.inputs): out[i][j] = lti(asarray(self.A), asarray(self.B[:, j]), - asarray(self.C[i, :]), asarray(self.D[i, j])) + asarray(self.C[i, :]), self.D[i, j]) return out @@ -800,7 +840,7 @@ def sample(self, Ts, method='zoh', alpha=None): * gbt: generalized bilinear transformation * bilinear: Tustin's approximation ("gbt" with alpha=0.5) - * euler: Euler (or forward differencing) method ("gbt" with + * euler: Euler (or forward differencing) method ("gbt" with alpha=0) * backward_diff: Backwards differencing ("gbt" with alpha=1.0) * zoh: zero-order hold (default) @@ -946,7 +986,7 @@ def _convertToStateSpace(sys, **kw): # If this is a matrix, try to create a constant feedthrough try: - D = _matrix(sys) + D = _ssmatrix(sys) return StateSpace([], [], [], D) except Exception as e: print("Failure to assume argument is matrix-like in" \ diff --git a/control/tests/config_test.py b/control/tests/config_test.py index 6d485f822..99b86e9d3 100644 --- a/control/tests/config_test.py +++ b/control/tests/config_test.py @@ -182,6 +182,9 @@ def tearDown(self): # Get rid of any figures that we created plt.close('all') + # Reset the configuration defaults + ct.config.reset_defaults() + def suite(): return unittest.TestLoader().loadTestsFromTestCase(TestTimeresp) diff --git a/control/tests/modelsimp_array_test.py b/control/tests/modelsimp_array_test.py new file mode 100644 index 000000000..f56f492a8 --- /dev/null +++ b/control/tests/modelsimp_array_test.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python +# +# modelsimp_test.py - test model reduction functions +# RMM, 30 Mar 2011 (based on TestModelSimp from v0.4a) + +import unittest +import numpy as np +import warnings +import control +from control.modelsimp import * +from control.matlab import * +from control.exception import slycot_check + +class TestModelsimp(unittest.TestCase): + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + control.use_numpy_matrix(False) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testHSVD(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5.], [7.]]) + C = np.array([[6., 8.]]) + D = np.array([[9.]]) + sys = ss(A,B,C,D) + hsv = hsvd(sys) + hsvtrue = np.array([24.42686, 0.5731395]) # from MATLAB + np.testing.assert_array_almost_equal(hsv, hsvtrue) + + # Make sure default type values are correct + self.assertTrue(isinstance(hsv, np.ndarray)) + self.assertFalse(isinstance(hsv, np.matrix)) + + # Check that using numpy.matrix does *not* affect answer + with warnings.catch_warnings(record=True) as w: + control.use_numpy_matrix(True) + self.assertTrue(issubclass(w[-1].category, UserWarning)) + + # Redefine the system (using np.matrix for storage) + sys = ss(A, B, C, D) + + # Compute the Hankel singular value decomposition + hsv = hsvd(sys) + + # Make sure that return type is correct + self.assertTrue(isinstance(hsv, np.ndarray)) + self.assertFalse(isinstance(hsv, np.matrix)) + + # Go back to using the normal np.array representation + control.use_numpy_matrix(False) + + def testMarkov(self): + U = np.array([[1.], [1.], [1.], [1.], [1.]]) + Y = U + M = 3 + H = markov(Y,U,M) + Htrue = np.array([[1.], [0.], [0.]]) + np.testing.assert_array_almost_equal( H, Htrue ) + + def testModredMatchDC(self): + #balanced realization computed in matlab for the transfer function: + # num = [1 11 45 32], den = [1 15 60 200 60] + A = np.array( + [[-1.958, -1.194, 1.824, -1.464], + [-1.194, -0.8344, 2.563, -1.351], + [-1.824, -2.563, -1.124, 2.704], + [-1.464, -1.351, -2.704, -11.08]]) + B = np.array([[-0.9057], [-0.4068], [-0.3263], [-0.3474]]) + C = np.array([[-0.9057, -0.4068, 0.3263, -0.3474]]) + D = np.array([[0.]]) + sys = ss(A,B,C,D) + rsys = modred(sys,[2, 3],'matchdc') + Artrue = np.array([[-4.431, -4.552], [-4.552, -5.361]]) + Brtrue = np.array([[-1.362], [-1.031]]) + Crtrue = np.array([[-1.362, -1.031]]) + Drtrue = np.array([[-0.08384]]) + np.testing.assert_array_almost_equal(rsys.A, Artrue,decimal=3) + np.testing.assert_array_almost_equal(rsys.B, Brtrue,decimal=3) + np.testing.assert_array_almost_equal(rsys.C, Crtrue,decimal=3) + np.testing.assert_array_almost_equal(rsys.D, Drtrue,decimal=2) + + def testModredUnstable(self): + # Check if an error is thrown when an unstable system is given + A = np.array( + [[4.5418, 3.3999, 5.0342, 4.3808], + [0.3890, 0.3599, 0.4195, 0.1760], + [-4.2117, -3.2395, -4.6760, -4.2180], + [0.0052, 0.0429, 0.0155, 0.2743]]) + B = np.array([[1.0, 1.0], [2.0, 2.0], [3.0, 3.0], [4.0, 4.0]]) + C = np.array([[1.0, 2.0, 3.0, 4.0], [1.0, 2.0, 3.0, 4.0]]) + D = np.array([[0.0, 0.0], [0.0, 0.0]]) + sys = ss(A,B,C,D) + np.testing.assert_raises(ValueError, modred, sys, [2, 3]) + + def testModredTruncate(self): + #balanced realization computed in matlab for the transfer function: + # num = [1 11 45 32], den = [1 15 60 200 60] + A = np.array( + [[-1.958, -1.194, 1.824, -1.464], + [-1.194, -0.8344, 2.563, -1.351], + [-1.824, -2.563, -1.124, 2.704], + [-1.464, -1.351, -2.704, -11.08]]) + B = np.array([[-0.9057], [-0.4068], [-0.3263], [-0.3474]]) + C = np.array([[-0.9057, -0.4068, 0.3263, -0.3474]]) + D = np.array([[0.]]) + sys = ss(A,B,C,D) + rsys = modred(sys,[2, 3],'truncate') + Artrue = np.array([[-1.958, -1.194], [-1.194, -0.8344]]) + Brtrue = np.array([[-0.9057], [-0.4068]]) + Crtrue = np.array([[-0.9057, -0.4068]]) + Drtrue = np.array([[0.]]) + np.testing.assert_array_almost_equal(rsys.A, Artrue) + np.testing.assert_array_almost_equal(rsys.B, Brtrue) + np.testing.assert_array_almost_equal(rsys.C, Crtrue) + np.testing.assert_array_almost_equal(rsys.D, Drtrue) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testBalredTruncate(self): + #controlable canonical realization computed in matlab for the transfer function: + # num = [1 11 45 32], den = [1 15 60 200 60] + A = np.array( + [[-15., -7.5, -6.25, -1.875], + [8., 0., 0., 0.], + [0., 4., 0., 0.], + [0., 0., 1., 0.]]) + B = np.array([[2.], [0.], [0.], [0.]]) + C = np.array([[0.5, 0.6875, 0.7031, 0.5]]) + D = np.array([[0.]]) + sys = ss(A,B,C,D) + orders = 2 + rsys = balred(sys,orders,method='truncate') + Artrue = np.array([[-1.958, -1.194], [-1.194, -0.8344]]) + Brtrue = np.array([[0.9057], [0.4068]]) + Crtrue = np.array([[0.9057, 0.4068]]) + Drtrue = np.array([[0.]]) + np.testing.assert_array_almost_equal(rsys.A, Artrue,decimal=2) + np.testing.assert_array_almost_equal(rsys.B, Brtrue,decimal=4) + np.testing.assert_array_almost_equal(rsys.C, Crtrue,decimal=4) + np.testing.assert_array_almost_equal(rsys.D, Drtrue,decimal=4) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testBalredMatchDC(self): + #controlable canonical realization computed in matlab for the transfer function: + # num = [1 11 45 32], den = [1 15 60 200 60] + A = np.array( + [[-15., -7.5, -6.25, -1.875], + [8., 0., 0., 0.], + [0., 4., 0., 0.], + [0., 0., 1., 0.]]) + B = np.array([[2.], [0.], [0.], [0.]]) + C = np.array([[0.5, 0.6875, 0.7031, 0.5]]) + D = np.array([[0.]]) + sys = ss(A,B,C,D) + orders = 2 + rsys = balred(sys,orders,method='matchdc') + Artrue = np.array( + [[-4.43094773, -4.55232904], + [-4.55232904, -5.36195206]]) + Brtrue = np.array([[1.36235673], [1.03114388]]) + Crtrue = np.array([[1.36235673, 1.03114388]]) + Drtrue = np.array([[-0.08383902]]) + np.testing.assert_array_almost_equal(rsys.A, Artrue,decimal=2) + np.testing.assert_array_almost_equal(rsys.B, Brtrue,decimal=4) + np.testing.assert_array_almost_equal(rsys.C, Crtrue,decimal=4) + np.testing.assert_array_almost_equal(rsys.D, Drtrue,decimal=4) + + def tearDown(self): + # Reset configuration variables to their original settings + control.config.reset_defaults() + +def suite(): + return unittest.TestLoader().loadTestsFromTestCase(TestModelsimp) + + +if __name__ == '__main__': + unittest.main() diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index f4bc0fd98..f79a86357 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -18,7 +18,7 @@ def testHSVD(self): D = np.matrix("9.") sys = ss(A,B,C,D) hsv = hsvd(sys) - hsvtrue = np.matrix("24.42686 0.5731395") # from MATLAB + hsvtrue = [24.42686, 0.5731395] # from MATLAB np.testing.assert_array_almost_equal(hsv, hsvtrue) def testMarkov(self): diff --git a/control/tests/robust_array_test.py b/control/tests/robust_array_test.py new file mode 100644 index 000000000..51114f879 --- /dev/null +++ b/control/tests/robust_array_test.py @@ -0,0 +1,392 @@ +import unittest +import numpy as np +import control +import control.robust +from control.exception import slycot_check + +class TestHinf(unittest.TestCase): + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + control.use_numpy_matrix(False) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testHinfsyn(self): + """Test hinfsyn""" + p = control.ss(-1, [[1, 1]], [[1], [1]], [[0, 1], [1, 0]]) + k, cl, gam, rcond = control.robust.hinfsyn(p, 1, 1) + # from Octave, which also uses SB10AD: + # a= -1; b1= 1; b2= 1; c1= 1; c2= 1; d11= 0; d12= 1; d21= 1; d22= 0; + # g = ss(a,[b1,b2],[c1;c2],[d11,d12;d21,d22]); + # [k,cl] = hinfsyn(g,1,1); + np.testing.assert_array_almost_equal(k.A, [[-3]]) + np.testing.assert_array_almost_equal(k.B, [[1]]) + np.testing.assert_array_almost_equal(k.C, [[-1]]) + np.testing.assert_array_almost_equal(k.D, [[0]]) + np.testing.assert_array_almost_equal(cl.A, [[-1, -1], [1, -3]]) + np.testing.assert_array_almost_equal(cl.B, [[1], [1]]) + np.testing.assert_array_almost_equal(cl.C, [[1, -1]]) + np.testing.assert_array_almost_equal(cl.D, [[0]]) + + # TODO: add more interesting examples + + def tearDown(self): + control.config.reset_defaults() + + +class TestH2(unittest.TestCase): + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + control.use_numpy_matrix(False) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testH2syn(self): + """Test h2syn""" + p = control.ss(-1, [[1, 1]], [[1], [1]], [[0, 1], [1, 0]]) + k = control.robust.h2syn(p, 1, 1) + # from Octave, which also uses SB10HD for H-2 synthesis: + # a= -1; b1= 1; b2= 1; c1= 1; c2= 1; d11= 0; d12= 1; d21= 1; d22= 0; + # g = ss(a,[b1,b2],[c1;c2],[d11,d12;d21,d22]); + # k = h2syn(g,1,1); + # the solution is the same as for the hinfsyn test + np.testing.assert_array_almost_equal(k.A, [[-3]]) + np.testing.assert_array_almost_equal(k.B, [[1]]) + np.testing.assert_array_almost_equal(k.C, [[-1]]) + np.testing.assert_array_almost_equal(k.D, [[0]]) + + def tearDown(self): + control.config.reset_defaults() + + +class TestAugw(unittest.TestCase): + """Test control.robust.augw""" + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + control.use_numpy_matrix(False) + + # tolerance for system equality + TOL = 1e-8 + + def siso_almost_equal(self, g, h): + """siso_almost_equal(g,h) -> None + Raises AssertionError if g and h, two SISO LTI objects, are not almost equal""" + from control import tf, minreal + gmh = tf(minreal(g - h, verbose=False)) + if not (gmh.num[0][0] < self.TOL).all(): + maxnum = max(abs(gmh.num[0][0])) + raise AssertionError( + 'systems not approx equal; max num. coeff is {}\nsys 1:\n{}\nsys 2:\n{}'.format( + maxnum, g, h)) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW1(self): + """SISO plant with S weighting""" + from control import augw, ss + g = ss([-1.], [1.], [1.], [1.]) + w1 = ss([-2], [2.], [1.], [2.]) + p = augw(g, w1) + self.assertEqual(2, p.outputs) + self.assertEqual(2, p.inputs) + # w->z1 should be w1 + self.siso_almost_equal(w1, p[0, 0]) + # w->v should be 1 + self.siso_almost_equal(ss([], [], [], [1]), p[1, 0]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1 * g, p[0, 1]) + # u->v should be -g + self.siso_almost_equal(-g, p[1, 1]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW2(self): + """SISO plant with KS weighting""" + from control import augw, ss + g = ss([-1.], [1.], [1.], [1.]) + w2 = ss([-2], [1.], [1.], [2.]) + p = augw(g, w2=w2) + self.assertEqual(2, p.outputs) + self.assertEqual(2, p.inputs) + # w->z2 should be 0 + self.siso_almost_equal(ss([], [], [], 0), p[0, 0]) + # w->v should be 1 + self.siso_almost_equal(ss([], [], [], [1]), p[1, 0]) + # u->z2 should be w2 + self.siso_almost_equal(w2, p[0, 1]) + # u->v should be -g + self.siso_almost_equal(-g, p[1, 1]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW3(self): + """SISO plant with T weighting""" + from control import augw, ss + g = ss([-1.], [1.], [1.], [1.]) + w3 = ss([-2], [1.], [1.], [2.]) + p = augw(g, w3=w3) + self.assertEqual(2, p.outputs) + self.assertEqual(2, p.inputs) + # w->z3 should be 0 + self.siso_almost_equal(ss([], [], [], 0), p[0, 0]) + # w->v should be 1 + self.siso_almost_equal(ss([], [], [], [1]), p[1, 0]) + # u->z3 should be w3*g + self.siso_almost_equal(w3 * g, p[0, 1]) + # u->v should be -g + self.siso_almost_equal(-g, p[1, 1]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW123(self): + """SISO plant with all weights""" + from control import augw, ss + g = ss([-1.], [1.], [1.], [1.]) + w1 = ss([-2.], [2.], [1.], [2.]) + w2 = ss([-3.], [3.], [1.], [3.]) + w3 = ss([-4.], [4.], [1.], [4.]) + p = augw(g, w1, w2, w3) + self.assertEqual(4, p.outputs) + self.assertEqual(2, p.inputs) + # w->z1 should be w1 + self.siso_almost_equal(w1, p[0, 0]) + # w->z2 should be 0 + self.siso_almost_equal(0, p[1, 0]) + # w->z3 should be 0 + self.siso_almost_equal(0, p[2, 0]) + # w->v should be 1 + self.siso_almost_equal(ss([], [], [], [1]), p[3, 0]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1 * g, p[0, 1]) + # u->z2 should be w2 + self.siso_almost_equal(w2, p[1, 1]) + # u->z3 should be w3*g + self.siso_almost_equal(w3 * g, p[2, 1]) + # u->v should be -g + self.siso_almost_equal(-g, p[3, 1]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW1(self): + """MIMO plant with S weighting""" + from control import augw, ss + g = ss([[-1., -2], [-3, -4]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]]) + w1 = ss([-2], [2.], [1.], [2.]) + p = augw(g, w1) + self.assertEqual(4, p.outputs) + self.assertEqual(4, p.inputs) + # w->z1 should be diag(w1,w1) + self.siso_almost_equal(w1, p[0, 0]) + self.siso_almost_equal(0, p[0, 1]) + self.siso_almost_equal(0, p[1, 0]) + self.siso_almost_equal(w1, p[1, 1]) + # w->v should be I + self.siso_almost_equal(1, p[2, 0]) + self.siso_almost_equal(0, p[2, 1]) + self.siso_almost_equal(0, p[3, 0]) + self.siso_almost_equal(1, p[3, 1]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1 * g[0, 0], p[0, 2]) + self.siso_almost_equal(-w1 * g[0, 1], p[0, 3]) + self.siso_almost_equal(-w1 * g[1, 0], p[1, 2]) + self.siso_almost_equal(-w1 * g[1, 1], p[1, 3]) + # # u->v should be -g + self.siso_almost_equal(-g[0, 0], p[2, 2]) + self.siso_almost_equal(-g[0, 1], p[2, 3]) + self.siso_almost_equal(-g[1, 0], p[3, 2]) + self.siso_almost_equal(-g[1, 1], p[3, 3]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW2(self): + """MIMO plant with KS weighting""" + from control import augw, ss + g = ss([[-1., -2], [-3, -4]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]]) + w2 = ss([-2], [2.], [1.], [2.]) + p = augw(g, w2=w2) + self.assertEqual(4, p.outputs) + self.assertEqual(4, p.inputs) + # w->z2 should be 0 + self.siso_almost_equal(0, p[0, 0]) + self.siso_almost_equal(0, p[0, 1]) + self.siso_almost_equal(0, p[1, 0]) + self.siso_almost_equal(0, p[1, 1]) + # w->v should be I + self.siso_almost_equal(1, p[2, 0]) + self.siso_almost_equal(0, p[2, 1]) + self.siso_almost_equal(0, p[3, 0]) + self.siso_almost_equal(1, p[3, 1]) + # u->z2 should be w2 + self.siso_almost_equal(w2, p[0, 2]) + self.siso_almost_equal(0, p[0, 3]) + self.siso_almost_equal(0, p[1, 2]) + self.siso_almost_equal(w2, p[1, 3]) + # # u->v should be -g + self.siso_almost_equal(-g[0, 0], p[2, 2]) + self.siso_almost_equal(-g[0, 1], p[2, 3]) + self.siso_almost_equal(-g[1, 0], p[3, 2]) + self.siso_almost_equal(-g[1, 1], p[3, 3]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW3(self): + """MIMO plant with T weighting""" + from control import augw, ss + g = ss([[-1., -2], [-3, -4]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]]) + w3 = ss([-2], [2.], [1.], [2.]) + p = augw(g, w3=w3) + self.assertEqual(4, p.outputs) + self.assertEqual(4, p.inputs) + # w->z3 should be 0 + self.siso_almost_equal(0, p[0, 0]) + self.siso_almost_equal(0, p[0, 1]) + self.siso_almost_equal(0, p[1, 0]) + self.siso_almost_equal(0, p[1, 1]) + # w->v should be I + self.siso_almost_equal(1, p[2, 0]) + self.siso_almost_equal(0, p[2, 1]) + self.siso_almost_equal(0, p[3, 0]) + self.siso_almost_equal(1, p[3, 1]) + # u->z3 should be w3*g + self.siso_almost_equal(w3 * g[0, 0], p[0, 2]) + self.siso_almost_equal(w3 * g[0, 1], p[0, 3]) + self.siso_almost_equal(w3 * g[1, 0], p[1, 2]) + self.siso_almost_equal(w3 * g[1, 1], p[1, 3]) + # # u->v should be -g + self.siso_almost_equal(-g[0, 0], p[2, 2]) + self.siso_almost_equal(-g[0, 1], p[2, 3]) + self.siso_almost_equal(-g[1, 0], p[3, 2]) + self.siso_almost_equal(-g[1, 1], p[3, 3]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW123(self): + """MIMO plant with all weights""" + from control import augw, ss, append + g = ss([[-1., -2], [-3, -4]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]], + [[1., 0.], [0., 1.]]) + # this should be expaned to w1*I + w1 = ss([-2.], [2.], [1.], [2.]) + # diagonal weighting + w2 = append(ss([-3.], [3.], [1.], [3.]), ss([-4.], [4.], [1.], [4.])) + # full weighting + w3 = ss([[-4., -5], [-6, -7]], + [[2., 3.], [5., 7.]], + [[11., 13.], [17., 19.]], + [[23., 29.], [31., 37.]]) + p = augw(g, w1, w2, w3) + self.assertEqual(8, p.outputs) + self.assertEqual(4, p.inputs) + # w->z1 should be w1 + self.siso_almost_equal(w1, p[0, 0]) + self.siso_almost_equal(0, p[0, 1]) + self.siso_almost_equal(0, p[1, 0]) + self.siso_almost_equal(w1, p[1, 1]) + # w->z2 should be 0 + self.siso_almost_equal(0, p[2, 0]) + self.siso_almost_equal(0, p[2, 1]) + self.siso_almost_equal(0, p[3, 0]) + self.siso_almost_equal(0, p[3, 1]) + # w->z3 should be 0 + self.siso_almost_equal(0, p[4, 0]) + self.siso_almost_equal(0, p[4, 1]) + self.siso_almost_equal(0, p[5, 0]) + self.siso_almost_equal(0, p[5, 1]) + # w->v should be I + self.siso_almost_equal(1, p[6, 0]) + self.siso_almost_equal(0, p[6, 1]) + self.siso_almost_equal(0, p[7, 0]) + self.siso_almost_equal(1, p[7, 1]) + + # u->z1 should be -w1*g + self.siso_almost_equal(-w1 * g[0, 0], p[0, 2]) + self.siso_almost_equal(-w1 * g[0, 1], p[0, 3]) + self.siso_almost_equal(-w1 * g[1, 0], p[1, 2]) + self.siso_almost_equal(-w1 * g[1, 1], p[1, 3]) + # u->z2 should be w2 + self.siso_almost_equal(w2[0, 0], p[2, 2]) + self.siso_almost_equal(w2[0, 1], p[2, 3]) + self.siso_almost_equal(w2[1, 0], p[3, 2]) + self.siso_almost_equal(w2[1, 1], p[3, 3]) + # u->z3 should be w3*g + w3g = w3 * g; + self.siso_almost_equal(w3g[0, 0], p[4, 2]) + self.siso_almost_equal(w3g[0, 1], p[4, 3]) + self.siso_almost_equal(w3g[1, 0], p[5, 2]) + self.siso_almost_equal(w3g[1, 1], p[5, 3]) + # u->v should be -g + self.siso_almost_equal(-g[0, 0], p[6, 2]) + self.siso_almost_equal(-g[0, 1], p[6, 3]) + self.siso_almost_equal(-g[1, 0], p[7, 2]) + self.siso_almost_equal(-g[1, 1], p[7, 3]) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testErrors(self): + """Error cases handled""" + from control import augw, ss + # no weights + g1by1 = ss(-1, 1, 1, 0) + g2by2 = ss(-np.eye(2), np.eye(2), np.eye(2), np.zeros((2, 2))) + self.assertRaises(ValueError, augw, g1by1) + # mismatched size of weight and plant + self.assertRaises(ValueError, augw, g1by1, w1=g2by2) + self.assertRaises(ValueError, augw, g1by1, w2=g2by2) + self.assertRaises(ValueError, augw, g1by1, w3=g2by2) + + def tearDown(self): + control.config.reset_defaults() + + +class TestMixsyn(unittest.TestCase): + """Test control.robust.mixsyn""" + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + control.use_numpy_matrix(False) + + # it's a relatively simple wrapper; compare results with augw, hinfsyn + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSiso(self): + """mixsyn with SISO system""" + from control import tf, augw, hinfsyn, mixsyn + from control import ss + # Skogestad+Postlethwaite, Multivariable Feedback Control, 1st Ed., Example 2.11 + s = tf([1, 0], 1) + # plant + g = 200 / (10 * s + 1) / (0.05 * s + 1) ** 2 + # sensitivity weighting + M = 1.5 + wb = 10 + A = 1e-4 + w1 = (s / M + wb) / (s + wb * A) + # KS weighting + w2 = tf(1, 1) + + p = augw(g, w1, w2) + kref, clref, gam, rcond = hinfsyn(p, 1, 1) + ktest, cltest, info = mixsyn(g, w1, w2) + # check similar to S+P's example + np.testing.assert_allclose(gam, 1.37, atol=1e-2) + + # mixsyn is a convenience wrapper around augw and hinfsyn, so + # results will be exactly the same. Given than, use the lazy + # but fragile testing option. + np.testing.assert_allclose(ktest.A, kref.A) + np.testing.assert_allclose(ktest.B, kref.B) + np.testing.assert_allclose(ktest.C, kref.C) + np.testing.assert_allclose(ktest.D, kref.D) + + np.testing.assert_allclose(cltest.A, clref.A) + np.testing.assert_allclose(cltest.B, clref.B) + np.testing.assert_allclose(cltest.C, clref.C) + np.testing.assert_allclose(cltest.D, clref.D) + + np.testing.assert_allclose(gam, info[0]) + + np.testing.assert_allclose(rcond, info[1]) + + def tearDown(self): + control.config.reset_defaults() + +if __name__ == "__main__": + unittest.main() diff --git a/control/tests/statefbk_array_test.py b/control/tests/statefbk_array_test.py new file mode 100644 index 000000000..941488978 --- /dev/null +++ b/control/tests/statefbk_array_test.py @@ -0,0 +1,419 @@ +#!/usr/bin/env python +# +# statefbk_test.py - test state feedback functions +# RMM, 30 Mar 2011 (based on TestStatefbk from v0.4a) + +from __future__ import print_function +import unittest +import sys as pysys +import numpy as np +import warnings +from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker +from control.matlab import * +from control.exception import slycot_check, ControlDimension +from control.mateqn import care, dare +from control.config import use_numpy_matrix, reset_defaults + +class TestStatefbk(unittest.TestCase): + """Test state feedback functions""" + + def setUp(self): + # Use array instead of matrix (and save old value to restore at end) + use_numpy_matrix(False) + + # Maximum number of states to test + 1 + self.maxStates = 5 + # Maximum number of inputs and outputs to test + 1 + self.maxTries = 4 + # Set to True to print systems to the output. + self.debug = False + # get consistent test results + np.random.seed(0) + + def testCtrbSISO(self): + A = np.array([[1., 2.], [3., 4.]]) + B = np.array([[5.], [7.]]) + Wctrue = np.array([[5., 19.], [7., 43.]]) + + Wc = ctrb(A, B) + np.testing.assert_array_almost_equal(Wc, Wctrue) + self.assertTrue(isinstance(Wc, np.ndarray)) + self.assertFalse(isinstance(Wc, np.matrix)) + + # This test only works in Python 3 due to a conflict with the same + # warning type in other test modules (frd_test.py). See + # https://bugs.python.org/issue4180 for more details + @unittest.skipIf(pysys.version_info < (3, 0), "test requires Python 3+") + def test_ctrb_siso_deprecated(self): + A = np.array([[1., 2.], [3., 4.]]) + B = np.array([[5.], [7.]]) + + # Check that default using np.matrix generates a warning + # TODO: remove this check with matrix type is deprecated + warnings.resetwarnings() + with warnings.catch_warnings(record=True) as w: + use_numpy_matrix(True) + self.assertTrue(issubclass(w[-1].category, UserWarning)) + + Wc = ctrb(A, B) + self.assertTrue(isinstance(Wc, np.matrix)) + self.assertTrue(issubclass(w[-1].category, + PendingDeprecationWarning)) + use_numpy_matrix(False) + + def testCtrbMIMO(self): + A = np.array([[1., 2.], [3., 4.]]) + B = np.array([[5., 6.], [7., 8.]]) + Wctrue = np.array([[5., 6., 19., 22.], [7., 8., 43., 50.]]) + Wc = ctrb(A, B) + np.testing.assert_array_almost_equal(Wc, Wctrue) + + # Make sure default type values are correct + self.assertTrue(isinstance(Wc, np.ndarray)) + + def testObsvSISO(self): + A = np.array([[1., 2.], [3., 4.]]) + C = np.array([[5., 7.]]) + Wotrue = np.array([[5., 7.], [26., 38.]]) + Wo = obsv(A, C) + np.testing.assert_array_almost_equal(Wo, Wotrue) + + # Make sure default type values are correct + self.assertTrue(isinstance(Wo, np.ndarray)) + + # This test only works in Python 3 due to a conflict with the same + # warning type in other test modules (frd_test.py). See + # https://bugs.python.org/issue4180 for more details + @unittest.skipIf(pysys.version_info < (3, 0), "test requires Python 3+") + def test_obsv_siso_deprecated(self): + A = np.array([[1., 2.], [3., 4.]]) + C = np.array([[5., 7.]]) + + # Check that default type generates a warning + # TODO: remove this check with matrix type is deprecated + with warnings.catch_warnings(record=True) as w: + use_numpy_matrix(True, warn=False) # warnings off + self.assertEqual(len(w), 0) + + Wo = obsv(A, C) + self.assertTrue(isinstance(Wo, np.matrix)) + use_numpy_matrix(False) + + def testObsvMIMO(self): + A = np.array([[1., 2.], [3., 4.]]) + C = np.array([[5., 6.], [7., 8.]]) + Wotrue = np.array([[5., 6.], [7., 8.], [23., 34.], [31., 46.]]) + Wo = obsv(A, C) + np.testing.assert_array_almost_equal(Wo, Wotrue) + + def testCtrbObsvDuality(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) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testGramWc(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5., 6.], [7., 8.]]) + C = np.array([[4., 5.], [6., 7.]]) + D = np.array([[13., 14.], [15., 16.]]) + sys = ss(A, B, C, D) + Wctrue = np.array([[18.5, 24.5], [24.5, 32.5]]) + Wc = gram(sys, 'c') + np.testing.assert_array_almost_equal(Wc, Wctrue) + + # This test only works in Python 3 due to a conflict with the same + # warning type in other test modules (frd_test.py). See + # https://bugs.python.org/issue4180 for more details + @unittest.skipIf(pysys.version_info < (3, 0) or not slycot_check(), + "test requires Python 3+ and slycot") + def test_gram_wc_deprecated(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5., 6.], [7., 8.]]) + C = np.array([[4., 5.], [6., 7.]]) + D = np.array([[13., 14.], [15., 16.]]) + sys = ss(A, B, C, D) + + # Check that default type generates a warning + # TODO: remove this check with matrix type is deprecated + with warnings.catch_warnings(record=True) as w: + use_numpy_matrix(True) + self.assertTrue(issubclass(w[-1].category, UserWarning)) + + Wc = gram(sys, 'c') + self.assertTrue(isinstance(Wc, np.ndarray)) + use_numpy_matrix(False) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testGramRc(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5., 6.], [7., 8.]]) + C = np.array([[4., 5.], [6., 7.]]) + D = np.array([[13., 14.], [15., 16.]]) + sys = ss(A, B, C, D) + Rctrue = np.array([[4.30116263, 5.6961343], [0., 0.23249528]]) + Rc = gram(sys, 'cf') + np.testing.assert_array_almost_equal(Rc, Rctrue) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testGramWo(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5., 6.], [7., 8.]]) + C = np.array([[4., 5.], [6., 7.]]) + D = np.array([[13., 14.], [15., 16.]]) + sys = ss(A, B, C, D) + Wotrue = np.array([[257.5, -94.5], [-94.5, 56.5]]) + Wo = gram(sys, 'o') + np.testing.assert_array_almost_equal(Wo, Wotrue) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testGramWo2(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5.], [7.]]) + C = np.array([[6., 8.]]) + D = np.array([[9.]]) + sys = ss(A,B,C,D) + Wotrue = np.array([[198., -72.], [-72., 44.]]) + Wo = gram(sys, 'o') + np.testing.assert_array_almost_equal(Wo, Wotrue) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testGramRo(self): + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5., 6.], [7., 8.]]) + C = np.array([[4., 5.], [6., 7.]]) + D = np.array([[13., 14.], [15., 16.]]) + sys = ss(A, B, C, D) + Rotrue = np.array([[16.04680654, -5.8890222], [0., 4.67112593]]) + Ro = gram(sys, 'of') + np.testing.assert_array_almost_equal(Ro, Rotrue) + + def testGramsys(self): + num =[1.] + den = [1., 1., 1.] + sys = tf(num,den) + self.assertRaises(ValueError, gram, sys, 'o') + self.assertRaises(ValueError, gram, sys, 'c') + + def testAcker(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): + 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): + print(" skipping (not reachable or ill conditioned)") + continue + + # Place the poles at random locations + des = rss(states, 1, 1); + poles = pole(des) + + # Now place the poles using acker + K = acker(sys.A, sys.B, poles) + new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D) + placed = pole(new) + + # Debugging code + # diff = np.sort(poles) - np.sort(placed) + # if not all(diff < 0.001): + # print("Found a problem:") + # print(sys) + # print("desired = ", poles) + + np.testing.assert_array_almost_equal(np.sort(poles), + np.sort(placed), decimal=4) + + def testPlace(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]) + K = place(A, B, P) + P_placed = np.linalg.eigvals(A - B.dot(K)) + # No guarantee of the ordering, so sort them + P.sort() + P_placed.sort() + np.testing.assert_array_almost_equal(P, P_placed) + + # Test that the dimension checks work. + np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P) + np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P) + + # 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) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testPlace_varga_continuous(self): + """ + Check that we can place eigenvalues for dtime=False + """ + A = np.array([[1., -2.], [3., -4.]]) + B = np.array([[5.], [7.]]) + + P = np.array([-2., -2.]) + K = place_varga(A, B, P) + P_placed = np.linalg.eigvals(A - B.dot(K)) + # No guarantee of the ordering, so sort them + P.sort() + P_placed.sort() + np.testing.assert_array_almost_equal(P, P_placed) + + # Test that the dimension checks work. + np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P) + np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P) + + # Regression test against bug #177 + # 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_placed = np.linalg.eigvals(A - B.dot(K)) + + # No guarantee of the ordering, so sort them + P.sort() + P_placed.sort() + np.testing.assert_array_almost_equal(P, P_placed) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testPlace_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.]]) + + P = np.array([-3.]) + P_expected = np.array([-2.0, -3.0]) + alpha = -1.5 + K = place_varga(A, B, P, alpha=alpha) + + P_placed = np.linalg.eigvals(A - B.dot(K)) + # No guarantee of the ordering, so sort them + P_expected.sort() + P_placed.sort() + np.testing.assert_array_almost_equal(P_expected, P_placed) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testPlace_varga_discrete(self): + """ + Check that we can place poles using dtime=True (discrete time) + """ + A = np.array([[1., 0], [0, 0.5]]) + B = np.array([[5.], [7.]]) + + P = np.array([0.5, 0.5]) + K = place_varga(A, B, P, dtime=True) + P_placed = np.linalg.eigvals(A - B.dot(K)) + # No guarantee of the ordering, so sort them + P.sort() + P_placed.sort() + np.testing.assert_array_almost_equal(P, P_placed) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testPlace_varga_discrete_partial_eigs(self): + """" + Check that we can only assign a single eigenvalue in the discrete + time case. + """ + # A matrix has eigenvalues at 1.0 and 0.5. Set alpha = 0.51, and + # check that the eigenvalue at 0.5 is not moved. + A = np.array([[1., 0], [0, 0.5]]) + B = np.array([[5.], [7.]]) + 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) + 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): + S_expected = np.array(np.sqrt(Q * R)) + K_expected = S_expected / R + poles_expected = np.array([-K_expected]) + np.testing.assert_array_almost_equal(S, S_expected) + 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): + A, B, Q, R = 0., 1., 10., 2. + K, S, poles = lqr(A, B, Q, R) + self.check_LQR(K, S, poles, Q, R) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + 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) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_care(self): + #unit test for stabilizing and anti-stabilizing feedbacks + #continuous-time + + A = np.diag([1,-1]) + B = np.identity(2) + Q = np.identity(2) + R = np.identity(2) + S = 0 * B + E = 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) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_dare(self): + #discrete-time + A = np.diag([0.5,2]) + B = np.identity(2) + Q = np.identity(2) + R = np.identity(2) + S = 0 * B + E = 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) + + def tearDown(self): + reset_defaults() + + +def test_suite(): + + status1 = unittest.TestLoader().loadTestsFromTestCase(TestStatefbk) + status2 = unittest.TestLoader().loadTestsFromTestCase(TestStatefbk) + return status1 and status2 + +if __name__ == '__main__': + unittest.main() diff --git a/control/tests/statesp_array_test.py b/control/tests/statesp_array_test.py new file mode 100644 index 000000000..a45e008bc --- /dev/null +++ b/control/tests/statesp_array_test.py @@ -0,0 +1,637 @@ +#!/usr/bin/env python +# +# statesp_test.py - test state space class with use_numpy_matrix(False) +# RMM, 14 Jun 2019 (coverted from statesp_test.py) + +import unittest +import numpy as np +from numpy.linalg import solve +from scipy.linalg import eigvals, block_diag +from control import matlab +from control.statesp import StateSpace, _convertToStateSpace, tf2ss +from control.xferfcn import TransferFunction, ss2tf +from control.lti import evalfr +from control.exception import slycot_check +from control.config import use_numpy_matrix, reset_defaults + +class TestStateSpace(unittest.TestCase): + """Tests for the StateSpace class.""" + + def setUp(self): + """Set up a MIMO system to test operations on.""" + use_numpy_matrix(False) + + # sys1: 3-states square system (2 inputs x 2 outputs) + A322 = [[-3., 4., 2.], + [-1., -3., 0.], + [2., 5., 3.]] + B322 = [[1., 4.], + [-3., -3.], + [-2., 1.]] + C322 = [[4., 2., -3.], + [1., 4., 3.]] + D322 = [[-2., 4.], + [0., 1.]] + self.sys322 = StateSpace(A322, B322, C322, D322) + + # sys1: 2-states square system (2 inputs x 2 outputs) + A222 = [[4., 1.], + [2., -3]] + B222 = [[5., 2.], + [-3., -3.]] + C222 = [[2., -4], + [0., 1.]] + D222 = [[3., 2.], + [1., -1.]] + self.sys222 = StateSpace(A222, B222, C222, D222) + + # sys3: 6 states non square system (2 inputs x 3 outputs) + A623 = np.array([[1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 3, 0, 0, 0], + [0, 0, 0, -4, 0, 0], + [0, 0, 0, 0, -1, 0], + [0, 0, 0, 0, 0, 3]]) + B623 = np.array([[0, -1], + [-1, 0], + [1, -1], + [0, 0], + [0, 1], + [-1, -1]]) + C623 = np.array([[1, 0, 0, 1, 0, 0], + [0, 1, 0, 1, 0, 1], + [0, 0, 1, 0, 0, 1]]) + D623 = np.zeros((3, 2)) + self.sys623 = StateSpace(A623, B623, C623, D623) + + def test_matlab_style_constructor(self): + # Use (deprecated?) matrix-style construction string (w/ warnings off) + import warnings + warnings.filterwarnings("ignore") # turn off warnings + sys = StateSpace("-1 1; 0 2", "0; 1", "1, 0", "0") + warnings.resetwarnings() # put things back to original state + self.assertEqual(sys.A.shape, (2, 2)) + self.assertEqual(sys.B.shape, (2, 1)) + self.assertEqual(sys.C.shape, (1, 2)) + self.assertEqual(sys.D.shape, (1, 1)) + for X in [sys.A, sys.B, sys.C, sys.D]: + self.assertTrue(isinstance(X, np.matrix)) + + def test_pole(self): + """Evaluate the poles of a MIMO system.""" + + p = np.sort(self.sys322.pole()) + true_p = np.sort([3.34747678408874, + -3.17373839204437 + 1.47492908003839j, + -3.17373839204437 - 1.47492908003839j]) + + np.testing.assert_array_almost_equal(p, true_p) + + def test_zero_empty(self): + """Test to make sure zero() works with no zeros in system.""" + sys = _convertToStateSpace(TransferFunction([1], [1, 2, 1])) + np.testing.assert_array_equal(sys.zero(), np.array([])) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_zero_siso(self): + """Evaluate the zeros of a SISO system.""" + # extract only first input / first output system of sys222. This system is denoted sys111 + # or tf111 + tf111 = ss2tf(self.sys222) + sys111 = tf2ss(tf111[0, 0]) + + # compute zeros as root of the characteristic polynomial at the numerator of tf111 + # this method is simple and assumed as valid in this test + true_z = np.sort(tf111[0, 0].zero()) + # Compute the zeros through ab08nd, which is tested here + z = np.sort(sys111.zero()) + + np.testing.assert_almost_equal(true_z, z) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_zero_mimo_sys322_square(self): + """Evaluate the zeros of a square MIMO system.""" + + z = np.sort(self.sys322.zero()) + true_z = np.sort([44.41465, -0.490252, -5.924398]) + np.testing.assert_array_almost_equal(z, true_z) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_zero_mimo_sys222_square(self): + """Evaluate the zeros of a square MIMO system.""" + + z = np.sort(self.sys222.zero()) + true_z = np.sort([-10.568501, 3.368501]) + np.testing.assert_array_almost_equal(z, true_z) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_zero_mimo_sys623_non_square(self): + """Evaluate the zeros of a non square MIMO system.""" + + z = np.sort(self.sys623.zero()) + true_z = np.sort([2., -1.]) + np.testing.assert_array_almost_equal(z, true_z) + + def test_add_ss(self): + """Add two MIMO systems.""" + + A = [[-3., 4., 2., 0., 0.], [-1., -3., 0., 0., 0.], + [2., 5., 3., 0., 0.], [0., 0., 0., 4., 1.], [0., 0., 0., 2., -3.]] + B = [[1., 4.], [-3., -3.], [-2., 1.], [5., 2.], [-3., -3.]] + C = [[4., 2., -3., 2., -4.], [1., 4., 3., 0., 1.]] + D = [[1., 6.], [1., 0.]] + + sys = self.sys322 + self.sys222 + + np.testing.assert_array_almost_equal(sys.A, A) + np.testing.assert_array_almost_equal(sys.B, B) + np.testing.assert_array_almost_equal(sys.C, C) + np.testing.assert_array_almost_equal(sys.D, D) + + def test_subtract_ss(self): + """Subtract two MIMO systems.""" + + A = [[-3., 4., 2., 0., 0.], [-1., -3., 0., 0., 0.], + [2., 5., 3., 0., 0.], [0., 0., 0., 4., 1.], [0., 0., 0., 2., -3.]] + B = [[1., 4.], [-3., -3.], [-2., 1.], [5., 2.], [-3., -3.]] + C = [[4., 2., -3., -2., 4.], [1., 4., 3., 0., -1.]] + D = [[-5., 2.], [-1., 2.]] + + sys = self.sys322 - self.sys222 + + np.testing.assert_array_almost_equal(sys.A, A) + np.testing.assert_array_almost_equal(sys.B, B) + np.testing.assert_array_almost_equal(sys.C, C) + np.testing.assert_array_almost_equal(sys.D, D) + + def test_multiply_ss(self): + """Multiply two MIMO systems.""" + + A = [[4., 1., 0., 0., 0.], [2., -3., 0., 0., 0.], [2., 0., -3., 4., 2.], + [-6., 9., -1., -3., 0.], [-4., 9., 2., 5., 3.]] + B = [[5., 2.], [-3., -3.], [7., -2.], [-12., -3.], [-5., -5.]] + C = [[-4., 12., 4., 2., -3.], [0., 1., 1., 4., 3.]] + D = [[-2., -8.], [1., -1.]] + + sys = self.sys322 * self.sys222 + + np.testing.assert_array_almost_equal(sys.A, A) + np.testing.assert_array_almost_equal(sys.B, B) + np.testing.assert_array_almost_equal(sys.C, C) + np.testing.assert_array_almost_equal(sys.D, D) + + def test_evalfr(self): + """Evaluate the frequency response at one frequency.""" + + A = [[-2, 0.5], [0.5, -0.3]] + B = [[0.3, -1.3], [0.1, 0.]] + C = [[0., 0.1], [-0.3, -0.2]] + D = [[0., -0.8], [-0.3, 0.]] + sys = StateSpace(A, B, C, D) + + resp = [[4.37636761487965e-05 - 0.0152297592997812j, + -0.792603938730853 + 0.0261706783369803j], + [-0.331544857768052 + 0.0576105032822757j, + 0.128919037199125 - 0.143824945295405j]] + + # Correct versions of the call + np.testing.assert_almost_equal(evalfr(sys, 1j), resp) + np.testing.assert_almost_equal(sys._evalfr(1.), resp) + + # Deprecated version of the call (should generate warning) + import warnings + with warnings.catch_warnings(record=True) as w: + # Set up warnings filter to only show warnings in control module + warnings.filterwarnings("ignore") + warnings.filterwarnings("always", module="control") + + # Make sure that we get a pending deprecation warning + sys.evalfr(1.) + assert len(w) == 1 + assert issubclass(w[-1].category, PendingDeprecationWarning) + + # Leave the warnings filter like we found it + warnings.resetwarnings() + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_freq_resp(self): + """Evaluate the frequency response at multiple frequencies.""" + + A = [[-2, 0.5], [0.5, -0.3]] + B = [[0.3, -1.3], [0.1, 0.]] + C = [[0., 0.1], [-0.3, -0.2]] + D = [[0., -0.8], [-0.3, 0.]] + sys = StateSpace(A, B, C, D) + + true_mag = [[[0.0852992637230322, 0.00103596611395218], + [0.935374692849736, 0.799380720864549]], + [[0.55656854563842, 0.301542699860857], + [0.609178071542849, 0.0382108097985257]]] + true_phase = [[[-0.566195599644593, -1.68063565332582], + [3.0465958317514, 3.14141384339534]], + [[2.90457947657161, 3.10601268291914], + [-0.438157380501337, -1.40720969147217]]] + true_omega = [0.1, 10.] + + mag, phase, omega = sys.freqresp(true_omega) + + np.testing.assert_almost_equal(mag, true_mag) + np.testing.assert_almost_equal(phase, true_phase) + np.testing.assert_equal(omega, true_omega) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_minreal(self): + """Test a minreal model reduction.""" + # A = [-2, 0.5, 0; 0.5, -0.3, 0; 0, 0, -0.1] + A = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]] + # B = [0.3, -1.3; 0.1, 0; 1, 0] + B = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]] + # C = [0, 0.1, 0; -0.3, -0.2, 0] + C = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]] + # D = [0 -0.8; -0.3 0] + D = [[0., -0.8], [-0.3, 0.]] + # sys = ss(A, B, C, D) + + sys = StateSpace(A, B, C, D) + sysr = sys.minreal() + self.assertEqual(sysr.states, 2) + self.assertEqual(sysr.inputs, sys.inputs) + self.assertEqual(sysr.outputs, sys.outputs) + np.testing.assert_array_almost_equal( + eigvals(sysr.A), [-2.136154, -0.1638459]) + + def test_append_ss(self): + """Test appending two state-space systems.""" + A1 = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]] + B1 = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]] + C1 = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]] + D1 = [[0., -0.8], [-0.3, 0.]] + A2 = [[-1.]] + B2 = [[1.2]] + C2 = [[0.5]] + D2 = [[0.4]] + A3 = [[-2, 0.5, 0, 0], [0.5, -0.3, 0, 0], [0, 0, -0.1, 0], + [0, 0, 0., -1.]] + B3 = [[0.3, -1.3, 0], [0.1, 0., 0], [1.0, 0.0, 0], [0., 0, 1.2]] + C3 = [[0., 0.1, 0.0, 0.0], [-0.3, -0.2, 0.0, 0.0], [0., 0., 0., 0.5]] + D3 = [[0., -0.8, 0.], [-0.3, 0., 0.], [0., 0., 0.4]] + sys1 = StateSpace(A1, B1, C1, D1) + sys2 = StateSpace(A2, B2, C2, D2) + sys3 = StateSpace(A3, B3, C3, D3) + sys3c = sys1.append(sys2) + np.testing.assert_array_almost_equal(sys3.A, sys3c.A) + np.testing.assert_array_almost_equal(sys3.B, sys3c.B) + np.testing.assert_array_almost_equal(sys3.C, sys3c.C) + np.testing.assert_array_almost_equal(sys3.D, sys3c.D) + + def test_append_tf(self): + """Test appending a state-space system with a tf""" + A1 = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]] + B1 = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]] + C1 = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]] + D1 = [[0., -0.8], [-0.3, 0.]] + s = TransferFunction([1, 0], [1]) + h = 1 / (s + 1) / (s + 2) + sys1 = StateSpace(A1, B1, C1, D1) + sys2 = _convertToStateSpace(h) + sys3c = sys1.append(sys2) + np.testing.assert_array_almost_equal(sys1.A, sys3c.A[:3, :3]) + np.testing.assert_array_almost_equal(sys1.B, sys3c.B[:3, :2]) + np.testing.assert_array_almost_equal(sys1.C, sys3c.C[:2, :3]) + np.testing.assert_array_almost_equal(sys1.D, sys3c.D[:2, :2]) + np.testing.assert_array_almost_equal(sys2.A, sys3c.A[3:, 3:]) + np.testing.assert_array_almost_equal(sys2.B, sys3c.B[3:, 2:]) + np.testing.assert_array_almost_equal(sys2.C, sys3c.C[2:, 3:]) + np.testing.assert_array_almost_equal(sys2.D, sys3c.D[2:, 2:]) + np.testing.assert_array_almost_equal(sys3c.A[:3, 3:], np.zeros((3, 2))) + np.testing.assert_array_almost_equal(sys3c.A[3:, :3], np.zeros((2, 3))) + + def test_array_access_ss(self): + + sys1 = StateSpace([[1., 2.], [3., 4.]], + [[5., 6.], [6., 8.]], + [[9., 10.], [11., 12.]], + [[13., 14.], [15., 16.]], 1) + + sys1_11 = sys1[0, 1] + np.testing.assert_array_almost_equal(sys1_11.A, + sys1.A) + np.testing.assert_array_almost_equal(sys1_11.B, + sys1.B[:, [1]]) + np.testing.assert_array_almost_equal(sys1_11.C, + sys1.C[[0], :]) + np.testing.assert_array_almost_equal(sys1_11.D, sys1.D[0,1]) + + assert sys1.dt == sys1_11.dt + + def test_dc_gain_cont(self): + """Test DC gain for continuous-time state-space systems.""" + sys = StateSpace(-2., 6., 5., 0) + np.testing.assert_equal(sys.dcgain(), 15.) + + sys2 = StateSpace(-2, [[6., 4.]], [[5.], [7.], [11]], np.zeros((3, 2))) + expected = np.array([[15., 10.], [21., 14.], [33., 22.]]) + np.testing.assert_array_equal(sys2.dcgain(), expected) + + sys3 = StateSpace(0., 1., 1., 0.) + np.testing.assert_equal(sys3.dcgain(), np.nan) + + def test_dc_gain_discr(self): + """Test DC gain for discrete-time state-space systems.""" + # static gain + sys = StateSpace([], [], [], 2, True) + np.testing.assert_equal(sys.dcgain(), 2) + + # averaging filter + sys = StateSpace(0.5, 0.5, 1, 0, True) + np.testing.assert_almost_equal(sys.dcgain(), 1) + + # differencer + sys = StateSpace(0, 1, -1, 1, True) + np.testing.assert_equal(sys.dcgain(), 0) + + # summer + sys = StateSpace(1, 1, 1, 0, True) + np.testing.assert_equal(sys.dcgain(), np.nan) + + def test_dc_gain_integrator(self): + """DC gain when eigenvalue at DC returns appropriately sized array of nan.""" + # the SISO case is also tested in test_dc_gain_{cont,discr} + import itertools + # iterate over input and output sizes, and continuous (dt=None) and discrete (dt=True) time + for inputs, outputs, dt in itertools.product(range(1, 6), range(1, 6), [None, True]): + states = max(inputs, outputs) + + # a matrix that is singular at DC, and has no "useless" states as in + # _remove_useless_states + a = np.triu(np.tile(2, (states, states))) + # eigenvalues all +2, except for ... + a[0, 0] = 0 if dt is None else 1 + b = np.eye(max(inputs, states))[:states, :inputs] + c = np.eye(max(outputs, states))[:outputs, :states] + d = np.zeros((outputs, inputs)) + sys = StateSpace(a, b, c, d, dt) + dc = np.squeeze(np.tile(np.nan, (outputs, inputs))) + np.testing.assert_array_equal(dc, sys.dcgain()) + + def test_scalar_static_gain(self): + """Regression: can we create a scalar static gain?""" + g1 = StateSpace([], [], [], [2]) + g2 = StateSpace([], [], [], [3]) + + # make sure StateSpace internals, specifically ABC matrix + # sizes, are OK for LTI operations + g3 = g1 * g2 + self.assertEqual(6, g3.D[0, 0]) + g4 = g1 + g2 + self.assertEqual(5, g4.D[0, 0]) + g5 = g1.feedback(g2) + np.testing.assert_array_almost_equal(2. / 7, g5.D[0, 0]) + g6 = g1.append(g2) + np.testing.assert_array_equal(np.diag([2, 3]), g6.D) + + def test_matrix_static_gain(self): + """Regression: can we create matrix static gains?""" + d1 = np.array([[1, 2, 3], [4, 5, 6]]) + d2 = np.array([[7, 8], [9, 10], [11, 12]]) + g1 = StateSpace([], [], [], d1) + + # _remove_useless_states was making A = [[0]] + self.assertEqual((0, 0), g1.A.shape) + + g2 = StateSpace([], [], [], d2) + g3 = StateSpace([], [], [], d2.T) + + h1 = g1 * g2 + np.testing.assert_array_equal(np.dot(d1, d2), h1.D) + h2 = g1 + g3 + np.testing.assert_array_equal(d1 + d2.T, h2.D) + h3 = g1.feedback(g2) + np.testing.assert_array_almost_equal( + solve(np.eye(2) + np.dot(d1, d2), d1), h3.D) + h4 = g1.append(g2) + np.testing.assert_array_equal(block_diag(d1, d2), h4.D) + + def test_remove_useless_states(self): + """Regression: _remove_useless_states gives correct ABC sizes.""" + g1 = StateSpace(np.zeros((3, 3)), + np.zeros((3, 4)), + np.zeros((5, 3)), + np.zeros((5, 4))) + self.assertEqual((0, 0), g1.A.shape) + self.assertEqual((0, 4), g1.B.shape) + self.assertEqual((5, 0), g1.C.shape) + self.assertEqual((5, 4), g1.D.shape) + self.assertEqual(0, g1.states) + + def test_bad_empty_matrices(self): + """Mismatched ABCD matrices when some are empty.""" + self.assertRaises(ValueError, StateSpace, [1], [], [], [1]) + self.assertRaises(ValueError, StateSpace, [1], [1], [], [1]) + self.assertRaises(ValueError, StateSpace, [1], [], [1], [1]) + self.assertRaises(ValueError, StateSpace, [], [1], [], [1]) + self.assertRaises(ValueError, StateSpace, [], [1], [1], [1]) + self.assertRaises(ValueError, StateSpace, [], [], [1], [1]) + self.assertRaises(ValueError, StateSpace, [1], [1], [1], []) + + def test_minreal_static_gain(self): + """Regression: minreal on static gain was failing.""" + g1 = StateSpace([], [], [], [1]) + g2 = g1.minreal() + np.testing.assert_array_equal(g1.A, g2.A) + np.testing.assert_array_equal(g1.B, g2.B) + np.testing.assert_array_equal(g1.C, g2.C) + np.testing.assert_array_equal(g1.D, g2.D) + + def test_empty(self): + """Regression: can we create an empty StateSpace object?""" + g1 = StateSpace([], [], [], []) + self.assertEqual(0, g1.states) + self.assertEqual(0, g1.inputs) + self.assertEqual(0, g1.outputs) + + def test_matrix_to_state_space(self): + """_convertToStateSpace(matrix) gives ss([],[],[],D)""" + D = np.array([[1, 2, 3], [4, 5, 6]]) + g = _convertToStateSpace(D) + + def empty(shape): + m = np.array([]) + m.shape = shape + return m + np.testing.assert_array_equal(empty((0, 0)), g.A) + np.testing.assert_array_equal(empty((0, D.shape[1])), g.B) + np.testing.assert_array_equal(empty((D.shape[0], 0)), g.C) + np.testing.assert_array_equal(D, g.D) + + def test_lft(self): + """ test lft function with result obtained from matlab implementation""" + # test case + A = [[1, 2, 3], + [1, 4, 5], + [2, 3, 4]] + B = [[0, 2], + [5, 6], + [5, 2]] + C = [[1, 4, 5], + [2, 3, 0]] + D = [[0, 0], + [3, 0]] + P = StateSpace(A, B, C, D) + Ak = [[0, 2, 3], + [2, 3, 5], + [2, 1, 9]] + Bk = [[1, 1], + [2, 3], + [9, 4]] + Ck = [[1, 4, 5], + [2, 3, 6]] + Dk = [[0, 2], + [0, 0]] + K = StateSpace(Ak, Bk, Ck, Dk) + + # case 1 + pk = P.lft(K, 2, 1) + Amatlab = [1, 2, 3, 4, 6, 12, 1, 4, 5, 17, 38, 61, 2, 3, 4, 9, 26, 37, 2, 3, 0, 3, 14, 18, 4, 6, 0, 8, 27, 35, 18, 27, 0, 29, 109, 144] + Bmatlab = [0, 10, 10, 7, 15, 58] + Cmatlab = [1, 4, 5, 0, 0, 0] + Dmatlab = [0] + np.testing.assert_allclose(np.array(pk.A).reshape(-1), Amatlab) + np.testing.assert_allclose(np.array(pk.B).reshape(-1), Bmatlab) + np.testing.assert_allclose(np.array(pk.C).reshape(-1), Cmatlab) + np.testing.assert_allclose(np.array(pk.D).reshape(-1), Dmatlab) + + # case 2 + pk = P.lft(K) + Amatlab = [1, 2, 3, 4, 6, 12, -3, -2, 5, 11, 14, 31, -2, -3, 4, 3, 2, 7, 0.6, 3.4, 5, -0.6, -0.4, 0, 0.8, 6.2, 10, 0.2, -4.2, -4, 7.4, 33.6, 45, -0.4, -8.6, -3] + Bmatlab = [] + Cmatlab = [] + Dmatlab = [] + np.testing.assert_allclose(np.array(pk.A).reshape(-1), Amatlab) + np.testing.assert_allclose(np.array(pk.B).reshape(-1), Bmatlab) + np.testing.assert_allclose(np.array(pk.C).reshape(-1), Cmatlab) + np.testing.assert_allclose(np.array(pk.D).reshape(-1), Dmatlab) + + def test_horner(self): + """Test horner() function""" + # Make sure we can compute the transfer function at a complex value + self.sys322.horner(1.+1.j) + + # Make sure result agrees with frequency response + mag, phase, omega = self.sys322.freqresp([1]) + np.testing.assert_array_almost_equal( + self.sys322.horner(1.j), + mag[:,:,0] * np.exp(1.j * phase[:,:,0])) + + def tearDown(self): + reset_defaults() # reset configuration defaults + + +class TestRss(unittest.TestCase): + """These are tests for the proper functionality of statesp.rss.""" + + def setUp(self): + use_numpy_matrix(False) + + # Number of times to run each of the randomized tests. + self.numTests = 100 + # Maxmimum number of states to test + 1 + self.maxStates = 10 + # Maximum number of inputs and outputs to test + 1 + self.maxIO = 5 + + def test_shape(self): + """Test that rss outputs have the right state, input, and output size.""" + + for states in range(1, self.maxStates): + for inputs in range(1, self.maxIO): + for outputs in range(1, self.maxIO): + sys = matlab.rss(states, outputs, inputs) + self.assertEqual(sys.states, states) + self.assertEqual(sys.inputs, inputs) + self.assertEqual(sys.outputs, outputs) + + def test_pole(self): + """Test that the poles of rss outputs have a negative real part.""" + + for states in range(1, self.maxStates): + for inputs in range(1, self.maxIO): + for outputs in range(1, self.maxIO): + sys = matlab.rss(states, outputs, inputs) + p = sys.pole() + for z in p: + self.assertTrue(z.real < 0) + + def tearDown(self): + reset_defaults() # reset configuration defaults + + +class TestDrss(unittest.TestCase): + """These are tests for the proper functionality of statesp.drss.""" + + def setUp(self): + use_numpy_matrix(False) + + # Number of times to run each of the randomized tests. + self.numTests = 100 + # Maximum number of states to test + 1 + self.maxStates = 10 + # Maximum number of inputs and outputs to test + 1 + self.maxIO = 5 + + def test_shape(self): + """Test that drss outputs have the right state, input, and output size.""" + + for states in range(1, self.maxStates): + for inputs in range(1, self.maxIO): + for outputs in range(1, self.maxIO): + sys = matlab.drss(states, outputs, inputs) + self.assertEqual(sys.states, states) + self.assertEqual(sys.inputs, inputs) + self.assertEqual(sys.outputs, outputs) + + def test_pole(self): + """Test that the poles of drss outputs have less than unit magnitude.""" + + for states in range(1, self.maxStates): + for inputs in range(1, self.maxIO): + for outputs in range(1, self.maxIO): + sys = matlab.drss(states, outputs, inputs) + p = sys.pole() + for z in p: + self.assertTrue(abs(z) < 1) + + def test_pole_static(self): + """Regression: pole() of static gain is empty array.""" + np.testing.assert_array_equal(np.array([]), + StateSpace([], [], [], [[1]]).pole()) + + def test_copy_constructor(self): + # Create a set of matrices for a simple linear system + A = np.array([[-1]]) + B = np.array([[1]]) + C = np.array([[1]]) + D = np.array([[0]]) + + # Create the first linear system and a copy + linsys = StateSpace(A, B, C, D) + cpysys = StateSpace(linsys) + + # Change the original A matrix + A[0, 0] = -2 + np.testing.assert_array_equal(linsys.A, [[-1]]) # original value + np.testing.assert_array_equal(cpysys.A, [[-1]]) # original value + + # Change the A matrix for the original system + linsys.A[0, 0] = -3 + np.testing.assert_array_equal(cpysys.A, [[-1]]) # original value + + def tearDown(self): + reset_defaults() # reset configuration defaults + +def suite(): + return unittest.TestLoader().loadTestsFromTestCase(TestStateSpace) + + +if __name__ == "__main__": + unittest.main() From 4c9c2c876f3c40c872d5af5038987b4ba6990537 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 16 Jun 2019 17:44:18 -0700 Subject: [PATCH 2/4] fixed docstring example error + added TODO's in response to review --- control/bdalg.py | 2 +- control/frdata.py | 2 ++ control/mateqn.py | 2 +- control/statefbk.py | 1 + 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/control/bdalg.py b/control/bdalg.py index 57e8763ff..0fcadf5d3 100644 --- a/control/bdalg.py +++ b/control/bdalg.py @@ -326,7 +326,7 @@ def connect(sys, Q, inputv, outputv): >>> sys1 = ss([[1., -2], [3., -4]], [[5.], [7]], [[6, 8]], [[9.]]) >>> sys2 = ss([[-1.]], [[1.]], [[1.]], [[0.]]) >>> sys = append(sys1, sys2) - >>> Q = [[1, 2], [2, -1]]) # negative feedback interconnection + >>> Q = [[1, 2], [2, -1]] # negative feedback interconnection >>> sysc = connect(sys, Q, [2], [1, 2]) """ diff --git a/control/frdata.py b/control/frdata.py index 1a29df0d8..df2922113 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -439,6 +439,8 @@ def feedback(self, other=1, sign=-1): dtype=complex) # TODO: vectorize this # TODO: handle omega re-mapping + # TODO: is there a reason to use linalg.solve instead of linalg.inv? + # https://github.com/python-control/python-control/pull/314#discussion_r294075154 for k, w in enumerate(other.omega): fresp[:, :, k] = np.dot( self.fresp[:, :, k], diff --git a/control/mateqn.py b/control/mateqn.py index 19f9930a4..604e94e21 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -41,7 +41,7 @@ Author: Bjorn Olofsson """ -from scipy import shape, size, array, asarray, copy, zeros, eye, dot +from numpy import shape, size, array, asarray, copy, zeros, eye, dot from scipy.linalg import eigvals, solve_discrete_are, solve from .exception import ControlSlycot, ControlArgument from .statesp import _ssmatrix diff --git a/control/statefbk.py b/control/statefbk.py index 5f4d6e09d..cd69190d3 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -252,6 +252,7 @@ def acker(A, B, poles): p = np.real(np.poly(poles)) # Place the poles using Ackermann's method + # TODO: compute pmat using Horner's method (O(n) instead of O(n^2)) n = np.size(p) pmat = p[n-1] * np.linalg.matrix_power(a, 0) for i in np.arange(1,n): From 4f48c5df380ce055995531bc1b8b682d38d75a07 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 23 Jun 2019 12:54:57 -0700 Subject: [PATCH 3/4] allow D=0 to broadcast to correct size for MIMO statespace systems --- control/statesp.py | 4 ++++ control/tests/statesp_test.py | 23 +++++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/control/statesp.py b/control/statesp.py index d401966d3..820e40728 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -182,9 +182,13 @@ def __init__(self, *args, **kw): # Process keyword arguments remove_useless = kw.get('remove_useless', True) + # Convert all matrices to standard form A = _ssmatrix(A) B = _ssmatrix(B, axis=0) C = _ssmatrix(C, axis=1) + if np.isscalar(D) and D == 0 and B.shape[1] > 0 and C.shape[0] > 0: + # If D is a scalar zero, broadcast it to the proper size + D = np.zeros((C.shape[0], B.shape[1])) D = _ssmatrix(D) # TODO: use super here? diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 30fc2c913..191271da4 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -63,6 +63,29 @@ def setUp(self): D623 = np.zeros((3, 2)) self.sys623 = StateSpace(A623, B623, C623, D623) + def test_D_broadcast(self): + """Test broadcast of D=0 to the right shape""" + # Giving D as a scalar 0 should broadcast to the right shape + sys = StateSpace(self.sys623.A, self.sys623.B, self.sys623.C, 0) + np.testing.assert_array_equal(self.sys623.D, sys.D) + + # Giving D as a matrix of the wrong size should generate an error + with self.assertRaises(ValueError): + sys = StateSpace(sys.A, sys.B, sys.C, np.array([[0]])) + + # Make sure that empty systems still work + sys = StateSpace([], [], [], 1) + np.testing.assert_array_equal(sys.D, [[1]]) + + sys = StateSpace([], [], [], [[0]]) + np.testing.assert_array_equal(sys.D, [[0]]) + + sys = StateSpace([], [], [], [0]) + np.testing.assert_array_equal(sys.D, [[0]]) + + sys = StateSpace([], [], [], 0) + np.testing.assert_array_equal(sys.D, [[0]]) + def test_pole(self): """Evaluate the poles of a MIMO system.""" From 7c9d9a6ee283ab134ee994710d7a2d07a65e3761 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 30 Jun 2019 09:55:55 -0700 Subject: [PATCH 4/4] change np.bmat -> np.block in timeresp.py --- control/timeresp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/timeresp.py b/control/timeresp.py index df3dcf270..8e15251dd 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -333,7 +333,7 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, # [ u(dt) ] = exp [ 0 0 I ] [ u0 ] # [u1 - u0] [ 0 0 0 ] [u1 - u0] - M = np.bmat([[A * dt, B * dt, np.zeros((n_states, n_inputs))], + M = np.block([[A * dt, B * dt, np.zeros((n_states, n_inputs))], [np.zeros((n_inputs, n_states + n_inputs)), np.identity(n_inputs)], [np.zeros((n_inputs, n_states + 2 * n_inputs))]])