diff --git a/control/statesp.py b/control/statesp.py index bff14d241..7b191b50f 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -757,7 +757,6 @@ def _convertToStateSpace(sys, **kw): [1., 1., 1.]]. """ - from .xferfcn import TransferFunction import itertools if isinstance(sys, StateSpace): @@ -771,20 +770,17 @@ def _convertToStateSpace(sys, **kw): try: from slycot import td04ad if len(kw): - raise TypeError("If sys is a TransferFunction, _convertToStateSpace \ - cannot take keywords.") + raise TypeError("If sys is a TransferFunction, " + "_convertToStateSpace cannot take keywords.") # Change the numerator and denominator arrays so that the transfer # function matrix has a common denominator. - num, den = sys._common_den() - # Make a list of the orders of the denominator polynomials. - index = [len(den) - 1 for i in range(sys.outputs)] - # Repeat the common denominator along the rows. - den = array([den for i in range(sys.outputs)]) - #! TODO: transfer function to state space conversion is still buggy! - #print num - #print shape(num) - ssout = td04ad('R',sys.inputs, sys.outputs, index, den, num,tol=0.0) + # matrices are also sized/padded to fit td04ad + num, den, denorder = sys.minreal()._common_den() + + # transfer function to state space conversion now should work! + ssout = td04ad('C', sys.inputs, sys.outputs, + denorder, den, num, tol=0) states = ssout[0] return StateSpace(ssout[1][:states, :states], diff --git a/control/tests/convert_test.py b/control/tests/convert_test.py index e95e03bcf..5d9012399 100644 --- a/control/tests/convert_test.py +++ b/control/tests/convert_test.py @@ -40,7 +40,7 @@ def setUp(self): # Set to True to print systems to the output. self.debug = False # get consistent results - np.random.seed(9) + np.random.seed(7) def printSys(self, sys, ind): """Print system to the standard output.""" @@ -141,10 +141,9 @@ def testConvert(self): ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase) ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase) np.testing.assert_array_almost_equal( \ - ssorig_real, ssxfrm_real) + ssorig_real, ssxfrm_real) np.testing.assert_array_almost_equal( \ - ssorig_imag, ssxfrm_imag) - + ssorig_imag, ssxfrm_imag) # # Make sure xform'd TF has same frequency response # @@ -198,8 +197,9 @@ def testTf2ssStaticMimo(self): """Regression: tf2ss for MIMO static gain""" import control # 2x3 TFM - gmimo = control.tf2ss(control.tf([[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]], - [[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]])) + gmimo = control.tf2ss(control.tf( + [[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]], + [[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]])) self.assertEqual(0, gmimo.states) self.assertEqual(3, gmimo.inputs) self.assertEqual(2, gmimo.outputs) @@ -229,6 +229,21 @@ def testSs2tfStaticMimo(self): numref = np.asarray(d)[...,np.newaxis] np.testing.assert_array_equal(numref, np.array(gtf.num) / np.array(gtf.den)) + def testTf2SsDuplicatePoles(self): + """Tests for "too few poles for MIMO tf #111" """ + import control + try: + import slycot + num = [ [ [1], [0] ], + [ [0], [1] ] ] + + den = [ [ [1,0], [1] ], + [ [1], [1,0] ] ] + g = control.tf(num, den) + s = control.ss(g) + np.testing.assert_array_equal(g.pole(), s.pole()) + except ImportError: + print("Slycot not present, skipping") def suite(): return unittest.TestLoader().loadTestsFromTestCase(TestConvert) diff --git a/control/tests/discrete_test.py b/control/tests/discrete_test.py index 2d4f3f457..fe7b62c17 100644 --- a/control/tests/discrete_test.py +++ b/control/tests/discrete_test.py @@ -6,6 +6,7 @@ import unittest import numpy as np from control import * +from control import matlab class TestDiscrete(unittest.TestCase): """Tests for the DiscreteStateSpace class.""" diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index 1793dee16..efde21c1d 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -396,9 +396,9 @@ def testBalred(self): @unittest.skipIf(not slycot_check(), "slycot not installed") def testModred(self): modred(self.siso_ss1, [1]) - modred(self.siso_ss2 * self.siso_ss3, [0, 1]) - modred(self.siso_ss3, [1], 'matchdc') - modred(self.siso_ss3, [1], 'truncate') + modred(self.siso_ss2 * self.siso_ss1, [0, 1]) + modred(self.siso_ss1, [1], 'matchdc') + modred(self.siso_ss1, [1], 'truncate') @unittest.skipIf(not slycot_check(), "slycot not installed") def testPlace_varga(self): diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py index 7225e5323..204c6dfd8 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -425,10 +425,16 @@ def testPoleMIMO(self): [[[1., 2.], [1., 3.]], [[1., 4., 4.], [1., 9., 14.]]]) p = sys.pole() - np.testing.assert_array_almost_equal(p, [-7., -3., -2., -2.]) - - # Tests for TransferFunction.feedback. + np.testing.assert_array_almost_equal(p, [-2., -2., -7., -3., -2.]) + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testDoubleCancelingPoleSiso(self): + + H = TransferFunction([1,1],[1,2,1]) + p = H.pole() + np.testing.assert_array_almost_equal(p, [-1, -1]) + + # Tests for TransferFunction.feedback def testFeedbackSISO(self): """Test for correct SISO transfer function feedback.""" diff --git a/control/xferfcn.py b/control/xferfcn.py index edaf19191..5280a0dd3 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -55,12 +55,14 @@ import numpy as np from numpy import angle, any, array, empty, finfo, insert, ndarray, ones, \ polyadd, polymul, polyval, roots, sort, sqrt, zeros, squeeze, exp, pi, \ - where, delete, real, poly, poly1d + where, delete, real, poly, poly1d, nonzero import scipy as sp +from numpy.polynomial.polynomial import polyfromroots from scipy.signal import lti, tf2zpk, zpk2tf, cont2discrete from copy import deepcopy import warnings from warnings import warn +from itertools import chain from .lti import LTI, timebaseEqual, timebase, isdtime __all__ = ['TransferFunction', 'tf', 'ss2tf', 'tfdata'] @@ -574,8 +576,11 @@ def freqresp(self, omega): def pole(self): """Compute the poles of a transfer function.""" - num, den = self._common_den() - return roots(den) + num, den, denorder = self._common_den() + rts = [] + for d, o in zip(den,denorder): + rts.extend(roots(d[:o+1])) + return np.array(rts) def zero(self): """Compute the zeros of a transfer function.""" @@ -687,15 +692,17 @@ def returnScipySignalLTI(self): return out + def _common_den(self, imag_tol=None): """ - Compute MIMO common denominator; return it and an adjusted numerator. - - This function computes the single denominator containing all - the poles of sys.den, and reports it as the array d. The - output numerator array n is modified to use the common - denominator; the coefficient arrays are also padded with zeros - to be the same size as d. n is an sys.outputs by sys.inputs + Compute MIMO common denominators; return them and adjusted numerators. + + This function computes the denominators per input containing all + the poles of sys.den, and reports it as the array den. The + output numerator array num is modified to use the common + denominator for this input/column; the coefficient arrays are also + padded with zeros to be the same size for all num/den. + num is an sys.outputs by sys.inputs by len(d) array. Parameters @@ -709,14 +716,23 @@ def _common_den(self, imag_tol=None): num: array Multi-dimensional array of numerator coefficients. num[i][j] gives the numerator coefficient array for the ith input and jth - output + output, also prepared for use in td04ad; matches the denorder + order; highest coefficient starts on the left. den: array - Array of coefficients for common denominator polynomial + Multi-dimensional array of coefficients for common denominator + polynomial, one row per input. The array is prepared for use in + slycot td04ad, the first element is the highest-order polynomial + coefficiend of s, matching the order in denorder, if denorder < + number of columns in den, the den is padded with zeros + + denorder: array of int, orders of den, one per input + + Examples -------- - >>> n, d = sys._common_den() + >>> num, den, denorder = sys._common_den() """ @@ -727,144 +743,90 @@ def _common_den(self, imag_tol=None): if (imag_tol is None): imag_tol = 1e-8 # TODO: figure out the right number to use - # A sorted list to keep track of cumulative poles found as we scan - # self.den. - poles = [] + # A list to keep track of cumulative poles found as we scan + # self.den[..][..] + poles = [ [] for j in range(self.inputs) ] + + # RvP, new implementation 180526, issue #194 - # A 3-D list to keep track of common denominator poles not present in - # the self.den[i][j]. - missingpoles = [[[] for j in range(self.inputs)] - for i in range(self.outputs)] + # pre-calculate the poles for all num, den + # has zeros, poles, gain, list for pole indices not in den, + # number of poles known at the time analyzed + # do not calculate minreal. Rory's hint .minreal() + poleset = [] for i in range(self.outputs): + poleset.append([]) for j in range(self.inputs): - # A sorted array of the poles of this SISO denominator. - currentpoles = sort(roots(self.den[i][j])) - - cp_ind = 0 # Index in currentpoles. - p_ind = 0 # Index in poles. - - # Crawl along the list of current poles and the list of - # cumulative poles, until one of them reaches the end. Keep in - # mind that both lists are always sorted. - while cp_ind < len(currentpoles) and p_ind < len(poles): - if abs(currentpoles[cp_ind] - poles[p_ind]) < (10 * eps): - # If the current element of both - # lists match, then we're - # good. Move to the next pair of elements. - cp_ind += 1 - elif currentpoles[cp_ind] < poles[p_ind]: - # We found a pole in this transfer function that's not - # in the list of cumulative poles. Add it to the list. - poles.insert(p_ind, currentpoles[cp_ind]) - # Now mark this pole as "missing" in all previous - # denominators. - for k in range(i): - for m in range(self.inputs): - # All previous rows. - missingpoles[k][m].append(currentpoles[cp_ind]) - for m in range(j): - # This row only. - missingpoles[i][m].append(currentpoles[cp_ind]) - cp_ind += 1 + if abs(self.num[i][j]).max() <= eps: + poleset[-1].append( [array([], dtype=float), + roots(self.den[i][j]), 0.0, [], 0 ]) + else: + z, p, k = tf2zpk(self.num[i][j], self.den[i][j]) + poleset[-1].append([ z, p, k, [], 0]) + + # collect all individual poles + epsnm = eps * self.inputs * self.outputs + for j in range(self.inputs): + for i in range(self.outputs): + currentpoles = poleset[i][j][1] + nothave = ones(currentpoles.shape, dtype=bool) + for ip, p in enumerate(poles[j]): + idx, = nonzero( + (abs(currentpoles - p) < epsnm) * nothave) + if len(idx): + nothave[idx[0]] = False else: - # There is a pole in the cumulative list of poles that - # is not in our transfer function denominator. Mark - # this pole as "missing", and do not increment cp_ind. - missingpoles[i][j].append(poles[p_ind]) - p_ind += 1 - - if cp_ind == len(currentpoles) and p_ind < len(poles): - # If we finished scanning currentpoles first, then all the - # remaining cumulative poles are missing poles. - missingpoles[i][j].extend(poles[p_ind:]) - elif cp_ind < len(currentpoles) and p_ind == len(poles): - # If we finished scanning the cumulative poles first, then - # all the reamining currentpoles need to be added to poles. - poles.extend(currentpoles[cp_ind:]) - # Now mark these poles as "missing" in previous - # denominators. - for k in range(i): - for m in range(self.inputs): - # All previous rows. - missingpoles[k][m].extend(currentpoles[cp_ind:]) - for m in range(j): - # This row only. - missingpoles[i][m].extend(currentpoles[cp_ind:]) - - # Construct the common denominator. - den = 1. - n = 0 - while n < len(poles): - if abs(poles[n].imag) > 10 * eps: - # To prevent buildup of imaginary part error, handle complex - # pole pairs together. - # - # Because we might have repeated real parts of poles - # and the fact that we are using lexigraphical - # ordering, we can't just combine adjacent poles. - # Instead, we have to figure out the multiplicity - # first, then multiple the pairs from the outside in. - - # Figure out the multiplicity - m = 1 # multiplicity count - while (n+m < len(poles) and - poles[n].real == poles[n+m].real and - poles[n].imag * poles[n+m].imag > 0): - m += 1 - - # Multiple pairs from the outside in - for i in range(m): - quad = polymul([1., -poles[n]], [1., -poles[n+2*(m-i)-1]]) - assert all(quad.imag < 10 * eps), \ - "Quadratic has a nontrivial imaginary part: %g" \ - % quad.imag.max() - - den = polymul(den, quad.real) - n += 1 # move to next pair - n += m # skip past conjugate pairs + # remember id of pole not in tf + poleset[i][j][3].append(ip) + for h, c in zip(nothave, currentpoles): + if h: + poles[j].append(c) + # remember how many poles now known + poleset[i][j][4] = len(poles[j]) + + # figure out maximum number of poles, for sizing the den + npmax = max([len(p) for p in poles]) + den = zeros((self.inputs, npmax+1), dtype=float) + num = zeros((max(1,self.outputs,self.inputs), + max(1,self.outputs,self.inputs), npmax+1), dtype=float) + denorder = zeros((self.inputs,), dtype=int) + + for j in range(self.inputs): + if not len(poles[j]): + # no poles matching this input; only one or more gains + den[j,0] = 1.0 + for i in range(self.outputs): + num[i,j,0] = poleset[i][j][2] else: - den = polymul(den, [1., -poles[n].real]) - n += 1 + # create the denominator matching this input + np = len(poles[j]) + den[j,np::-1] = polyfromroots(poles[j]).real + denorder[j] = np + for i in range(self.outputs): + # start with the current set of zeros for this output + nwzeros = list(poleset[i][j][0]) + # add all poles not found in the original denominator, + # and the ones later added from other denominators + for ip in chain(poleset[i][j][3], + range(poleset[i][j][4],np)): + nwzeros.append(poles[j][ip]) + + numpoly = poleset[i][j][2] * polyfromroots(nwzeros).real + m = npmax - len(numpoly) + #print(j,i,m,len(numpoly),len(poles[j])) + if m < 0: + num[i,j,::-1] = numpoly + else: + num[i,j,:m:-1] = numpoly + if (abs(den.imag) > epsnm).any(): + print("Warning: The denominator has a nontrivial imaginary part: %f" + % abs(den.imag).max()) + den = den.real - # Modify the numerators so that they each take the common denominator. - num = deepcopy(self.num) - if isinstance(den, float): - den = array([den]) - - for i in range(self.outputs): - for j in range(self.inputs): - # The common denominator has leading coefficient 1. Scale out - # the existing denominator's leading coefficient. - assert self.den[i][j][0], "The i = %i, j = %i denominator has \ -a zero leading coefficient." % (i, j) - num[i][j] = num[i][j] / self.den[i][j][0] - - # Multiply in the missing poles. - for p in missingpoles[i][j]: - num[i][j] = polymul(num[i][j], [1., -p]) - - # Pad all numerator polynomials with zeros so that the numerator arrays - # are the same size as the denominator. - for i in range(self.outputs): - for j in range(self.inputs): - pad = len(den) - len(num[i][j]) - if (pad > 0): - num[i][j] = insert( - num[i][j], zeros(pad, dtype=int), - zeros(pad)) - - # Finally, convert the numerator to a 3-D array. - num = array(num) - # Remove trivial imaginary parts. - # Check for nontrivial imaginary parts. - if any(abs(num.imag) > sqrt(eps)): - print ("Warning: The numerator has a nontrivial imaginary part: %g" - % abs(num.imag).max()) - num = num.real - - return num, den + return num, den, denorder + def sample(self, Ts, method='zoh', alpha=None): """Convert a continuous-time system to discrete time @@ -1353,6 +1315,11 @@ def _cleanPart(data): (isinstance(data, ndarray) and data.ndim == 0)): # Data is a scalar (including 0d ndarray) data = [[array([data])]] + elif (isinstance(data, ndarray) and data.ndim == 3 and + isinstance(data[0,0,0], valid_types)): + data = [ [ array(data[i,j]) + for j in range(data.shape[1])] + for i in range(data.shape[0])] elif (isinstance(data, valid_collection) and all([isinstance(d, valid_types) for d in data])): data = [[array(data)]]