From 2e988813ec9381f8048c9665bea99bc6e71da1fa Mon Sep 17 00:00:00 2001 From: bnavigator Date: Tue, 10 Nov 2020 00:43:24 +0100 Subject: [PATCH 1/4] refactor stability_margins and add routines for discrete time --- control/margins.py | 342 ++++++++++++++++++++++++----------- control/tests/margin_test.py | 9 +- 2 files changed, 240 insertions(+), 111 deletions(-) diff --git a/control/margins.py b/control/margins.py index 7bdcf6caa..299b1c493 100644 --- a/control/margins.py +++ b/control/margins.py @@ -54,26 +54,157 @@ import numpy as np import scipy as sp from . import xferfcn -from .lti import issiso +from .lti import issiso, evalfr from . import frdata +from .exception import ControlMIMONotImplemented __all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin'] -# helper functions for stability_margins -def _polyimsplit(pol): - """split a polynomial with (iw) applied into a real and an - imaginary part with w applied""" - rpencil = np.zeros_like(pol) - ipencil = np.zeros_like(pol) - rpencil[-1::-4] = 1. - rpencil[-3::-4] = -1. - ipencil[-2::-4] = 1. - ipencil[-4::-4] = -1. - return pol * rpencil, pol*ipencil - -def _polysqr(pol): - """return a polynomial squared""" - return np.polymul(pol, pol) + +# private helper functions +def _poly_iw(sys): + """Apply s = iw to G(s)=num(s)/den(s) + + Splits the num and den polynomials with (iw) applied into real and + imaginary parts with w applied + """ + num = sys.num[0][0] + den = sys.den[0][0] + num_iw = (1J)**np.arange(len(num) - 1, -1, -1) * num + den_iw = (1J)**np.arange(len(den) - 1, -1, -1) * den + return num_iw, den_iw + + +def _poly_iw_sqr(pol_iw): + return np.real(np.polymul(pol_iw, pol_iw.conj())) + + +def _poly_iw_real_crossing(num_iw, den_iw, epsw): + # Return w where imag(H(iw)) == 0 + test_w = np.polysub(np.polymul(num_iw.imag, den_iw.real), + np.polymul(num_iw.real, den_iw.imag)) + w = np.roots(test_w) + w = np.real(w[np.isreal(w)]) + w = w[w >= epsw] + return w + + +def _poly_iw_mag1_crossing(num_iw, den_iw, epsw): + # Return w where |H(iw)| == 1, |num(iw)| - |den(iw)| == 0 + w = np.roots(np.polysub(_poly_iw_sqr(num_iw), _poly_iw_sqr(den_iw))) + w = np.real(w[np.isreal(w)]) + w = w[w > epsw] + return w + + +def _poly_iw_wstab(num_iw, den_iw, epsw): + # Stability margin: minimum distance to point -1 + # find zero derivative. Second derivative needs to be >0 + # to have a minimum + test_wstabn = _poly_iw_sqr(np.polyadd(num_iw, den_iw)) + test_wstabd = _poly_iw_sqr(den_iw) + test_wstab = np.polysub( + np.polymul(np.polyder(test_wstabn), test_wstabd), + np.polymul(np.polyder(test_wstabd), test_wstabn)) + + # find the solutions, for positive omega, and only real ones + wstab = np.roots(test_wstab) + wstab = np.real(wstab[np.isreal(wstab)]) + wstab = wstab[wstab > epsw] + + # and find the value of the 2nd derivative there, needs to be positive + wstabplus = np.polyval(np.polyder(test_wstab), wstab) + wstab = wstab[wstabplus > 0.] + return wstab + + +def _poly_z_invz(sys): + num = sys.num[0][0] # num(z) = a_p * z^p + a_(p-1) * z^(p-1) + ... + a_0 + den = sys.den[0][0] # num(z) = b_q * z^p + b_(q-1) * z^(q-1) + ... + b_0 + p_q = len(num) - len(den) + if p_q > 0: + raise ValueError("Not a proper transfer function: Denominator must " + "have equal or higher order than numerator.") + num_inv_zp = num[::-1] # num(1/z) * z^p + den_inv_zq = den[::-1] # den(1/z) * z^q + return num, den, num_inv_zp, den_inv_zq, p_q, sys.dt + + +def _z_filter(z, dt, eps): + # z = exp(1J w dt) + # |z| == 1 with some float precision tolerance + z = z[np.abs(np.abs(z) - 1.) < eps] + zarg = np.angle(z) + zidx = (0 <= zarg) * (zarg < np.pi) + omega = zarg[zidx] / dt + return z[zidx], omega + + +def _poly_z_real_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw): + # H(z)==H(1/z), num(z)*den(1/z) == num(1/z)*den(z) + p1 = np.polymul(num, den_inv_zq) + p2 = np.polymul(num_inv_zp, den) + if p_q < 0: + # Future: numpy >= 1.5.0 defines np.polymulx() + x = [1] + [0] * (-p_q) + p2 = np.polymul(p2, x) + z = np.roots(np.polysub(p1, p2)) + eps = np.finfo(float).eps**(1 / len(p2)) + z, w = _z_filter(z, dt, eps) + z = z[w >= epsw] + w = w[w >= epsw] + return z, w + + +def _poly_z_mag1_crossing(num, den, num_inv, den_inv, p_q, dt, epsw): + # |H(z)| = 1, H(z)*H(1/z)=1, num(z)*num(1/z) == den(z)*den(1/z) + p1 = np.polymul(num, num_inv) + p2 = np.polymul(den, den_inv) + if p_q < 0: + x = [1] + [0] * (-p_q) + p2 = np.polymul(p2, x) + z = np.roots(np.polysub(p1, p2)) + eps = np.finfo(float).eps**(1 / len(p2)) + z, w = _z_filter(z, dt, eps) + z = z[w > epsw] + w = w[w > epsw] + return z, w + + +def _poly_z_wstab(num, den, num_inv, den_inv, p_q, dt, epsw): + # Stability margin: Minimum distance to -1 + + # TODO: Find a way to solve for z or omega analytically with given + # polynomials + # d|1 + H(z)|/dz = 0, or d|1 + H(exp(iwdt))|/dw = 0 + + # optimization function to minimize + def fun(wdt): + with np.errstate(all='ignore'): # den=0 is okay + return np.abs(1 + (np.polyval(num, np.exp(1J * wdt)) / + np.polyval(den, np.exp(1J * wdt)))) + + # find initial guess + wdt_v = np.geomspace(1e-4, 2 * np.pi, num=100) + wdt0 = wdt_v[np.argmin(fun(wdt_v))] + + # Use `minimize` instead of univariate `minimize_scalars` because we want + # to provide some initial value in order to not converge on frequencies + # with extremely low gradients. + res = sp.optimize.minimize( + fun=fun, + x0=[wdt0], + bounds=[(0, 2 * np.pi)]) + if res.success: + wdt = res.x + z = np.exp(1J * wdt) + w = wdt / dt + else: + z = np.array([]) + w = np.array([]) + + return z, w + # Took the framework for the old function by # Sawyer B. Fuller , removed a lot of the innards @@ -98,6 +229,9 @@ def _polysqr(pol): # issue 1, pp 51-59, closer to Matlab behavior, but # not completely identical in edge cases, which don't # cross but touch gain=1 +# BG, Nov 9, 2020, removed duplicate implementations of the same code +# for crossover frequencies and enhanced to handle discrete +# systems def stability_margins(sysdata, returnall=False, epsw=0.0): """Calculate stability margins and associated crossover frequencies. @@ -133,7 +267,6 @@ def stability_margins(sysdata, returnall=False, epsw=0.0): ws: float or array_like Frequency for stability margin (complex gain closest to -1) """ - try: if isinstance(sysdata, frdata.FRD): sys = frdata.FRD(sysdata, smooth=True) @@ -141,73 +274,66 @@ def stability_margins(sysdata, returnall=False, epsw=0.0): sys = sysdata elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3: mag, phase, omega = sysdata - sys = frdata.FRD(mag * np.exp(1j * phase * math.pi/180), + sys = frdata.FRD(mag * np.exp(1j * phase * math.pi / 180.), omega, smooth=True) else: sys = xferfcn._convert_to_transfer_function(sysdata) except Exception as e: - print (e) + print(e) raise ValueError("Margin sysdata must be either a linear system or " "a 3-sequence of mag, phase, omega.") - # calculate gain of system - if isinstance(sys, xferfcn.TransferFunction): - - # check for siso - if not issiso(sys): - raise ValueError("Can only do margins for SISO system") + # check for siso + if not issiso(sys): + raise ControlMIMONotImplemented( + "Can only do margins for SISO system") - # real and imaginary part polynomials in omega: - rnum, inum = _polyimsplit(sys.num[0][0]) - rden, iden = _polyimsplit(sys.den[0][0]) + if isinstance(sys, xferfcn.TransferFunction): + if sys.isctime(): + num_iw, den_iw = _poly_iw(sys) + # frequency for gain margin: phase crosses -180 degrees + w_180 = _poly_iw_real_crossing(num_iw, den_iw, epsw) + with np.errstate(all='ignore'): # den=0 is okay + w180_resp = evalfr(sys, 1J * w_180) + + # frequency for phase margin : gain crosses magnitude 1 + wc = _poly_iw_mag1_crossing(num_iw, den_iw, epsw) + wc_resp = evalfr(sys, 1J * wc) + + # stability margin + wstab = _poly_iw_wstab(num_iw, den_iw, epsw) + ws_resp = evalfr(sys, 1J * wstab) + + else: # Discrete Time + zargs = _poly_z_invz(sys) + # gain margin + z, w_180 = _poly_z_real_crossing(*zargs, epsw=epsw) + w180_resp = evalfr(sys, z) + + # phase margin + z, wc = _poly_z_mag1_crossing(*zargs, epsw=epsw) + wc_resp = evalfr(sys, z) + + # stability margin + z, wstab = _poly_z_wstab(*zargs, epsw=epsw) + ws_resp = evalfr(sys, z) - # test (imaginary part of tf) == 0, for phase crossover/gain margins - test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden)) - w_180 = np.roots(test_w_180) + # only keep frequencies where the negative real axis is crossed + w_180 = w_180[w180_resp <= 0.] + w180_resp = w180_resp[w180_resp <= 0.] - # first remove imaginary and negative frequencies, epsw removes the - # "0" frequency for type-2 systems - w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)]) + # sort + idx = np.argsort(w_180) + w_180 = w_180[idx] + w180_resp = w180_resp[idx] - # evaluate response at remaining frequencies, to test for phase 180 vs 0 - with np.errstate(all='ignore'): - resp_w_180 = np.real( - np.polyval(sys.num[0][0], 1.j*w_180) / - np.polyval(sys.den[0][0], 1.j*w_180)) + idx = np.argsort(wc) + wc = wc[idx] + wc_resp = wc_resp[idx] - # only keep frequencies where the negative real axis is crossed - w_180 = w_180[np.real(resp_w_180) <= 0.0] - - # and sort - w_180.sort() - - # test magnitude is 1 for gain crossover/phase margins - test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)), - np.polyadd(_polysqr(rden), _polysqr(iden))) - wc = np.roots(test_wc) - wc = np.real(wc[(np.imag(wc) == 0) * (wc > epsw)]) - wc.sort() - - # stability margin was a bitch to elaborate, relies on magnitude to - # point -1, then take the derivative. Second derivative needs to be >0 - # to have a minimum - test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden)) - test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)), - _polysqr(np.polyadd(inum,iden))) - test_wstab = np.polysub( - np.polymul(np.polyder(test_wstabn),test_wstabd), - np.polymul(np.polyder(test_wstabd),test_wstabn)) - - # find the solutions, for positive omega, and only real ones - wstab = np.roots(test_wstab) - wstab = np.real(wstab[(np.imag(wstab) == 0) * - (np.real(wstab) >= 0)]) - - # and find the value of the 2nd derivative there, needs to be positive - wstabplus = np.polyval(np.polyder(test_wstab), wstab) - wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) * - (wstabplus > 0.)]) - wstab.sort() + idx = np.argsort(wstab) + wstab = wstab[idx] + ws_resp = ws_resp[idx] else: # a bit coarse, have the interpolated frd evaluated again @@ -223,19 +349,22 @@ def _dstab(w): """Calculate the distance from -1 point""" return np.abs(sys._evalfr(w)[0][0] + 1.) - # Find all crossings, note that this depends on omega having - # a correct range - widx = np.where(np.diff(np.sign(_mod(sys.omega))))[0] - wc = np.array( - [sp.optimize.brentq(_mod, sys.omega[i], sys.omega[i+1]) - for i in widx]) - # find the phase crossings ang(H(jw) == -180 widx = np.where(np.diff(np.sign(_arg(sys.omega))))[0] widx = widx[np.real(sys._evalfr(sys.omega[widx])[0][0]) <= 0] w_180 = np.array( [sp.optimize.brentq(_arg, sys.omega[i], sys.omega[i+1]) for i in widx]) + # TODO: replace by evalfr(sys, 1J*w) or sys(1J*w), (needs gh-449) + w180_resp = sys._evalfr(w_180)[0][0] + + # Find all crossings, note that this depends on omega having + # a correct range + widx = np.where(np.diff(np.sign(_mod(sys.omega))))[0] + wc = np.array( + [sp.optimize.brentq(_mod, sys.omega[i], sys.omega[i+1]) + for i in widx]) + wc_resp = sys._evalfr(wc)[0][0] # find all stab margins? widx, = np.where(np.diff(np.sign(np.diff(_dstab(sys.omega)))) > 0) @@ -245,14 +374,12 @@ def _dstab(w): ).x for i in widx]) wstab = wstab[(wstab >= sys.omega[0]) * (wstab <= sys.omega[-1])] + ws_resp = sys._evalfr(wstab)[0][0] - # margins, as iterables, converted frdata and xferfcn calculations to - # vector for this - with np.errstate(all='ignore'): - gain_w_180 = np.abs(sys._evalfr(w_180)[0][0]) - GM = 1.0/gain_w_180 - SM = np.abs(sys._evalfr(wstab)[0][0]+1) - PM = np.remainder(np.angle(sys._evalfr(wc)[0][0], deg=True), 360.0) - 180.0 + with np.errstate(all='ignore'): # |G|=0 is okay and yields inf + GM = 1. / np.abs(w180_resp) + PM = np.remainder(np.angle(wc_resp, deg=True), 360.) - 180. + SM = np.abs(ws_resp + 1.) if returnall: return GM, PM, SM, w_180, wc, wstab @@ -279,43 +406,44 @@ def phase_crossover_frequencies(sys): """Compute frequencies and gains at intersections with real axis in Nyquist plot. - Call as: - omega, gain = phase_crossover_frequencies() + Parameters + ---------- + sys : SISO LTI system Returns ------- - omega: 1d array of (non-negative) frequencies where Nyquist plot - intersects the real axis - - gain: 1d array of corresponding gains + omega : ndarray + 1d array of (non-negative) frequencies where Nyquist plot + intersects the real axis + gain : ndarray + 1d array of corresponding gains Examples -------- >>> tf = TransferFunction([1], [1, 2, 3, 4]) - >>> PhaseCrossoverFrequenies(tf) + >>> phase_crossover_frequencies(tf) (array([ 1.73205081, 0. ]), array([-0.5 , 0.25])) """ - # Convert to a transfer function tf = xferfcn._convert_to_transfer_function(sys) - # if not siso, fall back to (0,0) element - #! TODO: should add a check and warning here - num = tf.num[0][0] - den = tf.den[0][0] + if not issiso(tf): + raise ControlMIMONotImplemented( + "Can only calculate crossovers for SISO system") # Compute frequencies that we cross over the real axis - numj = (1.j)**np.arange(len(num)-1,-1,-1)*num - denj = (-1.j)**np.arange(len(den)-1,-1,-1)*den - allfreq = np.roots(np.imag(np.polymul(numj,denj))) - realfreq = np.real(allfreq[np.isreal(allfreq)]) - realposfreq = realfreq[realfreq >= 0.] + if sys.isctime(): + num_iw, den_iw = _poly_iw(tf) + omega = _poly_iw_real_crossing(num_iw, den_iw, 0.) - # using real() to avoid rounding errors and results like 1+0j - # it would be nice to have a vectorized version of self.evalfr here - gain = np.real(np.asarray([tf._evalfr(f)[0][0] for f in realposfreq])) + # using real() to avoid rounding errors and results like 1+0j + gain = np.real(evalfr(sys, 1J * omega)) + else: + zargs = _poly_z_invz(sys) + z, omega = _poly_z_real_crossing(*zargs, epsw=0.) + gain = np.real(evalfr(sys, z)) - return realposfreq, gain + return omega, gain def margin(*args): diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 2f60c7bc6..7ce8c3302 100755 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -17,6 +17,7 @@ stability_margins from control.statesp import StateSpace from control.xferfcn import TransferFunction +from control.exception import ControlMIMONotImplemented s = TransferFunction([1, 0], [1]) @@ -108,14 +109,14 @@ def test_phase_crossover_frequencies(): assert_allclose(omega, [0.], atol=1.5e-3) assert_allclose(gain, [1.], atol=1.5e-3) - # testing MIMO, only (0,0) element is considered + # MIMO tf = TransferFunction([[[1], [2]], [[3], [4]]], [[[1, 2, 3, 4], [1, 1]], [[1, 1], [1, 1]]]) - omega, gain = phase_crossover_frequencies(tf) - assert_allclose(omega, [1.73205, 0.], atol=1.5e-3) - assert_allclose(gain, [-0.5, 0.25], atol=1.5e-3) + with pytest.raises(ControlMIMONotImplemented): + omega, gain = phase_crossover_frequencies(tf) + def test_mag_phase_omega(): From 199858b1610a857ce6fbd47c05bf20126d664b32 Mon Sep 17 00:00:00 2001 From: bnavigator Date: Mon, 16 Nov 2020 17:28:23 +0100 Subject: [PATCH 2/4] pick margin_test.py from #438 --- control/tests/margin_test.py | 286 ++++++++++++++++++----------------- 1 file changed, 151 insertions(+), 135 deletions(-) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 7ce8c3302..7b513a9f5 100755 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -3,51 +3,65 @@ margin_test.py - test suite for stability margin commands RMM, 15 Jul 2011 -BG, 30 Juin 2020 -- convert to pytest, gh-425 +BG, 30 June 2020 -- convert to pytest, gh-425 """ from __future__ import print_function import numpy as np +import pytest from numpy import inf, nan from numpy.testing import assert_allclose -import pytest from control.frdata import FrequencyResponseData -from control.margins import margin, phase_crossover_frequencies, \ - stability_margins +from control.margins import (margin, phase_crossover_frequencies, + stability_margins) from control.statesp import StateSpace from control.xferfcn import TransferFunction from control.exception import ControlMIMONotImplemented - -s = TransferFunction([1, 0], [1]) - -# (system, stability_margins(sys), stability_margins(sys, returnall=True)) -tsys = [(TransferFunction([1, 2], [1, 2, 3]), - (inf, inf, inf, nan, nan, nan), - ([], [], [], [], [], [])), - (TransferFunction([1], [1, 2, 3, 4]), - (2., inf, 0.4170, 1.7321, nan, 1.6620), - ([2.], [], [1.2500, 0.4170], [1.7321], [], [0.1690, 1.6620])), - (StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]], - [[1., 0.]], [[0.]]), - (inf, 147.0743, inf, nan, 2.5483, nan), - ([], [147.0743], [], [], [2.5483], [])), - ((8.75*(4*s**2+0.4*s+1)) / ((100*s+1)*(s**2+0.22*s+1)) - / (s**2/(10.**2)+2*0.04*s/10.+1), - (2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918), - ([2.2716], [97.5941, -157.7844, 134.7359], [1.0381, 0.5591], - [10.0053], [0.0850, 0.9373, 1.0919], [0.4064, 9.9918])), - (1/(1+s), # no gain/phase crossovers - (inf, inf, inf, nan, nan, nan), - ([], [], [], [], [], [])), - (3*(10+s)/(2+s), # no gain/phase crossovers - (inf, inf, inf, nan, nan, nan), - ([], [], [], [], [], [])), - (0.01*(10-s)/(2+s)/(1+s), # no phase crossovers - (300.0, inf, 0.9917, 5.6569, nan, 2.3171), - ([300.0], [], [0.9917], [5.6569], [], 2.3171))] - +s = TransferFunction.s + +@pytest.fixture(params=[ + # sysfn, args, + # stability_margins(sys), + # stability_margins(sys, returnall=True) + (TransferFunction, ([1, 2], [1, 2, 3]), + (inf, inf, inf, nan, nan, nan), + ([], [], [], [], [], [])), + (TransferFunction, ([1], [1, 2, 3, 4]), + (2., inf, 0.4170, 1.7321, nan, 1.6620), + ([2.], [], [1.2500, 0.4170], [1.7321], [], [0.1690, 1.6620])), + (StateSpace, ([[1., 4.], + [3., 2.]], + [[1.], [-4.]], + [[1., 0.]], + [[0.]]), + (inf, 147.0743, inf, nan, 2.5483, nan), + ([], [147.0743], [], [], [2.5483], [])), + (None, ((8.75 * (4 * s**2 + 0.4 * s + 1)) + / ((100 * s + 1) * (s**2 + 0.22 * s + 1)) + / (s**2 / 10.**2 + 2 * 0.04 * s / 10. + 1)), + (2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918), + ([2.2716], [97.5941, -157.7844, 134.7359], [1.0381, 0.5591], + [10.0053], [0.0850, 0.9373, 1.0919], [0.4064, 9.9918])), + (None, (1 / (1 + s)), # no gain/phase crossovers + (inf, inf, inf, nan, nan, nan), + ([], [], [], [], [], [])), + (None, (3 * (10 + s) / (2 + s)), # no gain/phase crossovers + (inf, inf, inf, nan, nan, nan), + ([], [], [], [], [], [])), + (None, 0.01 * (10 - s) / (2 + s) / (1 + s), # no phase crossovers + (300.0, inf, 0.9917, 5.6569, nan, 2.3171), + ([300.0], [], [0.9917], [5.6569], [], 2.3171)), +]) +def tsys(request): + """Return test systems and reference data""" + sysfn, args = request.param[:2] + if sysfn: + sys = sysfn(*args) + else: + sys = args + return (sys,) + request.param[2:] def compare_allmargins(actual, desired, **kwargs): """Compare all elements of stability_margins(returnall=True) result""" @@ -56,8 +70,8 @@ def compare_allmargins(actual, desired, **kwargs): assert_allclose(a, d, **kwargs) -@pytest.mark.parametrize("sys, refout, refoutall", tsys) -def test_stability_margins(sys, refout, refoutall): +def test_stability_margins(tsys): + sys, refout, refoutall = tsys """Test stability_margins() function""" out = stability_margins(sys) assert_allclose(out, refout, atol=1.5e-2) @@ -65,16 +79,17 @@ def test_stability_margins(sys, refout, refoutall): compare_allmargins(out, refoutall, atol=1.5e-2) -@pytest.mark.parametrize("sys, refout, refoutall", tsys) -def test_stability_margins_omega(sys, refout, refoutall): + +def test_stability_margins_omega(tsys): + sys, refout, refoutall = tsys """Test stability_margins() with interpolated frequencies""" omega = np.logspace(-2, 2, 2000) out = stability_margins(FrequencyResponseData(sys, omega)) assert_allclose(out, refout, atol=1.5e-3) -@pytest.mark.parametrize("sys, refout, refoutall", tsys) -def test_stability_margins_3input(sys, refout, refoutall): +def test_stability_margins_3input(tsys): + sys, refout, refoutall = tsys """Test stability_margins() function with mag, phase, omega input""" omega = np.logspace(-2, 2, 2000) mag, phase, omega_ = sys.freqresp(omega) @@ -82,15 +97,15 @@ def test_stability_margins_3input(sys, refout, refoutall): assert_allclose(out, refout, atol=1.5e-3) -@pytest.mark.parametrize("sys, refout, refoutall", tsys) -def test_margin_sys(sys, refout, refoutall): +def test_margin_sys(tsys): + sys, refout, refoutall = tsys """Test margin() function with system input""" out = margin(sys) assert_allclose(out, np.array(refout)[[0, 1, 3, 4]], atol=1.5e-3) -@pytest.mark.parametrize("sys, refout, refoutall", tsys) -def test_margin_3input(sys, refout, refoutall): +def test_margin_3input(tsys): + sys, refout, refoutall = tsys """Test margin() function with mag, phase, omega input""" omega = np.logspace(-2, 2, 2000) mag, phase, omega_ = sys.freqresp(omega) @@ -100,7 +115,8 @@ def test_margin_3input(sys, refout, refoutall): def test_phase_crossover_frequencies(): """Test phase_crossover_frequencies() function""" - omega, gain = phase_crossover_frequencies(tsys[1][0]) + sys = TransferFunction([1], [1, 2, 3, 4]) + omega, gain = phase_crossover_frequencies(sys) assert_allclose(omega, [1.73205, 0.], atol=1.5e-3) assert_allclose(gain, [-0.5, 0.25], atol=1.5e-3) @@ -215,99 +231,99 @@ def test_frd_indexing(): assert_allclose(ws, [1., 2.], atol=0.01) -""" -NOTE: -Matlab gives gain margin 0 for system `type2`, python-control gives inf -Difficult to argue which is right? Special case or different approach? +@pytest.fixture +def tsys_zmoresystems(): + """A cornucopia of tricky systems for phase / gain margin -Edge cases, like `type0` which approaches a gain of 1 for w -> 0, are also not -identically indicated, Matlab gives phase margin -180, at w = 0. For higher or -lower gains, results match. -""" -tzmore_sys = { - 'typem1': s/(s+1), - 'type0': 1/(s+1)**3, - 'type1': (s + 0.1)/s/(s+1), - 'type2': (s + 0.1)/s**2/(s+1), - 'type3': (s + 0.1)*(s+0.1)/s**3/(s+1)} -tzmore_margin = [ - dict(sys='typem1', K=2.0, atol=1.5e-3, result=( - float('Inf'), -120.0007, float('NaN'), 0.5774)), - dict(sys='type0', K=0.8, atol=1.5e-3, result=( - 10.0014, float('inf'), 1.7322, float('nan'))), - dict(sys='type0', K=2.0, atol=1e-2, result=( - 4.000, 67.6058, 1.7322, 0.7663)), - dict(sys='type1', K=1.0, atol=1e-4, result=( - float('Inf'), 144.9032, float('NaN'), 0.3162)), - dict(sys='type2', K=1.0, atol=1e-4, result=( - float('Inf'), 44.4594, float('NaN'), 0.7907)), - dict(sys='type3', K=1.0, atol=1.5e-3, result=( - 0.0626, 37.1748, 0.1119, 0.7951)), - ] -tzmore_stability_margins = [] + `example*` from "A note on the Gain and Phase Margin Concepts + Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi, + Dec 2015, vol 3 iss 1, pp 51-59 -""" -from "A note on the Gain and Phase Margin Concepts -Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi, -Dec 2015, vol 3 iss 1, pp 51-59 + TODO: still have to convert more to tests + fix margin to handle + also these torture cases + """ -A cornucopia of tricky systems for phase / gain margin -TODO: still have to convert more to tests + fix margin to handle -also these torture cases -""" -yazdan = { - 'example21': - 0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10)/( - (s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)**2), - 'example23': - ((s+0.1)**2 + 1)*(s-0.1)/( - ((s+0.1)**2+4)*(s+1)), - 'example25a': - s/(s**2+2*s+2)**4, - 'example26a': - ((s-0.1)**2 + 1)/( - (s + 0.1)*((s-0.2)**2 + 4)), - 'example26b': ((s-0.1)**2 + 1)/( - (s - 0.3)*((s-0.2)**2 + 4)) -} -yazdan['example24'] = yazdan['example21']*20000 -yazdan['example25b'] = yazdan['example25a']*100 -yazdan['example22'] = yazdan['example21']*(s**2 - 2*s + 401) -ymargin = [ - dict(sys='example21', K=1.0, atol=1e-2, - result=(0.0100, -14.5640, 0, 0.0022)), - dict(sys='example21', K=1000.0, atol=1e-2, - result=(0.1793, 22.5215, 0.0243, 0.0630)), - dict(sys='example21', K=5000.0, atol=1.5e-3, - result=(4.5596, 21.2101, 0.4385, 0.1868)), - ] -ystability_margins = [ - dict(sys='example21', K=1.0, rtol=1e-3, atol=1e-3, - result=([0.01, 179.2931, 2.2798e+4, 1.5946e+07, 7.2477e+08], - [-14.5640], - [0.2496], - [0, 0.0243, 0.4385, 6.8640, 84.9323], - [0.0022], - [0.0022])), - ] - -tzmore_sys.update(yazdan) -tzmore_margin += ymargin -tzmore_stability_margins += ystability_margins - - -@pytest.mark.parametrize('tmargin', tzmore_margin) -def test_zmore_margin(tmargin): - """Test margins for more tricky systems""" - res = margin(tzmore_sys[tmargin['sys']]*tmargin['K']) - assert_allclose(res, tmargin['result'], atol=tmargin['atol']) - - -@pytest.mark.parametrize('tmarginall', tzmore_stability_margins) -def test_zmore_stability_margins(tmarginall): + systems = { + 'typem1': s/(s+1), + 'type0': 1/(s+1)**3, + 'type1': (s + 0.1)/s/(s+1), + 'type2': (s + 0.1)/s**2/(s+1), + 'type3': (s + 0.1)*(s+0.1)/s**3/(s+1), + 'example21': 0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10) / ( + (s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)**2), + 'example23': ((s+0.1)**2 + 1)*(s-0.1)/(((s+0.1)**2+4)*(s+1)), + 'example25a': s/(s**2+2*s+2)**4, + 'example26a': ((s-0.1)**2 + 1)/((s + 0.1)*((s-0.2)**2 + 4)), + 'example26b': ((s-0.1)**2 + 1)/((s - 0.3)*((s-0.2)**2 + 4)) + } + systems['example24'] = systems['example21'] * 20000 + systems['example25b'] = systems['example25a'] * 100 + systems['example22'] = systems['example21'] * (s**2 - 2*s + 401) + return systems + + +@pytest.fixture +def tsys_zmore(request, tsys_zmoresystems): + tsys = request.param + tsys['sys'] = tsys_zmoresystems[tsys['sysname']] + return tsys + + +@pytest.mark.parametrize( + 'tsys_zmore', + [dict(sysname='typem1', K=2.0, atol=1.5e-3, + result=(float('Inf'), -120.0007, float('NaN'), 0.5774)), + dict(sysname='type0', K=0.8, atol=1.5e-3, + result=(10.0014, float('inf'), 1.7322, float('nan'))), + dict(sysname='type0', K=2.0, atol=1e-2, + result=(4.000, 67.6058, 1.7322, 0.7663)), + dict(sysname='type1', K=1.0, atol=1e-4, + result=(float('Inf'), 144.9032, float('NaN'), 0.3162)), + dict(sysname='type2', K=1.0, atol=1e-4, + result=(float('Inf'), 44.4594, float('NaN'), 0.7907)), + dict(sysname='type3', K=1.0, atol=1.5e-3, + result=(0.0626, 37.1748, 0.1119, 0.7951)), + dict(sysname='example21', K=1.0, atol=1e-2, + result=(0.0100, -14.5640, 0, 0.0022)), + dict(sysname='example21', K=1000.0, atol=1e-2, + result=(0.1793, 22.5215, 0.0243, 0.0630)), + dict(sysname='example21', K=5000.0, atol=1.5e-3, + result=(4.5596, 21.2101, 0.4385, 0.1868)), + ], + indirect=True) +def test_zmore_margin(tsys_zmore): + """Test margins for more tricky systems + + Note + ---- + Matlab gives gain margin 0 for system `type2`, python-control gives inf + Difficult to argue which is right? Special case or different approach? + + Edge cases, like `type0` which approaches a gain of 1 for w -> 0, are also + not identically indicated, Matlab gives phase margin -180, at w = 0. For + higher or lower gains, results match. + """ + + res = margin(tsys_zmore['sys'] * tsys_zmore['K']) + assert_allclose(res, tsys_zmore['result'], atol=tsys_zmore['atol']) + + +@pytest.mark.parametrize( + 'tsys_zmore', + [dict(sysname='example21', K=1.0, rtol=1e-3, atol=1e-3, + result=([0.01, 179.2931, 2.2798e+4, 1.5946e+07, 7.2477e+08], + [-14.5640], + [0.2496], + [0, 0.0243, 0.4385, 6.8640, 84.9323], + [0.0022], + [0.0022])), + ], + indirect=True) +def test_zmore_stability_margins(tsys_zmore): """Test stability_margins for more tricky systems with returnall""" - res = stability_margins(tzmore_sys[tmarginall['sys']]*tmarginall['K'], + res = stability_margins(tsys_zmore['sys'] * tsys_zmore['K'], returnall=True) - compare_allmargins(res, tmarginall['result'], - atol=tmarginall['atol'], - rtol=tmarginall['rtol']) + compare_allmargins(res, + tsys_zmore['result'], + atol=tsys_zmore['atol'], + rtol=tsys_zmore['rtol']) From ad0a22549f9ad5e0f84e7f81f28ee4ad405cca65 Mon Sep 17 00:00:00 2001 From: bnavigator Date: Mon, 16 Nov 2020 22:23:00 +0100 Subject: [PATCH 3/4] Tests for stability_margin and phase_crossover_frequencies with discrete TF --- control/tests/margin_test.py | 45 +++++++++++++++++++++++++++--------- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 7b513a9f5..80916da1b 100755 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -3,7 +3,8 @@ margin_test.py - test suite for stability margin commands RMM, 15 Jul 2011 -BG, 30 June 2020 -- convert to pytest, gh-425 +BG, 30 Jun 2020 -- convert to pytest, gh-425 +BG, 16 Nov 2020 -- pick from gh-438 and add discrete test """ from __future__ import print_function @@ -113,19 +114,24 @@ def test_margin_3input(tsys): assert_allclose(out, np.array(refout)[[0, 1, 3, 4]], atol=1.5e-3) -def test_phase_crossover_frequencies(): +@pytest.mark.parametrize( + 'tfargs, omega_ref, gain_ref', + [(([1], [1, 2, 3, 4]), [1.7325, 0.], [-0.5, 0.25]), + (([1], [1, 1]), [0.], [1.]), + (([2], [1, 3, 3, 1]), [1.732, 0.], [-0.25, 2.]), + ((np.array([3, 11, 3]) * 1e-4, [1., -2.7145, 2.4562, -0.7408], .1), + [1.6235, 0.], [-0.28598, 1.88889]), + ]) +def test_phase_crossover_frequencies(tfargs, omega_ref, gain_ref): """Test phase_crossover_frequencies() function""" - sys = TransferFunction([1], [1, 2, 3, 4]) + sys = TransferFunction(*tfargs) omega, gain = phase_crossover_frequencies(sys) - assert_allclose(omega, [1.73205, 0.], atol=1.5e-3) - assert_allclose(gain, [-0.5, 0.25], atol=1.5e-3) + assert_allclose(omega, omega_ref, atol=1.5e-3) + assert_allclose(gain, gain_ref, atol=1.5e-3) - tf = TransferFunction([1], [1, 1]) - omega, gain = phase_crossover_frequencies(tf) - assert_allclose(omega, [0.], atol=1.5e-3) - assert_allclose(gain, [1.], atol=1.5e-3) - # MIMO +def test_phase_crossover_frequencies_mimo(): + """Test MIMO exception""" tf = TransferFunction([[[1], [2]], [[3], [4]]], [[[1, 2, 3, 4], [1, 1]], @@ -134,7 +140,6 @@ def test_phase_crossover_frequencies(): omega, gain = phase_crossover_frequencies(tf) - def test_mag_phase_omega(): """Test for bug reported in gh-58""" sys = TransferFunction(15, [1, 6, 11, 6]) @@ -327,3 +332,21 @@ def test_zmore_stability_margins(tsys_zmore): tsys_zmore['result'], atol=tsys_zmore['atol'], rtol=tsys_zmore['rtol']) + + +@pytest.mark.parametrize( + 'cnum, cden, dt,' + 'ref,' + 'rtol', + [([2], [1, 3, 2, 0], 1e-2, # gh-465 + (2.9558, 32.8170, 0.43584, 1.4037, 0.74953, 0.97079), + 0.1 # very crude tolerance, because the gradients are not great + ), + ([2], [1, 3, 3, 1], .1, # 2/(s+1)**3 + [3.4927, 69.9996, 0.5763, 1.6283, 0.7631, 1.2019], + 1e-3)]) +def test_stability_margins_discrete(cnum, cden, dt, ref, rtol): + """Test stability_margins with discrete TF input""" + tf = TransferFunction(cnum, cden).sample(dt) + out = stability_margins(tf) + assert_allclose(out, ref, rtol=rtol) From d0f333e823bc10b268d64254c54c1a1f240c8d5b Mon Sep 17 00:00:00 2001 From: Ben Greiner Date: Wed, 18 Nov 2020 13:18:40 +0100 Subject: [PATCH 4/4] remove misguided comment about polymulx --- control/margins.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/margins.py b/control/margins.py index 299b1c493..03e78352f 100644 --- a/control/margins.py +++ b/control/margins.py @@ -145,7 +145,7 @@ def _poly_z_real_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw): p1 = np.polymul(num, den_inv_zq) p2 = np.polymul(num_inv_zp, den) if p_q < 0: - # Future: numpy >= 1.5.0 defines np.polymulx() + # * z**(-p_q) x = [1] + [0] * (-p_q) p2 = np.polymul(p2, x) z = np.roots(np.polysub(p1, p2))