Disk margin calculations (#1146)

josiahdelange · web-flow · commit aa92b653d4f0 · 2025-06-25T09:54:25.000-04:00
* Initial version of disk margin calculation and example/test script * Comment updates: update margins.py header, clarify import exception handler comment, fix typo in skew description of disk_margins docstring * More work in progress on disk margin calculation, adding new prototype function to plot allowable gain/phase variations. * Add disk_margin_plot to subroutine list in comment header in margins.py * Follow-on to ba15789, add disk_margin_plot to list of functions within the margins module * More work in progress on disk_margin_plot. Corrected a typo/bug in the calculation of 'f', the bounding complex curve. Seems to look correct for balanced (skew = 0) case, still verifying the skewed equivalent. * Further progress/debugging on disk margin calculation + plot utility * Clean up docstring/code for disk_margin_plot * Clean up docstring/code for disk_margin_plot * Remove debugging statements, update comments, add unit tests. * Minor change to fix logic to find minimum across DGM, DPM numpy vectors * Rename disk margin example, since unit tests are now written in control/tests/margin_test.py * Remove unneeded dependencies from margins.py, used for debugging * Minor updates to docstrings * Undo d92fb20 * Minor tweaks to plots in example script for readability * Fix typo in disk_margin_plot. * Fix mag2db import hack/workaround and trim down disk_margin docstring. * Add input handling to disk_margin, clean up column width/comments * Move disk_margin_plot out of the library into the example script * Recommended changes from the linter * Follow-on to 5f34a7b * Add disk_margins to function list * Whittle down the docstring from disk_margins * Put more comments in the disk margin example, add example to documentation * Fixing docstrings * Corrected expected values for 'no-slycot' condition in newly-added unit tests * Attempt #2 at 397efab, based on linter recommendation * Address @murrayrm review comments. * Update formatting per PEP8/@murrayrm review comments. Add additional reference on disk/ellipse-based margin calculations. * Follow-on to e8897f6: remove now-unnecessary import of importlib * Update formatting per @murrayrm review comments * Remove temporarily-added string from docstring * Minor tweak to docstring to fit the word 'function' back into the description of skew = 0.0
diff --git a/control/margins.py b/control/margins.py
@@ -11,12 +11,17 @@
 import numpy as np
 import scipy as sp
 
-from . import frdata, freqplot, xferfcn
+from . import frdata, freqplot, xferfcn, statesp
 from .exception import ControlMIMONotImplemented
 from .iosys import issiso
+from .ctrlutil import mag2db
+try:
+    from slycot import ab13md
+except ImportError:
+    ab13md = None
 
-__all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin']
-
+__all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin',
+           'disk_margins']
 
 # private helper functions
 def _poly_iw(sys):
@@ -168,6 +173,7 @@ 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
@@ -517,3 +523,135 @@ def margin(*args):
                          % len(args))
 
     return margin[0], margin[1], margin[3], margin[4]
+
+
+def disk_margins(L, omega, skew=0.0, returnall=False):
+    """Disk-based stability margins of loop transfer function.
+
+    Parameters
+    ----------
+    L : `StateSpace` or `TransferFunction`
+        Linear SISO or MIMO loop transfer function.
+    omega : sequence of array_like
+        1D array of (non-negative) frequencies (rad/s) at which
+        to evaluate the disk-based stability margins.
+    skew : float or array_like, optional
+        skew parameter(s) for disk margin (default = 0.0).
+        skew = 0.0 "balanced" sensitivity function 0.5*(S - T).
+        skew = 1.0 sensitivity function S.
+        skew = -1.0 complementary sensitivity function T.
+    returnall : bool, optional
+        If True, return frequency-dependent margins.
+        If False (default), return worst-case (minimum) margins.
+
+    Returns
+    -------
+    DM : float or array_like
+        Disk margin.
+    DGM : float or array_like
+        Disk-based gain margin.
+    DPM : float or array_like
+        Disk-based phase margin.
+
+    Example
+    --------
+    >> omega = np.logspace(-1, 3, 1001)
+    >> P = control.ss([[0, 10], [-10, 0]], np.eye(2), [[1, 10],
+    [-10, 1]], 0)
+    >> K = control.ss([], [], [], [[1, -2], [0, 1]])
+    >> L = P * K
+    >> DM, DGM, DPM = control.disk_margins(L, omega, skew=0.0)
+    """
+
+    # First argument must be a system
+    if not isinstance(L, (statesp.StateSpace, xferfcn.TransferFunction)):
+        raise ValueError(
+            "Loop gain must be state-space or transfer function object")
+
+    # Loop transfer function must be square
+    if statesp.ss(L).B.shape[1] != statesp.ss(L).C.shape[0]:
+        raise ValueError("Loop gain must be square (n_inputs = n_outputs)")
+
+    # Need slycot if L is MIMO, for mu calculation
+    if not L.issiso() and ab13md == None:
+        raise ControlMIMONotImplemented(
+            "Need slycot to compute MIMO disk_margins")
+
+    # Get dimensions of feedback system
+    num_loops = statesp.ss(L).C.shape[0]
+    I = statesp.ss([], [], [], np.eye(num_loops))
+
+    # Loop sensitivity function
+    S = I.feedback(L)
+
+    # Compute frequency response of the "balanced" (according
+    # to the skew parameter "sigma") sensitivity function [1-2]
+    ST = S + 0.5 * (skew - 1) * I
+    ST_mag, ST_phase, _ = ST.frequency_response(omega)
+    ST_jw = (ST_mag * np.exp(1j * ST_phase))
+    if not L.issiso():
+        ST_jw = ST_jw.transpose(2, 0, 1)
+
+    # Frequency-dependent complex disk margin, computed using
+    # upper bound of the structured singular value, a.k.a. "mu",
+    # of (S + (skew - I)/2).
+    DM = np.zeros(omega.shape)
+    DGM = np.zeros(omega.shape)
+    DPM = np.zeros(omega.shape)
+    for ii in range(0, len(omega)):
+        # Disk margin (a.k.a. "alpha") vs. frequency
+        if L.issiso() and ab13md == None:
+            # For the SISO case, the norm on (S + (skew - I)/2) is
+            # unstructured, and can be computed as the magnitude
+            # of the frequency response.
+            DM[ii] = 1.0 / ST_mag[ii]
+        else:
+            # For the MIMO case, the norm on (S + (skew - I)/2)
+            # assumes a single complex uncertainty block diagonal
+            # uncertainty structure. AB13MD provides an upper bound
+            # on this norm at the given frequency omega[ii].
+            DM[ii] = 1.0 / ab13md(ST_jw[ii], np.array(num_loops * [1]),
+                                  np.array(num_loops * [2]))[0]
+
+        # Disk-based gain margin (dB) and phase margin (deg)
+        with np.errstate(divide='ignore', invalid='ignore'):
+            # Real-axis intercepts with the disk
+            gamma_min = (1 - 0.5 * DM[ii] * (1 - skew)) \
+                      / (1 + 0.5 * DM[ii] * (1 + skew))
+            gamma_max = (1 + 0.5 * DM[ii] * (1 - skew)) \
+                      / (1 - 0.5 * DM[ii] * (1 + skew))
+
+            # Gain margin (dB)
+            DGM[ii] = mag2db(np.minimum(1 / gamma_min, gamma_max))
+            if np.isnan(DGM[ii]):
+                DGM[ii] = float('inf')
+
+            # Phase margin (deg)
+            if np.isinf(gamma_max):
+                DPM[ii] = 90.0
+            else:
+                DPM[ii] = (1 + gamma_min * gamma_max) \
+                        / (gamma_min + gamma_max)
+                if abs(DPM[ii]) >= 1.0:
+                    DPM[ii] = float('Inf')
+                else:
+                    DPM[ii] = np.rad2deg(np.arccos(DPM[ii]))
+
+    if returnall:
+        # Frequency-dependent disk margin, gain margin and phase margin
+        return DM, DGM, DPM
+    else:
+        # Worst-case disk margin, gain margin and phase margin
+        if DGM.shape[0] and not np.isinf(DGM).all():
+            with np.errstate(all='ignore'):
+                gmidx = np.where(DGM == np.min(DGM))
+        else:
+            gmidx = -1
+
+        if DPM.shape[0]:
+            pmidx = np.where(DPM == np.min(DPM))
+
+        return (
+            float('inf') if DM.shape[0] == 0 else np.amin(DM),
+            float('inf') if gmidx == -1 else DGM[gmidx][0],
+            float('inf') if DPM.shape[0] == 0 else DPM[pmidx][0])
diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py
@@ -14,7 +14,8 @@
 
 from control import ControlMIMONotImplemented, FrequencyResponseData, \
     StateSpace, TransferFunction, margin, phase_crossover_frequencies, \
-    stability_margins
+    stability_margins, disk_margins, tf, ss
+from control.exception import slycot_check
 
 s = TransferFunction.s
 
@@ -372,3 +373,106 @@ def test_stability_margins_discrete(cnum, cden, dt,
     else:
         out = stability_margins(tf)
     assert_allclose(out, ref, rtol=rtol)
+
+def test_siso_disk_margin():
+    # Frequencies of interest
+    omega = np.logspace(-1, 2, 1001)
+
+    # Loop transfer function
+    L = tf(25, [1, 10, 10, 10])
+
+    # Balanced (S - T) disk-based stability margins
+    DM, DGM, DPM = disk_margins(L, omega, skew=0.0)
+    assert_allclose([DM], [0.46], atol=0.1) # disk margin of 0.46
+    assert_allclose([DGM], [4.05], atol=0.1) # disk-based gain margin of 4.05 dB 
+    assert_allclose([DPM], [25.8], atol=0.1) # disk-based phase margin of 25.8 deg
+
+    # For SISO systems, the S-based (S) disk margin should match the third output
+    # of existing library "stability_margins", i.e., minimum distance from the
+    # Nyquist plot to -1.
+    _, _, SM = stability_margins(L)[:3]
+    DM = disk_margins(L, omega, skew=1.0)[0]
+    assert_allclose([DM], [SM], atol=0.01)
+
+def test_mimo_disk_margin():
+    # Frequencies of interest
+    omega = np.logspace(-1, 3, 1001)
+
+    # Loop transfer gain
+    P = ss([[0, 10], [-10, 0]], np.eye(2), [[1, 10], [-10, 1]], 0) # plant
+    K = ss([], [], [], [[1, -2], [0, 1]]) # controller
+    Lo = P * K # loop transfer function, broken at plant output
+    Li = K * P # loop transfer function, broken at plant input
+
+    if slycot_check():
+        # Balanced (S - T) disk-based stability margins at plant output
+        DMo, DGMo, DPMo = disk_margins(Lo, omega, skew=0.0)
+        assert_allclose([DMo], [0.3754], atol=0.1) # disk margin of 0.3754
+        assert_allclose([DGMo], [3.3], atol=0.1) # disk-based gain margin of 3.3 dB 
+        assert_allclose([DPMo], [21.26], atol=0.1) # disk-based phase margin of 21.26 deg
+
+        # Balanced (S - T) disk-based stability margins at plant input
+        DMi, DGMi, DPMi = disk_margins(Li, omega, skew=0.0)
+        assert_allclose([DMi], [0.3754], atol=0.1) # disk margin of 0.3754
+        assert_allclose([DGMi], [3.3], atol=0.1) # disk-based gain margin of 3.3 dB 
+        assert_allclose([DPMi], [21.26], atol=0.1) # disk-based phase margin of 21.26 deg
+    else:
+        # Slycot not installed.  Should throw exception.
+        with pytest.raises(ControlMIMONotImplemented,\
+            match="Need slycot to compute MIMO disk_margins"):
+            DMo, DGMo, DPMo = disk_margins(Lo, omega, skew=0.0)
+
+def test_siso_disk_margin_return_all():
+    # Frequencies of interest
+    omega = np.logspace(-1, 2, 1001)
+
+    # Loop transfer function
+    L = tf(25, [1, 10, 10, 10])
+
+    # Balanced (S - T) disk-based stability margins
+    DM, DGM, DPM = disk_margins(L, omega, skew=0.0, returnall=True)
+    assert_allclose([omega[np.argmin(DM)]], [1.94],\
+        atol=0.01) # sensitivity peak at 1.94 rad/s
+    assert_allclose([min(DM)], [0.46], atol=0.1) # disk margin of 0.46
+    assert_allclose([DGM[np.argmin(DM)]], [4.05],\
+        atol=0.1) # disk-based gain margin of 4.05 dB 
+    assert_allclose([DPM[np.argmin(DM)]], [25.8],\
+        atol=0.1) # disk-based phase margin of 25.8 deg
+
+def test_mimo_disk_margin_return_all():
+    # Frequencies of interest
+    omega = np.logspace(-1, 3, 1001)
+
+    # Loop transfer gain
+    P = ss([[0, 10], [-10, 0]], np.eye(2),\
+        [[1, 10], [-10, 1]], 0) # plant
+    K = ss([], [], [], [[1, -2], [0, 1]]) # controller
+    Lo = P * K # loop transfer function, broken at plant output
+    Li = K * P # loop transfer function, broken at plant input
+
+    if slycot_check():
+        # Balanced (S - T) disk-based stability margins at plant output
+        DMo, DGMo, DPMo = disk_margins(Lo, omega, skew=0.0, returnall=True)
+        assert_allclose([omega[np.argmin(DMo)]], [omega[0]],\
+            atol=0.01) # sensitivity peak at 0 rad/s (or smallest provided)
+        assert_allclose([min(DMo)], [0.3754], atol=0.1) # disk margin of 0.3754
+        assert_allclose([DGMo[np.argmin(DMo)]], [3.3],\
+            atol=0.1) # disk-based gain margin of 3.3 dB 
+        assert_allclose([DPMo[np.argmin(DMo)]], [21.26],\
+            atol=0.1) # disk-based phase margin of 21.26 deg
+
+        # Balanced (S - T) disk-based stability margins at plant input
+        DMi, DGMi, DPMi = disk_margins(Li, omega, skew=0.0, returnall=True)
+        assert_allclose([omega[np.argmin(DMi)]], [omega[0]],\
+            atol=0.01) # sensitivity peak at 0 rad/s (or smallest provided)
+        assert_allclose([min(DMi)], [0.3754],\
+            atol=0.1) # disk margin of 0.3754
+        assert_allclose([DGMi[np.argmin(DMi)]], [3.3],\
+            atol=0.1) # disk-based gain margin of 3.3 dB 
+        assert_allclose([DPMi[np.argmin(DMi)]], [21.26],\
+            atol=0.1) # disk-based phase margin of 21.26 deg
+    else:
+        # Slycot not installed.  Should throw exception.
+        with pytest.raises(ControlMIMONotImplemented,\
+            match="Need slycot to compute MIMO disk_margins"):
+            DMo, DGMo, DPMo = disk_margins(Lo, omega, skew=0.0, returnall=True)
diff --git a/doc/examples/disk_margins.rst b/doc/examples/disk_margins.rst
@@ -0,0 +1,19 @@
+Disk margin example
+------------------------------------------
+
+This example demonstrates the use of the `disk_margins` routine
+to compute robust stability margins for a feedback system, i.e.,
+variation in gain and phase one or more loops.  The SISO examples
+are drawn from the published paper and the MIMO example is the
+"spinning satellite" example from the MathWorks documentation.
+
+Code
+....
+.. literalinclude:: disk_margins.py
+   :language: python
+   :linenos:
+
+Notes
+.....
+1. The environment variable `PYCONTROL_TEST_EXAMPLES` is used for
+testing to turn off plotting of the outputs.
diff --git a/doc/functions.rst b/doc/functions.rst
@@ -142,6 +142,7 @@ Frequency domain analysis:
 
     bandwidth
     dcgain
+    disk_margins
     linfnorm
     margin
     stability_margins
diff --git a/examples/disk_margins.py b/examples/disk_margins.py
