From 237597e412118633cb0e6d50becc82c5471d3ef1 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Tue, 29 May 2018 20:51:05 +0200 Subject: [PATCH 01/11] partial changes, not complete, to num/den combination --- control/xferfcn.py | 200 ++++++++++++++++----------------------------- 1 file changed, 71 insertions(+), 129 deletions(-) diff --git a/control/xferfcn.py b/control/xferfcn.py index be21f8509..cd8d2ecc7 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -54,11 +54,13 @@ # External function declarations 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 numpy as np +from numpy.polynomial.polynomial import polyfromroots from scipy.signal import lti, tf2zpk, zpk2tf, cont2discrete from copy import deepcopy from warnings import warn +from itertools import chain from .lti import LTI, timebaseEqual, timebase, isdtime __all__ = ['TransferFunction', 'tf', 'ss2tf', 'tfdata'] @@ -754,144 +756,67 @@ 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 = [ ] - # 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)] + # RvP, new implementation 180526, issue #194 - for i in range(self.outputs): - 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 - 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 - else: - den = polymul(den, [1., -poles[n].real]) - n += 1 - - # Modify the numerators so that they each take the common denominator. - num = deepcopy(self.num) - if isinstance(den, float): - den = array([den]) + # pre-calculate the poles for all den, leave room for index + # of unknown poles and size of current pole list + poleset = [ + [ [ roots(self.den[i][j]), [], None] for j in range(self.inputs) ] + for i in range(self.outputs) ] + # collect all individual poles + epsnm = eps * self.inputs * self.outputs 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. + currentpoles = poleset[i][j][0] + nothave = ones(currentpoles.shape, dtype=bool) + for ip, p in enumerate(poles): + idx, = nonzero( + (abs(currentpoles - p) < epsnm) * nothave) + if len(idx): + nothave[idx[0]] = False + else: + # remember id of pole not in tf + poleset[i][j][1].append(ip) + for h, c in zip(nothave, currentpoles): + if h: + poles.append(c) + # remember how many poles now + poleset[i][j][2] = len(poles) + + # calculate the denominator + den = polyfromroots(poles)[::-1] + if (abs(den.imag) > epsnm).any(): + print("Warning: The denominator has a nontrivial imaginary part: %" + % abs(den.imag).max()) + den = den.real + np = len(poles) + + # now supplement numerators with all new poles + num = zeros((self.outputs, self.inputs, len(poles)+1)) 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 + # collect as set of zeros + nwzeros = list(roots(self.num[i][j])) + # add all poles not found in this denominator, and the + # ones later added from other denominators + for ip in chain(poleset[i][j][1], + range(poleset[i][j][2],np)): + nwzeros.append(poles[ip]) + m = len(nwzeros) + 1 + num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] + + # determine tf gain correction + num[i,j] *= _tfgaincorrect( + self.num[i][j], self.den[i][j], num[i,j], den, 5*eps) return num, den + def sample(self, Ts, method='zoh', alpha=None): """Convert a continuous-time system to discrete time @@ -1064,6 +989,23 @@ def _addSISO(num1, den1, num2, den2): return num, den +def _tfgaincorrect(n1, d1, n2, d2, eps): + """Calculate a gain correction to make n2, d2 gain match n1, d1 + + n2, d2 may have additional cancelling poles, used by _common_den + """ + # get the smallest of numerator/denom size + nn = min(n1.size, n2.size) + nd = min(d1.size, d2.size) + try: + idxn = where((abs(n1[-nn:]) > eps) * (abs(n2[-nn:]) > eps))[0][-1] - nn + idxd = where((abs(d1[-nd:]) > eps) * (abs(d2[-nd:]) > eps))[0][-1] - nd + return n1[idxn]/n2[idxn]*d2[idxd]/d1[idxd] + except IndexError as e: + if abs(n1).max() <= eps: + print("assuming zero gain") + return 0.0 + raise e def _convertToTransferFunction(sys, **kw): """Convert a system to transfer function form (if needed). From 5acb42cde6428d9860bce084ed617fbb220c3cb7 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 30 May 2018 00:38:26 +0200 Subject: [PATCH 02/11] have replaced the _common_den function internals. Passes tests --- control/xferfcn.py | 72 +++++++++++++++++++++++----------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/control/xferfcn.py b/control/xferfcn.py index cd8d2ecc7..02f20a6be 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -762,17 +762,26 @@ def _common_den(self, imag_tol=None): # RvP, new implementation 180526, issue #194 - # pre-calculate the poles for all den, leave room for index - # of unknown poles and size of current pole list - poleset = [ - [ [ roots(self.den[i][j]), [], None] 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 + self2 = self.minreal() + poleset = [] + for i in range(self.outputs): + poleset.append([]) + for j in range(self.inputs): + if abs(self2.num[i][j]).max() <= eps: + poleset[-1].append( [array([], dtype=float), + roots(self2.den[i][j]), 0.0, [], 0 ]) + else: + poleset[-1].append( + [ *tf2zpk(self2.num[i][j], self2.den[i][j]), [], 0]) + # collect all individual poles epsnm = eps * self.inputs * self.outputs for i in range(self.outputs): for j in range(self.inputs): - currentpoles = poleset[i][j][0] + currentpoles = poleset[i][j][1] nothave = ones(currentpoles.shape, dtype=bool) for ip, p in enumerate(poles): idx, = nonzero( @@ -781,38 +790,46 @@ def _common_den(self, imag_tol=None): nothave[idx[0]] = False else: # remember id of pole not in tf - poleset[i][j][1].append(ip) + poleset[i][j][3].append(ip) for h, c in zip(nothave, currentpoles): if h: poles.append(c) - # remember how many poles now - poleset[i][j][2] = len(poles) + # remember how many poles now known + poleset[i][j][4] = len(poles) - # calculate the denominator + # for only gain systems + if len(poles) == 0: + den = ones((1,), dtype=float) + num = zeros((self.outputs, self.inputs, 1), dtype=float) + for i in range(self.outputs): + for j in range(self.inputs): + num[i,j,0] = poleset[i][j][2] + return num, den + + # recreate the denominator den = polyfromroots(poles)[::-1] if (abs(den.imag) > epsnm).any(): - print("Warning: The denominator has a nontrivial imaginary part: %" + print("Warning: The denominator has a nontrivial imaginary part: %f" % abs(den.imag).max()) den = den.real np = len(poles) - + # now supplement numerators with all new poles - num = zeros((self.outputs, self.inputs, len(poles)+1)) + num = zeros((self.outputs, self.inputs, len(poles)+1), dtype=float) for i in range(self.outputs): for j in range(self.inputs): # collect as set of zeros - nwzeros = list(roots(self.num[i][j])) + nwzeros = list(poleset[i][j][0]) # add all poles not found in this denominator, and the # ones later added from other denominators - for ip in chain(poleset[i][j][1], - range(poleset[i][j][2],np)): + for ip in chain(poleset[i][j][3], + range(poleset[i][j][4],np)): nwzeros.append(poles[ip]) m = len(nwzeros) + 1 num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] # determine tf gain correction - num[i,j] *= _tfgaincorrect( - self.num[i][j], self.den[i][j], num[i,j], den, 5*eps) + num[i,j] *= poleset[i][j][2] return num, den @@ -989,23 +1006,6 @@ def _addSISO(num1, den1, num2, den2): return num, den -def _tfgaincorrect(n1, d1, n2, d2, eps): - """Calculate a gain correction to make n2, d2 gain match n1, d1 - - n2, d2 may have additional cancelling poles, used by _common_den - """ - # get the smallest of numerator/denom size - nn = min(n1.size, n2.size) - nd = min(d1.size, d2.size) - try: - idxn = where((abs(n1[-nn:]) > eps) * (abs(n2[-nn:]) > eps))[0][-1] - nn - idxd = where((abs(d1[-nd:]) > eps) * (abs(d2[-nd:]) > eps))[0][-1] - nd - return n1[idxn]/n2[idxn]*d2[idxd]/d1[idxd] - except IndexError as e: - if abs(n1).max() <= eps: - print("assuming zero gain") - return 0.0 - raise e def _convertToTransferFunction(sys, **kw): """Convert a system to transfer function form (if needed). From a0810db1c16663460cf7169694053dfeef40362e Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 30 May 2018 01:37:11 +0200 Subject: [PATCH 03/11] make xferfcn.py python2 compatible again --- control/xferfcn.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/xferfcn.py b/control/xferfcn.py index 02f20a6be..2f17d7271 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -774,8 +774,8 @@ def _common_den(self, imag_tol=None): poleset[-1].append( [array([], dtype=float), roots(self2.den[i][j]), 0.0, [], 0 ]) else: - poleset[-1].append( - [ *tf2zpk(self2.num[i][j], self2.den[i][j]), [], 0]) + z, p, k = tf2zpk(self2.num[i][j], self2.den[i][j]) + poleset[-1].append([ z, p, k, [], 0]) # collect all individual poles epsnm = eps * self.inputs * self.outputs From 492635c70686dfb2c217af92df4e4b123cc1a284 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Mon, 4 Jun 2018 18:40:52 +0200 Subject: [PATCH 04/11] somewhere halfway --- control/tests/convert_test.py | 11 +++ control/xferfcn.py | 135 +++++++++++++++++++++++++++++++++- 2 files changed, 142 insertions(+), 4 deletions(-) diff --git a/control/tests/convert_test.py b/control/tests/convert_test.py index e95e03bcf..2da957bf6 100644 --- a/control/tests/convert_test.py +++ b/control/tests/convert_test.py @@ -229,6 +229,17 @@ 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 + 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()) def suite(): return unittest.TestLoader().loadTestsFromTestCase(TestConvert) diff --git a/control/xferfcn.py b/control/xferfcn.py index 8356e213e..780c41090 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -752,8 +752,8 @@ def _common_den(self, imag_tol=None): # collect all individual poles epsnm = eps * self.inputs * self.outputs - for i in range(self.outputs): - for j in range(self.inputs): + 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): @@ -769,7 +769,7 @@ def _common_den(self, imag_tol=None): poles.append(c) # remember how many poles now known poleset[i][j][4] = len(poles) - + # for only gain systems if len(poles) == 0: den = ones((1,), dtype=float) @@ -797,7 +797,134 @@ def _common_den(self, imag_tol=None): # ones later added from other denominators for ip in chain(poleset[i][j][3], range(poleset[i][j][4],np)): - nwzeros.append(poles[ip]) + nwzeros.append(poles[j][ip]) + for j2 in range(self.inputs): + if j2 != j: + for p in poles[j2]: + nwzeros.append(p) + m = len(nwzeros) + 1 + num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] + + # determine tf gain correction + num[i,j] *= poleset[i][j][2] + + return num, den + + def _common_den2(self, imag_tol=None): + """ + 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 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 + by len(d) array. + + Parameters + ---------- + imag_tol: float + Threshold for the imaginary part of a root to use in detecting + complex poles + + Returns + ------- + num: array + Multi-dimensional array of numerator coefficients. num[i][j] + gives the numerator coefficient array for the ith input and jth + output + + den: array + Array of coefficients for common denominator polynomial + + Examples + -------- + >>> n, d = sys._common_den() + + """ + + # Machine precision for floats. + eps = finfo(float).eps + + # Decide on the tolerance to use in deciding of a pole is complex + if (imag_tol is None): + imag_tol = 1e-8 # TODO: figure out the right number to use + + # 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 + + # 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 + self2 = self.minreal() + poleset = [] + for i in range(self.outputs): + poleset.append([]) + for j in range(self.inputs): + if abs(self2.num[i][j]).max() <= eps: + poleset[-1].append( [array([], dtype=float), + roots(self2.den[i][j]), 0.0, [], 0 ]) + else: + z, p, k = tf2zpk(self2.num[i][j], self2.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): + idx, = nonzero( + (abs(currentpoles - p) < epsnm) * nothave) + if len(idx): + nothave[idx[0]] = False + else: + # 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) + + # 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((self.outputs, self.inputs, npmax+1)) + + for j in range(self.inputs): + if not len(poles[j]): + den[j,npmax] = 1.0 + num[j,npmax] = poleset[i][j][2] + else: + + # recreate the denominator + den[j] = polyfromroots(poles[j])[::-1] + if (abs(den.imag) > epsnm).any(): + print("Warning: The denominator has a nontrivial imaginary part: %f" + % abs(den.imag).max()) + den = den.real + np = len(poles) + + # now supplement numerators with all new poles + num = zeros((self.outputs, self.inputs, len(poles)+1), dtype=float) + for i in range(self.outputs): + for j in range(self.inputs): + # collect as set of zeros + nwzeros = list(poleset[i][j][0]) + # add all poles not found in this 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]) + for j2 in range(self.inputs): + if j2 != j: + for p in poles[j2]: + nwzeros.append(p) m = len(nwzeros) + 1 num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] From 324c5730adbf5ca5556e2fa3c0742cca4b6935e8 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 6 Jun 2018 23:21:33 +0200 Subject: [PATCH 05/11] still not figured out --- control/statesp.py | 15 +++++------ control/xferfcn.py | 67 +++++++++++++++++++++------------------------- 2 files changed, 38 insertions(+), 44 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index bff14d241..6527b151e 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -776,15 +776,14 @@ def _convertToStateSpace(sys, **kw): # 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)]) + num, den, denorder = sys._common_den2() + #! 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) + print("num", num.shape, "=", num) + print("den",den.shape,"=",den) + print("denorder", denorder) + ssout = td04ad('C', sys.inputs, sys.outputs, + denorder, den, num, tol=0.0) states = ssout[0] return StateSpace(ssout[1][:states, :states], diff --git a/control/xferfcn.py b/control/xferfcn.py index 780c41090..3a22ea1ec 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -797,11 +797,7 @@ def _common_den(self, imag_tol=None): # 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]) - for j2 in range(self.inputs): - if j2 != j: - for p in poles[j2]: - nwzeros.append(p) + nwzeros.append(poles[ip]) m = len(nwzeros) + 1 num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] @@ -835,7 +831,12 @@ def _common_den2(self, imag_tol=None): output 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 + + denorder: array of int, orders of den, one per input Examples -------- @@ -877,7 +878,7 @@ def _common_den2(self, imag_tol=None): for i in range(self.outputs): currentpoles = poleset[i][j][1] nothave = ones(currentpoles.shape, dtype=bool) - for ip, p in enumerate(poles): + for ip, p in enumerate(poles[j]): idx, = nonzero( (abs(currentpoles - p) < epsnm) * nothave) if len(idx): @@ -889,49 +890,43 @@ def _common_den2(self, imag_tol=None): if h: poles[j].append(c) # remember how many poles now known - poleset[i][j][4] = len(poles) + 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((self.outputs, self.inputs, npmax+1)) + 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,npmax] = 1.0 - num[j,npmax] = poleset[i][j][2] + num[i,j,npmax] = poleset[i][j][2] else: - - # recreate the denominator - den[j] = polyfromroots(poles[j])[::-1] + # create the denominator matching this input + np = len(poles[j]) + den[j,np+1::-1] = polyfromroots(poles[j]) + 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]) + m = len(nwzeros) + 1 + num[i,j,m::-1] = poleset[i][j][2] * \ + polyfromroots(nwzeros).real + if (abs(den.imag) > epsnm).any(): print("Warning: The denominator has a nontrivial imaginary part: %f" % abs(den.imag).max()) den = den.real - np = len(poles) - - # now supplement numerators with all new poles - num = zeros((self.outputs, self.inputs, len(poles)+1), dtype=float) - for i in range(self.outputs): - for j in range(self.inputs): - # collect as set of zeros - nwzeros = list(poleset[i][j][0]) - # add all poles not found in this 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]) - for j2 in range(self.inputs): - if j2 != j: - for p in poles[j2]: - nwzeros.append(p) - m = len(nwzeros) + 1 - num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] - - # determine tf gain correction - num[i,j] *= poleset[i][j][2] - return num, den + return num, den, denorder def sample(self, Ts, method='zoh', alpha=None): From 0722cf361e228b4f0923f627962cd435a3092805 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Thu, 14 Jun 2018 17:19:45 +0200 Subject: [PATCH 06/11] work in progress --- control/tests/convert_test.py | 2 +- control/xferfcn.py | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/control/tests/convert_test.py b/control/tests/convert_test.py index 2da957bf6..7fa4db26f 100644 --- a/control/tests/convert_test.py +++ b/control/tests/convert_test.py @@ -198,7 +198,7 @@ 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] ]], + 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) diff --git a/control/xferfcn.py b/control/xferfcn.py index 3a22ea1ec..f56f11c3f 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -902,12 +902,12 @@ def _common_den2(self, imag_tol=None): for j in range(self.inputs): if not len(poles[j]): # no poles matching this input; only one or more gains - den[j,npmax] = 1.0 + den[j,0] = 1.0 num[i,j,npmax] = poleset[i][j][2] else: # create the denominator matching this input np = len(poles[j]) - den[j,np+1::-1] = polyfromroots(poles[j]) + den[j,np::-1] = polyfromroots(poles[j]) denorder[j] = np for i in range(self.outputs): # start with the current set of zeros for this output @@ -917,10 +917,13 @@ def _common_den2(self, imag_tol=None): for ip in chain(poleset[i][j][3], range(poleset[i][j][4],np)): nwzeros.append(poles[j][ip]) - m = len(nwzeros) + 1 - num[i,j,m::-1] = poleset[i][j][2] * \ - polyfromroots(nwzeros).real - + + numpoly = poleset[i][j][2] * polyfromroots(nwzeros).real + m = npmax - len(numpoly) - 1 + 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()) From dfb1507d7a8a62a43be95ce36ec0277e21335bc3 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Fri, 15 Jun 2018 18:32:22 +0200 Subject: [PATCH 07/11] more work in progress --- control/statesp.py | 12 ++++-------- control/tests/convert_test.py | 7 ++++--- control/xferfcn.py | 34 ++++++++++++++++++++++++---------- 3 files changed, 32 insertions(+), 21 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index 6527b151e..da44def5f 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,19 +770,16 @@ 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, denorder = sys._common_den2() - + #! TODO: transfer function to state space conversion is still buggy! - print("num", num.shape, "=", num) - print("den",den.shape,"=",den) - print("denorder", denorder) ssout = td04ad('C', sys.inputs, sys.outputs, - denorder, den, num, tol=0.0) + denorder, den, num, tol=1e-14) states = ssout[0] return StateSpace(ssout[1][:states, :states], diff --git a/control/tests/convert_test.py b/control/tests/convert_test.py index 7fa4db26f..387a0eecb 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(5) def printSys(self, sys, ind): """Print system to the standard output.""" @@ -198,8 +198,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) diff --git a/control/xferfcn.py b/control/xferfcn.py index f56f11c3f..20671eda1 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -576,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_den2() + 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.""" @@ -811,10 +814,11 @@ def _common_den2(self, imag_tol=None): 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 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 + 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 @@ -828,7 +832,8 @@ def _common_den2(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 Multi-dimensional array of coefficients for common denominator @@ -838,6 +843,8 @@ def _common_den2(self, imag_tol=None): denorder: array of int, orders of den, one per input + + Examples -------- >>> n, d = sys._common_den() @@ -903,11 +910,12 @@ def _common_den2(self, imag_tol=None): if not len(poles[j]): # no poles matching this input; only one or more gains den[j,0] = 1.0 - num[i,j,npmax] = poleset[i][j][2] + for i in range(self.outputs): + num[i,j,npmax] = poleset[i][j][2] else: # create the denominator matching this input np = len(poles[j]) - den[j,np::-1] = polyfromroots(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 @@ -919,7 +927,8 @@ def _common_den2(self, imag_tol=None): nwzeros.append(poles[j][ip]) numpoly = poleset[i][j][2] * polyfromroots(nwzeros).real - m = npmax - len(numpoly) - 1 + m = npmax - len(numpoly) + #print(j,i,m,len(numpoly),len(poles[j])) if m < 0: num[i,j,::-1] = numpoly else: @@ -1420,6 +1429,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)]] From 50c9bf50988bfb9cd0fdeed93e2febbaa4fb90cb Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 20 Jun 2018 23:13:15 +0200 Subject: [PATCH 08/11] working tf -> ss transformation now, also solved #111 --- control/statesp.py | 8 +-- control/tests/convert_test.py | 7 +- control/tests/xferfcn_test.py | 2 +- control/xferfcn.py | 127 ++-------------------------------- 4 files changed, 14 insertions(+), 130 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index da44def5f..672baac30 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -775,11 +775,11 @@ def _convertToStateSpace(sys, **kw): # Change the numerator and denominator arrays so that the transfer # function matrix has a common denominator. - num, den, denorder = sys._common_den2() - - #! TODO: transfer function to state space conversion is still buggy! + num, den, denorder = sys._common_den() + + # transfer function to state space conversion now should work! ssout = td04ad('C', sys.inputs, sys.outputs, - denorder, den, num, tol=1e-14) + 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 387a0eecb..72a9bd57f 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(5) + 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 # diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py index 7225e5323..d655ce413 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -425,7 +425,7 @@ 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.]) + np.testing.assert_array_almost_equal(p, [-2., -2., -7., -3., -2.]) # Tests for TransferFunction.feedback. diff --git a/control/xferfcn.py b/control/xferfcn.py index 20671eda1..7b7cf3abc 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -576,7 +576,7 @@ def freqresp(self, omega): def pole(self): """Compute the poles of a transfer function.""" - num, den, denorder = self._common_den2() + num, den, denorder = self._common_den() rts = [] for d, o in zip(den,denorder): rts.extend(roots(d[:o+1])) @@ -692,124 +692,8 @@ 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 - by len(d) array. - - Parameters - ---------- - imag_tol: float - Threshold for the imaginary part of a root to use in detecting - complex poles - - Returns - ------- - num: array - Multi-dimensional array of numerator coefficients. num[i][j] - gives the numerator coefficient array for the ith input and jth - output - - den: array - Array of coefficients for common denominator polynomial - - Examples - -------- - >>> n, d = sys._common_den() - """ - - # Machine precision for floats. - eps = finfo(float).eps - - # Decide on the tolerance to use in deciding of a pole is complex - if (imag_tol is None): - imag_tol = 1e-8 # TODO: figure out the right number to use - - # A list to keep track of cumulative poles found as we scan - # self.den[..][..] - poles = [ ] - - # RvP, new implementation 180526, issue #194 - - # 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 - self2 = self.minreal() - poleset = [] - for i in range(self.outputs): - poleset.append([]) - for j in range(self.inputs): - if abs(self2.num[i][j]).max() <= eps: - poleset[-1].append( [array([], dtype=float), - roots(self2.den[i][j]), 0.0, [], 0 ]) - else: - z, p, k = tf2zpk(self2.num[i][j], self2.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): - idx, = nonzero( - (abs(currentpoles - p) < epsnm) * nothave) - if len(idx): - nothave[idx[0]] = False - else: - # remember id of pole not in tf - poleset[i][j][3].append(ip) - for h, c in zip(nothave, currentpoles): - if h: - poles.append(c) - # remember how many poles now known - poleset[i][j][4] = len(poles) - - # for only gain systems - if len(poles) == 0: - den = ones((1,), dtype=float) - num = zeros((self.outputs, self.inputs, 1), dtype=float) - for i in range(self.outputs): - for j in range(self.inputs): - num[i,j,0] = poleset[i][j][2] - return num, den - - # recreate the denominator - den = polyfromroots(poles)[::-1] - if (abs(den.imag) > epsnm).any(): - print("Warning: The denominator has a nontrivial imaginary part: %f" - % abs(den.imag).max()) - den = den.real - np = len(poles) - - # now supplement numerators with all new poles - num = zeros((self.outputs, self.inputs, len(poles)+1), dtype=float) - for i in range(self.outputs): - for j in range(self.inputs): - # collect as set of zeros - nwzeros = list(poleset[i][j][0]) - # add all poles not found in this 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[ip]) - m = len(nwzeros) + 1 - num[i,j,-m:] = polyfromroots(nwzeros).real[::-1] - - # determine tf gain correction - num[i,j] *= poleset[i][j][2] - - return num, den - - def _common_den2(self, imag_tol=None): + def _common_den(self, imag_tol=None): """ Compute MIMO common denominators; return them and adjusted numerators. @@ -839,7 +723,8 @@ def _common_den2(self, imag_tol=None): 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 + 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 @@ -847,7 +732,7 @@ def _common_den2(self, imag_tol=None): Examples -------- - >>> n, d = sys._common_den() + >>> num, den, denorder = sys._common_den() """ @@ -911,7 +796,7 @@ def _common_den2(self, imag_tol=None): # no poles matching this input; only one or more gains den[j,0] = 1.0 for i in range(self.outputs): - num[i,j,npmax] = poleset[i][j][2] + num[i,j,0] = poleset[i][j][2] else: # create the denominator matching this input np = len(poles[j]) From 92a33541da4fe2d4b4f5f5647caa0ba6ebf8daa2 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 20 Jun 2018 23:45:44 +0200 Subject: [PATCH 09/11] disabled MIMO test when no slycot --- control/statesp.py | 1 + control/tests/convert_test.py | 16 ++++++++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index 672baac30..a651a6638 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -775,6 +775,7 @@ def _convertToStateSpace(sys, **kw): # Change the numerator and denominator arrays so that the transfer # function matrix has a common denominator. + # matrices are also sized/padded to fit td04ad num, den, denorder = sys._common_den() # transfer function to state space conversion now should work! diff --git a/control/tests/convert_test.py b/control/tests/convert_test.py index 72a9bd57f..5d9012399 100644 --- a/control/tests/convert_test.py +++ b/control/tests/convert_test.py @@ -232,14 +232,18 @@ def testSs2tfStaticMimo(self): def testTf2SsDuplicatePoles(self): """Tests for "too few poles for MIMO tf #111" """ import control - num = [ [ [1], [0] ], - [ [0], [1] ] ] + try: + import slycot + num = [ [ [1], [0] ], + [ [0], [1] ] ] - den = [ [ [1,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()) + 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) From 7e594c461a9d6fc0ffa24d4cebd186a1e3c44fe3 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Tue, 26 Jun 2018 17:56:02 +0200 Subject: [PATCH 10/11] - do not cancel pole/zero pairs before calculating pole() in xferfcn.py - for the above reason, do conversion on minreal'd xferfcn in statesp.py - add a test for not canceling pole/zero pairs when calculating pole() - add import of matlab in discrete_test.py --- control/statesp.py | 2 +- control/tests/discrete_test.py | 1 + control/tests/xferfcn_test.py | 10 ++++++++-- control/xferfcn.py | 9 +++++---- 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index a651a6638..7b191b50f 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -776,7 +776,7 @@ def _convertToStateSpace(sys, **kw): # Change the numerator and denominator arrays so that the transfer # function matrix has a common denominator. # matrices are also sized/padded to fit td04ad - num, den, denorder = sys._common_den() + num, den, denorder = sys.minreal()._common_den() # transfer function to state space conversion now should work! ssout = td04ad('C', sys.inputs, sys.outputs, 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/xferfcn_test.py b/control/tests/xferfcn_test.py index d655ce413..204c6dfd8 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -427,8 +427,14 @@ def testPoleMIMO(self): np.testing.assert_array_almost_equal(p, [-2., -2., -7., -3., -2.]) - # Tests for TransferFunction.feedback. - + @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 7b7cf3abc..5280a0dd3 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -752,16 +752,17 @@ def _common_den(self, imag_tol=None): # 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 - self2 = self.minreal() + + # do not calculate minreal. Rory's hint .minreal() poleset = [] for i in range(self.outputs): poleset.append([]) for j in range(self.inputs): - if abs(self2.num[i][j]).max() <= eps: + if abs(self.num[i][j]).max() <= eps: poleset[-1].append( [array([], dtype=float), - roots(self2.den[i][j]), 0.0, [], 0 ]) + roots(self.den[i][j]), 0.0, [], 0 ]) else: - z, p, k = tf2zpk(self2.num[i][j], self2.den[i][j]) + z, p, k = tf2zpk(self.num[i][j], self.den[i][j]) poleset[-1].append([ z, p, k, [], 0]) # collect all individual poles From 732c924d67bede6f3f0d065959dc10a5c46259dd Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Sun, 1 Jul 2018 23:53:14 +0200 Subject: [PATCH 11/11] - change testModred; that one did state removal on a system of which the selection of states was automatic --- control/tests/matlab_test.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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):