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..d22b6a0f7 100644 --- a/control/margins.py +++ b/control/margins.py @@ -51,11 +51,13 @@ """ import math +from warnings import warn import numpy as np import scipy as sp 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 +208,17 @@ def fun(wdt): return z, w +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) + 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 +250,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 ------- @@ -279,6 +301,7 @@ def stability_margins(sysdata, returnall=False, epsw=0.0): 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) @@ -300,6 +323,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, omega_sys, smooth=True) + elif method == 'best': + # convert to FRD if anticipated numerical issues + if isinstance(sys, xferfcn.TransferFunction) and not sys.isctime(): + 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) + elif method != 'poly': + raise ValueError("method " + method + " unknown") + if isinstance(sys, xferfcn.TransferFunction): if sys.isctime(): num_iw, den_iw = _poly_iw(sys) @@ -366,7 +410,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 fbd79c60b..afeb71efd 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,24 @@ 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(): + # 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(sysd, method='best') + # confirm getting reasonable results using FRD method + assert_allclose( + (18.876634740386308, 26.356358386241055, 0.40684127995261044, + 9.763585494645046, 2.3293357226374805, 2.55985695034263), + stability_margins(sysd, method='frd'), rtol=1e-5)