Merge pull request #566 from sawyerbfuller/frd-margin

bnavigator · web-flow · commit b41574f72c0b · 2021-03-17T11:39:13.000+01:00
new method keyword in stability_margins and auto-fallback back to FRD ..
diff --git 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
@@ -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 <minster@uw.edu>, 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
@@ -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)
