new method keyword in stability_margins; falls back to FRD when numerical inaccuracy in dt polynomial calculation

sawyerbfuller · sawyerbfuller · commit a8a536b670d7 · 2021-03-12T12:18:34.000-08:00
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
@@ -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 <minster@uw.edu>, 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
@@ -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')
+
