diff --git a/control/canonical.py b/control/canonical.py index bd9ee4a94..341ec5da4 100644 --- a/control/canonical.py +++ b/control/canonical.py @@ -1,17 +1,21 @@ # canonical.py - functions for converting systems to canonical forms # RMM, 10 Nov 2012 -from .exception import ControlNotImplemented +from .exception import ControlNotImplemented, ControlSlycot from .lti import issiso -from .statesp import StateSpace +from .statesp import StateSpace, _convert_to_statespace from .statefbk import ctrb, obsv +import numpy as np + from numpy import zeros, zeros_like, shape, poly, iscomplex, vstack, hstack, dot, \ - transpose, empty + transpose, empty, finfo, float64 from numpy.linalg import solve, matrix_rank, eig +from scipy.linalg import schur + __all__ = ['canonical_form', 'reachable_form', 'observable_form', 'modal_form', - 'similarity_transform'] + 'similarity_transform', 'bdschur'] def canonical_form(xsys, form='reachable'): """Convert a system into canonical form @@ -20,7 +24,7 @@ def canonical_form(xsys, form='reachable'): ---------- xsys : StateSpace object System to be transformed, with state 'x' - form : String + form : str Canonical form for transformation. Chosen from: * 'reachable' - reachable canonical form * 'observable' - observable canonical form @@ -30,7 +34,7 @@ def canonical_form(xsys, form='reachable'): ------- zsys : StateSpace object System in desired canonical form, with state 'z' - T : matrix + T : (M, M) real ndarray Coordinate transformation matrix, z = T * x """ @@ -59,7 +63,7 @@ def reachable_form(xsys): ------- zsys : StateSpace object System in reachable canonical form, with state `z` - T : matrix + T : (M, M) real ndarray Coordinate transformation: z = T * x """ # Check to make sure we have a SISO system @@ -113,7 +117,7 @@ def observable_form(xsys): ------- zsys : StateSpace object System in observable canonical form, with state `z` - T : matrix + T : (M, M) real ndarray Coordinate transformation: z = T * x """ # Check to make sure we have a SISO system @@ -149,97 +153,298 @@ def observable_form(xsys): return zsys, Tzx -def modal_form(xsys): - """Convert a system into modal canonical form + +def similarity_transform(xsys, T, timescale=1, inverse=False): + """Perform a similarity transformation, with option time rescaling. + + Transform a linear state space system to a new state space representation + z = T x, or x = T z, where T is an invertible matrix. Parameters ---------- xsys : StateSpace object - System to be transformed, with state `x` + System to transform + T : (M, M) array_like + The matrix `T` defines the new set of coordinates z = T x. + timescale : float, optional + If present, also rescale the time unit to tau = timescale * t + inverse: boolean, optional + If True (default), transform so z = T x. If False, transform + so x = T z. Returns ------- zsys : StateSpace object - System in modal canonical form, with state `z` - T : matrix - Coordinate transformation: z = T * x - """ - # Check to make sure we have a SISO system - if not issiso(xsys): - raise ControlNotImplemented( - "Canonical forms for MIMO systems not yet supported") + System in transformed coordinates, with state 'z' + """ # Create a new system, starting with a copy of the old one zsys = StateSpace(xsys) - # Calculate eigenvalues and matrix of eigenvectors Tzx, - eigval, eigvec = eig(xsys.A) + T = np.atleast_2d(T) - # Eigenvalues and corresponding eigenvectors are not sorted, - # thus modal transformation is ambiguous - # Sort eigenvalues and vectors from largest to smallest eigenvalue - idx = eigval.argsort()[::-1] - eigval = eigval[idx] - eigvec = eigvec[:,idx] + # Define a function to compute the right inverse (solve x M = y) + def rsolve(M, y): + return transpose(solve(transpose(M), transpose(y))) - # If all eigenvalues are real, the matrix of eigenvectors is Tzx directly - if not iscomplex(eigval).any(): - Tzx = eigvec + # Update the system matrices + if not inverse: + zsys.A = rsolve(T, dot(T, zsys.A)) / timescale + zsys.B = dot(T, zsys.B) / timescale + zsys.C = rsolve(T, zsys.C) else: - # A is an arbitrary semisimple matrix - - # Keep track of complex conjugates (need only one) - lst_conjugates = [] - Tzx = empty((0, xsys.A.shape[0])) # empty zero-height row matrix - for val, vec in zip(eigval, eigvec.T): - if iscomplex(val): - if val not in lst_conjugates: - lst_conjugates.append(val.conjugate()) - Tzx = vstack((Tzx, vec.real, vec.imag)) - else: - # if conjugate has already been seen, skip this eigenvalue - lst_conjugates.remove(val) - else: - Tzx = vstack((Tzx, vec.real)) - Tzx = Tzx.T + zsys.A = solve(T, zsys.A).dot(T) / timescale + zsys.B = solve(T, zsys.B) / timescale + zsys.C = zsys.C.dot(T) - # Generate the system matrices for the desired canonical form - zsys.A = solve(Tzx, xsys.A).dot(Tzx) - zsys.B = solve(Tzx, xsys.B) - zsys.C = xsys.C.dot(Tzx) + return zsys - return zsys, Tzx +_IM_ZERO_TOL = np.finfo(np.float64).eps ** 0.5 +_PMAX_SEARCH_TOL = 1.001 -def similarity_transform(xsys, T, timescale=1): - """Perform a similarity transformation, with option time rescaling. - Transform a linear state space system to a new state space representation - z = T x, where T is an invertible matrix. +def _bdschur_defective(blksizes, eigvals): + """Check for defective modal decomposition Parameters ---------- - T : 2D invertible array - The matrix `T` defines the new set of coordinates z = T x. - timescale : float - If present, also rescale the time unit to tau = timescale * t + blksizes: (N,) int ndarray + size of Schur blocks + eigvals: (M,) real or complex ndarray + Eigenvalues Returns ------- - zsys : StateSpace object - System in transformed coordinates, with state 'z' + True iff Schur blocks are defective. + blksizes, eigvals are the 3rd and 4th results returned by mb03rd. """ - # Create a new system, starting with a copy of the old one - zsys = StateSpace(xsys) + if any(blksizes > 2): + return True - # Define a function to compute the right inverse (solve x M = y) - def rsolve(M, y): - return transpose(solve(transpose(M), transpose(y))) + if all(blksizes == 1): + return False - # Update the system matrices - zsys.A = rsolve(T, dot(T, zsys.A)) / timescale - zsys.B = dot(T, zsys.B) / timescale - zsys.C = rsolve(T, zsys.C) + # check eigenvalues associated with blocks of size 2 + init_idxs = np.cumsum(np.hstack([0, blksizes[:-1]])) + blk_idx2 = blksizes == 2 - return zsys + im = eigvals[init_idxs[blk_idx2]].imag + re = eigvals[init_idxs[blk_idx2]].real + + if any(abs(im) < _IM_ZERO_TOL * abs(re)): + return True + + return False + + +def _bdschur_condmax_search(aschur, tschur, condmax): + """Block-diagonal Schur decomposition search up to condmax + + Iterates mb03rd with different pmax values until: + - result is non-defective; + - or condition number of similarity transform is unchanging despite large pmax; + - or condition number of similarity transform is close to condmax. + + Parameters + ---------- + aschur: (N, N) real ndarray + Real Schur-form matrix + tschur: (N, N) real ndarray + Orthogonal transformation giving aschur from some initial matrix a + condmax: float + Maximum condition number of final transformation. Must be >= 1. + + Returns + ------- + amodal: (N, N) real ndarray + block diagonal Schur form + tmodal: (N, N) real ndarray + similarity transformation give amodal from aschur + blksizes: (M,) int ndarray + Array of Schur block sizes + eigvals: (N,) real or complex ndarray + Eigenvalues of amodal (and a, etc.) + + Notes + ----- + Outputs as for slycot.mb03rd + + aschur, tschur are as returned by scipy.linalg.schur. + """ + try: + from slycot import mb03rd + except ImportError: + raise ControlSlycot("can't find slycot module 'mb03rd'") + + # see notes on RuntimeError below + pmaxlower = None + + # get lower bound; try condmax ** 0.5 first + pmaxlower = condmax ** 0.5 + amodal, tmodal, blksizes, eigvals = mb03rd(aschur.shape[0], aschur, tschur, pmax=pmaxlower) + if np.linalg.cond(tmodal) <= condmax: + reslower = amodal, tmodal, blksizes, eigvals + else: + pmaxlower = 1.0 + amodal, tmodal, blksizes, eigvals = mb03rd(aschur.shape[0], aschur, tschur, pmax=pmaxlower) + cond = np.linalg.cond(tmodal) + if cond > condmax: + msg = 'minimum cond={} > condmax={}; try increasing condmax'.format(cond, condmax) + raise RuntimeError(msg) + + pmax = pmaxlower + + # phase 1: search for upper bound on pmax + for i in range(50): + amodal, tmodal, blksizes, eigvals = mb03rd(aschur.shape[0], aschur, tschur, pmax=pmax) + cond = np.linalg.cond(tmodal) + if cond < condmax: + pmaxlower = pmax + reslower = amodal, tmodal, blksizes, eigvals + else: + # upper bound found; go to phase 2 + pmaxupper = pmax + break + + if _bdschur_defective(blksizes, eigvals): + pmax *= 2 + else: + return amodal, tmodal, blksizes, eigvals + else: + # no upper bound found; return current result + return reslower + + # phase 2: bisection search + for i in range(50): + pmax = (pmaxlower * pmaxupper) ** 0.5 + amodal, tmodal, blksizes, eigvals = mb03rd(aschur.shape[0], aschur, tschur, pmax=pmax) + cond = np.linalg.cond(tmodal) + + if cond < condmax: + if not _bdschur_defective(blksizes, eigvals): + return amodal, tmodal, blksizes, eigvals + pmaxlower = pmax + reslower = amodal, tmodal, blksizes, eigvals + else: + pmaxupper = pmax + + if pmaxupper / pmaxlower < _PMAX_SEARCH_TOL: + # hit search limit + return reslower + else: + raise ValueError('bisection failed to converge; pmaxlower={}, pmaxupper={}'.format(pmaxlower, pmaxupper)) + + +def bdschur(a, condmax=None, sort=None): + """Block-diagonal Schur decomposition + + Parameters + ---------- + a : (M, M) array_like + Real matrix to decompose + condmax : None or float, optional + If None (default), use 1/sqrt(eps), which is approximately 1e8 + sort : {None, 'continuous', 'discrete'} + Block sorting; see below. + + Returns + ------- + amodal : (M, M) real ndarray + Block-diagonal Schur decomposition of `a` + tmodal : (M, M) real ndarray + Similarity transform relating `a` and `amodal` + blksizes : (N,) int ndarray + Array of Schur block sizes + + Notes + ----- + If `sort` is None, the blocks are not sorted. + + If `sort` is 'continuous', the blocks are sorted according to + associated eigenvalues. The ordering is first by real part of + eigenvalue, in descending order, then by absolute value of + imaginary part of eigenvalue, also in decreasing order. + + If `sort` is 'discrete', the blocks are sorted as for + 'continuous', but applied to log of eigenvalues + (i.e., continuous-equivalent eigenvalues). + """ + if condmax is None: + condmax = np.finfo(np.float64).eps ** -0.5 + + if not (np.isscalar(condmax) and condmax >= 1.0): + raise ValueError('condmax="{}" must be a scalar >= 1.0'.format(condmax)) + + a = np.atleast_2d(a) + if a.shape[0] == 0 or a.shape[1] == 0: + return a.copy(), np.eye(a.shape[1], a.shape[0]), np.array([]) + + aschur, tschur = schur(a) + amodal, tmodal, blksizes, eigvals = _bdschur_condmax_search(aschur, tschur, condmax) + + if sort in ('continuous', 'discrete'): + + idxs = np.cumsum(np.hstack([0, blksizes[:-1]])) + + ev_per_blk = [complex(eigvals[i].real, abs(eigvals[i].imag)) + for i in idxs] + + if sort == 'discrete': + ev_per_blk = np.log(ev_per_blk) + + # put most unstable first + sortidx = np.argsort(ev_per_blk)[::-1] + + # block indices + blkidxs = [np.arange(i0, i0+ilen) + for i0, ilen in zip(idxs, blksizes)] + + # reordered + permidx = np.hstack([blkidxs[i] for i in sortidx]) + rperm = np.eye(amodal.shape[0])[permidx] + + tmodal = tmodal.dot(rperm) + amodal = rperm.dot(amodal).dot(rperm.T) + blksizes = blksizes[sortidx] + + elif sort is None: + pass + + else: + raise ValueError('unknown sort value "{}"'.format(sort)) + + return amodal, tmodal, blksizes + + +def modal_form(xsys, condmax=None, sort=False): + """Convert a system into modal canonical form + + Parameters + ---------- + xsys : StateSpace object + System to be transformed, with state `x` + condmax : None or float, optional + An upper bound on individual transformations. If None, use `bdschur` default. + sort : bool, optional + If False (default), Schur blocks will not be sorted. See `bdschur` for sort order. + + Returns + ------- + zsys : StateSpace object + System in modal canonical form, with state `z` + T : (M, M) ndarray + Coordinate transformation: z = T * x + """ + + if sort: + discrete = xsys.dt is not None and xsys.dt > 0 + bd_sort = 'discrete' if discrete else 'continuous' + else: + bd_sort = None + + xsys = _convert_to_statespace(xsys) + amodal, tmodal, _ = bdschur(xsys.A, condmax=condmax, sort=bd_sort) + + return similarity_transform(xsys, tmodal, inverse=True), tmodal diff --git a/control/tests/canonical_test.py b/control/tests/canonical_test.py index f88f1af56..0db6b924c 100644 --- a/control/tests/canonical_test.py +++ b/control/tests/canonical_test.py @@ -2,10 +2,13 @@ import numpy as np import pytest +import scipy.linalg + +from control.tests.conftest import slycotonly from control import ss, tf, tf2ss from control.canonical import canonical_form, reachable_form, \ - observable_form, modal_form, similarity_transform + observable_form, modal_form, similarity_transform, bdschur from control.exception import ControlNotImplemented class TestCanonical: @@ -59,75 +62,6 @@ def test_unreachable_system(self): # Check if an exception is raised np.testing.assert_raises(ValueError, canonical_form, sys, "reachable") - @pytest.mark.parametrize( - "A_true, B_true, C_true, D_true", - [(np.diag([4.0, 3.0, 2.0, 1.0]), # order from largest to smallest - np.array([[1.1, 2.2, 3.3, 4.4]]).T, - np.array([[1.3, 1.4, 1.5, 1.6]]), - np.array([[42.0]])), - (np.array([[-1, 1, 0, 0], - [-1, -1, 0, 0], - [ 0, 0, -2, 0], - [ 0, 0, 0, -3]]), - np.array([[0, 1, 0, 1]]).T, - np.array([[1, 0, 0, 1]]), - np.array([[0]])), - # Reorder rows to get complete coverage (real eigenvalue cxrtvfirst) - (np.array([[-1, 0, 0, 0], - [ 0, -2, 1, 0], - [ 0, -1, -2, 0], - [ 0, 0, 0, -3]]), - np.array([[0, 0, 1, 1]]).T, - np.array([[0, 1, 0, 1]]), - np.array([[0]])), - ], - ids=["sys1", "sys2", "sys3"]) - def test_modal_form(self, A_true, B_true, C_true, D_true): - """Test the modal canonical form""" - # Perform a coordinate transform with a random invertible matrix - T_true = np.array([[-0.27144004, -0.39933167, 0.75634684, 0.44135471], - [-0.74855725, -0.39136285, -0.18142339, -0.50356997], - [-0.40688007, 0.81416369, 0.38002113, -0.16483334], - [-0.44769516, 0.15654653, -0.50060858, 0.72419146]]) - A = np.linalg.solve(T_true, A_true).dot(T_true) - B = np.linalg.solve(T_true, B_true) - C = C_true.dot(T_true) - D = D_true - - # Create a state space system and convert it to modal canonical form - sys_check, T_check = canonical_form(ss(A, B, C, D), "modal") - - # Check against the true values - # TODO: Test in respect to ambiguous transformation - # (system characteristics?) - np.testing.assert_array_almost_equal(sys_check.A, A_true) - #np.testing.assert_array_almost_equal(sys_check.B, B_true) - #np.testing.assert_array_almost_equal(sys_check.C, C_true) - np.testing.assert_array_almost_equal(sys_check.D, D_true) - #np.testing.assert_array_almost_equal(T_check, T_true) - - # Create state space system and convert to modal canonical form - sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal') - - # B matrix should be all ones (or zero if not controllable) - # TODO: need to update modal_form() to implement this - if np.allclose(T_check, T_true): - np.testing.assert_array_almost_equal(sys_check.B, B_true) - np.testing.assert_array_almost_equal(sys_check.C, C_true) - - # Make sure Hankel coefficients are OK - for i in range(A.shape[0]): - np.testing.assert_almost_equal( - np.dot(np.dot(C_true, np.linalg.matrix_power(A_true, i)), - B_true), - np.dot(np.dot(C, np.linalg.matrix_power(A, i)), B)) - - def test_modal_form_MIMO(self): - """Test error because modal form only supports SISO""" - sys = tf([[[1], [1]]], [[[1, 2, 1], [1, 2, 1]]]) - with pytest.raises(ControlNotImplemented): - modal_form(sys) - def test_observable_form(self): """Test the observable canonical form""" # Create a system in the observable canonical form @@ -258,3 +192,253 @@ def test_similarity(self): np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D) + +def extract_bdiag(a, blksizes): + """ + Extract block diagonals + + Parameters + ---------- + a - matrix to get blocks from + blksizes - sequence of block diagonal sizes + + Returns + ------- + Block diagonals + + Notes + ----- + Conceptually, inverse of scipy.linalg.block_diag + """ + idx0s = np.hstack([0, np.cumsum(blksizes[:-1], dtype=int)]) + return tuple(a[idx0:idx0+blksize,idx0:idx0+blksize] + for idx0, blksize in zip(idx0s, blksizes)) + + +def companion_from_eig(eigvals): + """ + Find companion matrix for given eigenvalue sequence. + """ + from numpy.polynomial.polynomial import polyfromroots, polycompanion + return polycompanion(polyfromroots(eigvals)).real + + +def block_diag_from_eig(eigvals): + """ + Find block-diagonal matrix for given eigenvalue sequence + + Returns ideal, non-defective, schur block-diagonal form. + """ + blocks = [] + i = 0 + while i < len(eigvals): + e = eigvals[i] + if e.imag == 0: + blocks.append(e.real) + i += 1 + else: + assert e == eigvals[i+1].conjugate() + blocks.append([[e.real, e.imag], + [-e.imag, e.real]]) + i += 2 + return scipy.linalg.block_diag(*blocks) + + +@slycotonly +@pytest.mark.parametrize( + "eigvals, condmax, blksizes", + [ + ([-1,-2,-3,-4,-5], None, [1,1,1,1,1]), + ([-1,-2,-3,-4,-5], 1.01, [5]), + ([-1,-1,-2,-2,-2], None, [2,3]), + ([-1+1j,-1-1j,-2+2j,-2-2j,-2], None, [2,2,1]), + ]) +def test_bdschur_ref(eigvals, condmax, blksizes): + # "reference" check + # uses companion form to introduce numerical complications + from numpy.linalg import solve + + a = companion_from_eig(eigvals) + b, t, test_blksizes = bdschur(a, condmax=condmax) + + np.testing.assert_array_equal(np.sort(test_blksizes), np.sort(blksizes)) + + bdiag_b = scipy.linalg.block_diag(*extract_bdiag(b, test_blksizes)) + np.testing.assert_array_almost_equal(bdiag_b, b) + + np.testing.assert_array_almost_equal(solve(t, a).dot(t), b) + + +@slycotonly +@pytest.mark.parametrize( + "eigvals, sorted_blk_eigvals, sort", + [ + ([-2,-1,0,1,2], [2,1,0,-1,-2], 'continuous'), + ([-2,-2+2j,-2-2j,-2-3j,-2+3j], [-2+3j,-2+2j,-2], 'continuous'), + (np.exp([-0.2,-0.1,0,0.1,0.2]), np.exp([0.2,0.1,0,-0.1,-0.2]), 'discrete'), + (np.exp([-0.2+0.2j,-0.2-0.2j, -0.01, -0.03-0.3j,-0.03+0.3j,]), + np.exp([-0.01, -0.03+0.3j, -0.2+0.2j]), + 'discrete'), + ]) +def test_bdschur_sort(eigvals, sorted_blk_eigvals, sort): + # use block diagonal form to prevent numerical complications + # for discrete case, exp and log introduce round-off, can't test as compeletely + a = block_diag_from_eig(eigvals) + + b, t, blksizes = bdschur(a, sort=sort) + assert len(blksizes) == len(sorted_blk_eigvals) + + blocks = extract_bdiag(b, blksizes) + for block, blk_eigval in zip(blocks, sorted_blk_eigvals): + test_eigvals = np.linalg.eigvals(block) + np.testing.assert_allclose(test_eigvals.real, + blk_eigval.real) + + np.testing.assert_allclose(abs(test_eigvals.imag), + blk_eigval.imag) + + +@slycotonly +def test_bdschur_defective(): + # the eigenvalues of this simple defective matrix cannot be separated + # a previous version of the bdschur would fail on this + a = companion_from_eig([-1, -1]) + amodal, tmodal, blksizes = bdschur(a, condmax=1e200) + + +def test_bdschur_empty(): + # empty matrix in gives empty matrix out + a = np.empty(shape=(0,0)) + b, t, blksizes = bdschur(a) + np.testing.assert_array_equal(b, a) + np.testing.assert_array_equal(t, a) + np.testing.assert_array_equal(blksizes, np.array([])) + + +def test_bdschur_condmax_lt_1(): + # require condmax >= 1.0 + with pytest.raises(ValueError): + bdschur(1, condmax=np.nextafter(1, 0)) + + +@slycotonly +def test_bdschur_invalid_sort(): + # sort must be in ('continuous', 'discrete') + with pytest.raises(ValueError): + bdschur(1, sort='no-such-sort') + + +@slycotonly +@pytest.mark.parametrize( + "A_true, B_true, C_true, D_true", + [(np.diag([4.0, 3.0, 2.0, 1.0]), # order from largest to smallest + np.array([[1.1, 2.2, 3.3, 4.4]]).T, + np.array([[1.3, 1.4, 1.5, 1.6]]), + np.array([[42.0]])), + + (np.array([[-1, 1, 0, 0], + [-1, -1, 0, 0], + [ 0, 0, -2, 1], + [ 0, 0, 0, -3]]), + np.array([[0, 1, 0, 0], + [0, 0, 0, 1]]).T, + np.array([[1, 0, 1, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]), + np.array([[0, 1], + [1, 0], + [0, 0]])), + ], + ids=["sys1", "sys2"]) +def test_modal_form(A_true, B_true, C_true, D_true): + # Check modal_canonical corresponds to bdschur + # Perform a coordinate transform with a random invertible matrix + T_true = np.array([[-0.27144004, -0.39933167, 0.75634684, 0.44135471], + [-0.74855725, -0.39136285, -0.18142339, -0.50356997], + [-0.40688007, 0.81416369, 0.38002113, -0.16483334], + [-0.44769516, 0.15654653, -0.50060858, 0.72419146]]) + A = np.linalg.solve(T_true, A_true).dot(T_true) + B = np.linalg.solve(T_true, B_true) + C = C_true.dot(T_true) + D = D_true + + # Create a state space system and convert it to modal canonical form + sys_check, T_check = modal_form(ss(A, B, C, D)) + + a_bds, t_bds, _ = bdschur(A) + + np.testing.assert_array_almost_equal(sys_check.A, a_bds) + np.testing.assert_array_almost_equal(T_check, t_bds) + np.testing.assert_array_almost_equal(sys_check.B, np.linalg.solve(t_bds, B)) + np.testing.assert_array_almost_equal(sys_check.C, C.dot(t_bds)) + np.testing.assert_array_almost_equal(sys_check.D, D) + + # canonical_form(...,'modal') is the same as modal_form with default parameters + cf_sys, T_cf = canonical_form(ss(A, B, C, D), 'modal') + np.testing.assert_array_almost_equal(cf_sys.A, sys_check.A) + np.testing.assert_array_almost_equal(cf_sys.B, sys_check.B) + np.testing.assert_array_almost_equal(cf_sys.C, sys_check.C) + np.testing.assert_array_almost_equal(cf_sys.D, sys_check.D) + np.testing.assert_array_almost_equal(T_check, T_cf) + + # Make sure Hankel coefficients are OK + for i in range(A.shape[0]): + np.testing.assert_almost_equal( + np.dot(np.dot(C_true, np.linalg.matrix_power(A_true, i)), + B_true), + np.dot(np.dot(C, np.linalg.matrix_power(A, i)), B)) + + +@slycotonly +@pytest.mark.parametrize( + "condmax, len_blksizes", + [(1.1, 1), + (None, 5)]) +def test_modal_form_condmax(condmax, len_blksizes): + # condmax passed through as expected + a = companion_from_eig([-1, -2, -3, -4, -5]) + amodal, tmodal, blksizes = bdschur(a, condmax=condmax) + assert len(blksizes) == len_blksizes + xsys = ss(a, [[1],[0],[0],[0],[0]], [0,0,0,0,1], 0) + zsys, t = modal_form(xsys, condmax=condmax) + np.testing.assert_array_almost_equal(zsys.A, amodal) + np.testing.assert_array_almost_equal(t, tmodal) + np.testing.assert_array_almost_equal(zsys.B, np.linalg.solve(tmodal, xsys.B)) + np.testing.assert_array_almost_equal(zsys.C, xsys.C.dot(tmodal)) + np.testing.assert_array_almost_equal(zsys.D, xsys.D) + + +@slycotonly +@pytest.mark.parametrize( + "sys_type", + ['continuous', + 'discrete']) +def test_modal_form_sort(sys_type): + a = companion_from_eig([0.1+0.9j,0.1-0.9j, 0.2+0.8j, 0.2-0.8j]) + amodal, tmodal, blksizes = bdschur(a, sort=sys_type) + + dt = 0 if sys_type == 'continuous' else True + + xsys = ss(a, [[1],[0],[0],[0],], [0,0,0,1], 0, dt) + zsys, t = modal_form(xsys, sort=True) + + my_amodal = np.linalg.solve(tmodal, a).dot(tmodal) + np.testing.assert_array_almost_equal(amodal, my_amodal) + + np.testing.assert_array_almost_equal(t, tmodal) + np.testing.assert_array_almost_equal(zsys.A, amodal) + np.testing.assert_array_almost_equal(zsys.B, np.linalg.solve(tmodal, xsys.B)) + np.testing.assert_array_almost_equal(zsys.C, xsys.C.dot(tmodal)) + np.testing.assert_array_almost_equal(zsys.D, xsys.D) + + +def test_modal_form_empty(): + # empty system should be returned as-is + # t empty matrix + insys = ss([], [], [], 123) + outsys, t = modal_form(insys) + np.testing.assert_array_equal(outsys.A, insys.A) + np.testing.assert_array_equal(outsys.B, insys.B) + np.testing.assert_array_equal(outsys.C, insys.C) + np.testing.assert_array_equal(outsys.D, insys.D) + assert t.shape == (0,0) diff --git a/doc/control.rst b/doc/control.rst index a3423e379..80119f691 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -155,6 +155,7 @@ Utility functions and conversions :toctree: generated/ augw + bdschur canonical_form damp db2mag @@ -163,6 +164,7 @@ Utility functions and conversions issiso issys mag2db + modal_form observable_form pade reachable_form