python-control · josiahdelange · Jan 11, 2025 · Jan 11, 2025 · Jan 11, 2025 · Jan 12, 2025
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):
@@ -463,7 +468,6 @@ def phase_crossover_frequencies(sys):
 
     return omega, gains
 
-
 def margin(*args):
     """
     margin(sys) \
@@ -517,3 +521,132 @@ def margin(*args):
                          % len(args))
 
     return margin[0], margin[1], margin[3], margin[4]
+
+def disk_margins(L, omega, skew=0.0, returnall=False):
+    """Compute disk-based stability margins for SISO or MIMO LTI
+       loop transfer function.
+
+    Parameters
+    ----------
+    L : `StateSpace` or `TransferFunction`
+        Linear SISO or MIMO loop transfer function system
+    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 calculation.
+        skew = 0.0 (default) uses the "balanced" sensitivity function 0.5*(S - T)
+        skew = 1.0 uses the sensitivity function S
+        skew = -1.0 uses the complementary sensitivity function T
+    returnall : bool, optional
+        If True, return frequency-dependent margins. If False (default),
+        return only the 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
@@ -150,6 +150,7 @@ Frequency domain analysis:
     singular_values_plot
     singular_values_response
     sisotool
+    disk_margins
 
 Pole/zero-based analysis: