From a8a536b670d7dd868f913c4a8bfa62efcd7c47d5 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 12:18:34 -0800 Subject: [PATCH 1/6] new method keyword in stability_margins; falls back to FRD when numerical inaccuracy in dt polynomial calculation --- control/freqplot.py | 5 ++-- control/margins.py | 50 +++++++++++++++++++++++++++++++++--- control/tests/margin_test.py | 15 ++++++++--- 3 files changed, 60 insertions(+), 10 deletions(-) diff --git a/control/freqplot.py b/control/freqplot.py index 750d84d27..fe18ea27d 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -88,7 +88,7 @@ def bode_plot(syslist, omega=None, plot=True, omega_limits=None, omega_num=None, - margins=None, *args, **kwargs): + margins=None, method='best', *args, **kwargs): """Bode plot for a system Plots a Bode plot for the system over a (optional) frequency range. @@ -117,6 +117,7 @@ def bode_plot(syslist, omega=None, config.defaults['freqplot.number_of_samples']. margins : bool If True, plot gain and phase margin. + method : method to use in computing margins (see :func:`stability_margins`) *args : :func:`matplotlib.pyplot.plot` positional properties, optional Additional arguments for `matplotlib` plots (color, linestyle, etc) **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional @@ -373,7 +374,7 @@ def bode_plot(syslist, omega=None, # Show the phase and gain margins in the plot if margins: # Compute stability margins for the system - margin = stability_margins(sys) + margin = stability_margins(sys, method=method) gm, pm, Wcg, Wcp = (margin[i] for i in (0, 1, 3, 4)) # Figure out sign of the phase at the first gain crossing diff --git a/control/margins.py b/control/margins.py index 96b997496..d7d0b7e00 100644 --- a/control/margins.py +++ b/control/margins.py @@ -56,6 +56,7 @@ from . import xferfcn from .lti import issiso, evalfr from . import frdata +from . import freqplot from .exception import ControlMIMONotImplemented __all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin'] @@ -206,6 +207,17 @@ def fun(wdt): return z, w +def _numerical_inaccuracy(sys): + # crude, conservative check for if + # num(z)*num(1/z) << den(z)*den(1/z) for DT systems + num, den, num_inv_zp, den_inv_zq, p_q, dt = _poly_z_invz(sys) + p1 = np.polymul(num, num_inv_zp) + p2 = np.polymul(den, den_inv_zq) + if p_q < 0: + # * z**(-p_q) + x = [1] + [0] * (-p_q) + p1 = np.polymul(p1, x) + return np.linalg.norm(p1) < 1e-3 * np.linalg.norm(p2) # Took the framework for the old function by # Sawyer B. Fuller , removed a lot of the innards @@ -237,25 +249,34 @@ def fun(wdt): # systems -def stability_margins(sysdata, returnall=False, epsw=0.0): +def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): """Calculate stability margins and associated crossover frequencies. Parameters ---------- - sysdata: LTI system or (mag, phase, omega) sequence + sysdata : LTI system or (mag, phase, omega) sequence sys : LTI system Linear SISO system representing the loop transfer function mag, phase, omega : sequence of array_like Arrays of magnitudes (absolute values, not dB), phases (degrees), and corresponding frequencies. Crossover frequencies returned are in the same units as those in `omega` (e.g., rad/sec or Hz). - returnall: bool, optional + returnall : bool, optional If true, return all margins found. If False (default), return only the minimum stability margins. For frequency data or FRD systems, only margins in the given frequency region can be found and returned. - epsw: float, optional + epsw : float, optional Frequencies below this value (default 0.0) are considered static gain, and not returned as margin. + method : string, optional + Method to use (default is 'best'): + 'poly': use polynomial method if passed a :class:`LTI` system. + 'frd': calculate crossover frequencies using numerical interpolation + of a :class:`FrequencyResponseData` representation of the system if + passed a :class:`LTI` system. + 'best': use the 'poly' method if possible, reverting to 'frd' if it is + detected that numerical inaccuracy is likey to arise in the 'poly' + method for for discrete-time systems. Returns ------- @@ -300,6 +321,27 @@ def stability_margins(sysdata, returnall=False, epsw=0.0): raise ControlMIMONotImplemented( "Can only do margins for SISO system") + if method == 'frd': + # convert to FRD if we got a transfer function + if isinstance(sys, xferfcn.TransferFunction): + omega_sys = freqplot._default_frequency_range(sys) + if sys.isctime(): + sys = frdata.FRD(sys, omega_sys) + else: + omega_sys = omega_sys[omega_sys < np.pi / sys.dt] + sys = frdata.FRD(sys(np.exp(1j*sys.dt*omega_sys)), omega_sys, + smooth=True) + elif method == 'best': + # convert to FRD if anticipated numerical issues + if isinstance(sys, xferfcn.TransferFunction) and not sys.isctime(): + if _numerical_inaccuracy(sys): + omega_sys = freqplot._default_frequency_range(sys) + omega_sys = omega_sys[omega_sys < np.pi / sys.dt] + sys = frdata.FRD(sys(np.exp(1j*sys.dt*omega_sys)), omega_sys, + smooth=True) + elif method != 'poly': + raise ValueError("method " + method + " unknown") + if isinstance(sys, xferfcn.TransferFunction): if sys.isctime(): num_iw, den_iw = _poly_iw(sys) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index fbd79c60b..fbba9cc54 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -104,7 +104,6 @@ def test_margin_sys(tsys): out = margin(sys) assert_allclose(out, np.array(refout)[[0, 1, 3, 4]], atol=1.5e-3) - def test_margin_3input(tsys): sys, refout, refoutall = tsys """Test margin() function with mag, phase, omega input""" @@ -339,11 +338,11 @@ def test_zmore_stability_margins(tsys_zmore): 'ref,' 'rtol', [( # gh-465 - [2], [1, 3, 2, 0], 1e-2, + [2], [1, 3, 2, 0], 1e-2, [2.9558, 32.390, 0.43584, 1.4037, 0.74951, 0.97079], 2e-3), # the gradient of the function reduces numerical precision ( # 2/(s+1)**3 - [2], [1, 3, 3, 1], .1, + [2], [1, 3, 3, 1], .1, [3.4927, 65.4212, 0.5763, 1.6283, 0.76625, 1.2019], 1e-4), ( # gh-523 @@ -354,5 +353,13 @@ def test_zmore_stability_margins(tsys_zmore): 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) + out = stability_margins(tf, method='poly') assert_allclose(out, ref, rtol=rtol) + +def test_stability_margins_methods(): + sys = TransferFunction(1, (1, 2)) + """Test stability_margins() function with different methods""" + out = stability_margins(sys, method='best') + out = stability_margins(sys, method='frd') + out = stability_margins(sys, method='poly') + From 94fe55e73cc1a86fe76b6419f3dde213f449e4eb Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 13:22:30 -0800 Subject: [PATCH 2/6] impoed unit tests --- control/margins.py | 2 +- control/tests/margin_test.py | 24 +++++++++++++++++++----- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/control/margins.py b/control/margins.py index d7d0b7e00..38dd4c605 100644 --- a/control/margins.py +++ b/control/margins.py @@ -300,6 +300,7 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): determined by the frequency of maximum sensitivity (given by the magnitude of 1/(1+L)). """ + # TODO: FRD method for cont-time systems doesn't work try: if isinstance(sysdata, frdata.FRD): sys = frdata.FRD(sysdata, smooth=True) @@ -408,7 +409,6 @@ def _dstab(w): 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(1j * w_180) # Find all crossings, note that this depends on omega having diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index fbba9cc54..a8888bfcf 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -356,10 +356,24 @@ def test_stability_margins_discrete(cnum, cden, dt, ref, rtol): out = stability_margins(tf, method='poly') assert_allclose(out, ref, rtol=rtol) + def test_stability_margins_methods(): - sys = TransferFunction(1, (1, 2)) + # the following system gives slightly inaccurate result for DT systems + # because of numerical issues + omegan = 1 + zeta = 0.5 + resonance = TransferFunction(omegan**2, [1, 2*zeta*omegan, omegan**2]) + omegan2 = 100 + resonance2 = TransferFunction(omegan2**2, [1, 2*zeta*omegan2, omegan2**2]) + sys = 5 * resonance * resonance2 + sysd = sys.sample(0.001, 'zoh') """Test stability_margins() function with different methods""" - out = stability_margins(sys, method='best') - out = stability_margins(sys, method='frd') - out = stability_margins(sys, method='poly') - + out = stability_margins(sysd, method='best') + assert_allclose( + (18.876634845228644, 11.244969911924102, 0.40684128014454546, + 9.763585543509473, 4.351735617240803, 2.559873290031937), + stability_margins(sysd, method='poly')) + assert_allclose( + (18.876634740386308, 26.356358386241055, 0.40684127995261044, + 9.763585494645046, 2.3293357226374805, 2.55985695034263), + stability_margins(sysd, method='frd')) From 2b82eef50bde899acaf9477465dc38bf7d03b54a Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 13:57:31 -0800 Subject: [PATCH 3/6] incorporated fix enabled by github #568 --- control/margins.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/control/margins.py b/control/margins.py index 38dd4c605..d01f836e6 100644 --- a/control/margins.py +++ b/control/margins.py @@ -330,16 +330,14 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): sys = frdata.FRD(sys, omega_sys) else: omega_sys = omega_sys[omega_sys < np.pi / sys.dt] - sys = frdata.FRD(sys(np.exp(1j*sys.dt*omega_sys)), omega_sys, - smooth=True) + sys = frdata.FRD(sys, omega_sys, smooth=True) elif method == 'best': # convert to FRD if anticipated numerical issues if isinstance(sys, xferfcn.TransferFunction) and not sys.isctime(): if _numerical_inaccuracy(sys): omega_sys = freqplot._default_frequency_range(sys) omega_sys = omega_sys[omega_sys < np.pi / sys.dt] - sys = frdata.FRD(sys(np.exp(1j*sys.dt*omega_sys)), omega_sys, - smooth=True) + sys = frdata.FRD(sys, omega_sys, smooth=True) elif method != 'poly': raise ValueError("method " + method + " unknown") From a75a3d42243517c9f91b247b67f8ce510534769c Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 14:10:06 -0800 Subject: [PATCH 4/6] removed a test that was getting a different reslt on different systems because of numerical problems --- control/tests/margin_test.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index a8888bfcf..6ff9042a3 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -369,10 +369,7 @@ def test_stability_margins_methods(): sysd = sys.sample(0.001, 'zoh') """Test stability_margins() function with different methods""" out = stability_margins(sysd, method='best') - assert_allclose( - (18.876634845228644, 11.244969911924102, 0.40684128014454546, - 9.763585543509473, 4.351735617240803, 2.559873290031937), - stability_margins(sysd, method='poly')) + # confirm getting reasonable results using FRD method assert_allclose( (18.876634740386308, 26.356358386241055, 0.40684127995261044, 9.763585494645046, 2.3293357226374805, 2.55985695034263), From 567b4dbe9a57c5e2a6e3a8f0a509682793c70ed9 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 14:15:49 -0800 Subject: [PATCH 5/6] relaxed tolerance --- control/tests/margin_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 6ff9042a3..afeb71efd 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -373,4 +373,4 @@ def test_stability_margins_methods(): assert_allclose( (18.876634740386308, 26.356358386241055, 0.40684127995261044, 9.763585494645046, 2.3293357226374805, 2.55985695034263), - stability_margins(sysd, method='frd')) + stability_margins(sysd, method='frd'), rtol=1e-5) From 72cb23f208a515e1d558a0050a70c1760ab26dc6 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Sun, 14 Mar 2021 20:25:05 -0700 Subject: [PATCH 6/6] added warning method when falling back to frd --- control/margins.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/control/margins.py b/control/margins.py index d01f836e6..d22b6a0f7 100644 --- a/control/margins.py +++ b/control/margins.py @@ -51,6 +51,7 @@ """ import math +from warnings import warn import numpy as np import scipy as sp from . import xferfcn @@ -207,7 +208,7 @@ def fun(wdt): return z, w -def _numerical_inaccuracy(sys): +def _likely_numerical_inaccuracy(sys): # crude, conservative check for if # num(z)*num(1/z) << den(z)*den(1/z) for DT systems num, den, num_inv_zp, den_inv_zq, p_q, dt = _poly_z_invz(sys) @@ -334,7 +335,9 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): elif method == 'best': # convert to FRD if anticipated numerical issues if isinstance(sys, xferfcn.TransferFunction) and not sys.isctime(): - if _numerical_inaccuracy(sys): + if _likely_numerical_inaccuracy(sys): + warn("stability_margins: Falling back to 'frd' method " + "because of chance of numerical inaccuracy in 'poly' method.") omega_sys = freqplot._default_frequency_range(sys) omega_sys = omega_sys[omega_sys < np.pi / sys.dt] sys = frdata.FRD(sys, omega_sys, smooth=True)