Initial version of disk margin calculation and example/test script

murrayrm · Jun 25, 2025 · Jan 11, 2025 · Jan 11, 2025 · Jan 11, 2025 · Jan 12, 2025
commit f6dada2b7fb5a683169abb05faddd23d818a1389
diff --git a/control/margins.py b/control/margins.py
@@ -57,9 +57,23 @@
 from . import frdata
 from . import freqplot
 from .exception import ControlMIMONotImplemented
-
-__all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin']
-
+from . import ss
+try:
+    from slycot import ab13md
+except ImportError:
+    ab13md = None
+try:
+    from . import mag2db
+except:
+    # Likely the following:
+    #
+    # ImportError: cannot import name 'mag2db' from partially initialized module
+    # 'control' (most likely due to a circular import) (control/__init__.py)
+    #
+    def mag2db(mag):
+        return 20*np.log10(mag)
+
+__all__ = ['stability_margins', 'phase_crossover_frequencies', 'margin', 'disk_margins']
 
 # private helper functions
 def _poly_iw(sys):
@@ -501,6 +515,143 @@ def phase_crossover_frequencies(sys):
 
     return omega, gain
 
+def disk_margins(L, omega, skew = 0.0):
+    """Compute disk-based stability margins for SISO or MIMO LTI system.
+
+    Parameters
+    ----------
+    L : SISO or MIMO LTI system representing the loop transfer function
+    omega : ndarray
+        1d array of (non-negative) frequencies at which to evaluate
+        the disk-based stability margins
+    skew : (optional, default = 0) skew parameter for disk margin calculation.
+        skew = 0 uses the "balanced" sensitivity function 0.5*(S - T)
+        skew = -1 uses the sensitivity function S
+        skew = 1 uses the complementary sensitivity function T
+
+    Returns
+    -------
+    DM : ndarray
+        1d array of frequency-dependent disk margins.  DM is the same
+        size as "omega" parameter.
+    GM : ndarray
+        1d array of frequency-dependent disk-based gain margins, in dB.
+        GM is the same size as "omega" parameter.
+    PM : ndarray
+        1d array of frequency-dependent disk-based phase margins, in deg.
+        PM is the same size as "omega" parameter.
+
+    Examples
+    --------
+    >> import control
+    >> import numpy as np
+    >> import matplotlib
+    >> import matplotlib.pyplot as plt
+    >>
+    >> omega = np.logspace(-1, 3, 1001)
+    >> P = control.ss([[0, 10],[-10, 0]], np.eye(2), [[1, 10], [-10, 1]], [[0, 0],[0, 0]])
+    >> K = control.ss([],[],[], [[1, -2], [0, 1]])
+    >> L = P*K
+    >> DM, GM, PM = control.disk_margins(L, omega, 0.0) # balanced (S - T)
+    >> print(f"min(DM) = {min(DM)}")
+    >> print(f"min(GM) = {min(GM)} dB")
+    >> print(f"min(PM) = {min(PM)} deg")
+    >>
+    >> plt.figure(1)
+    >> plt.subplot(3,1,1)
+    >> plt.semilogx(omega, DM, label='$\\alpha$')
+    >> plt.legend()
+    >> plt.title('Disk Margin (Outputs)')
+    >> plt.grid()
+    >> plt.tight_layout()
+    >> plt.xlim([omega[0], omega[-1]])
+    >>
+    >> plt.figure(1)
+    >> plt.subplot(3,1,2)
+    >> plt.semilogx(omega, GM, label='$\\gamma_{m}$')
+    >> plt.ylabel('Margin (dB)')
+    >> plt.legend()
+    >> plt.title('Disk-Based Gain Margin (Outputs)')
+    >> plt.grid()
+    >> plt.ylim([0, 40])
+    >> plt.tight_layout()
+    >> plt.xlim([omega[0], omega[-1]])
+    >>
+    >> plt.figure(1)
+    >> plt.subplot(3,1,3)
+    >> plt.semilogx(omega, PM, label='$\\phi_{m}$')
+    >> plt.ylabel('Margin (deg)')
+    >> plt.legend()
+    >> plt.title('Disk-Based Phase Margin (Outputs)')
+    >> plt.grid()
+    >> plt.ylim([0, 90])
+    >> plt.tight_layout()
+    >> plt.xlim([omega[0], omega[-1]])
+
+    References
+    ----------
+    [1] Blight, James D., R. Lane Dailey, and Dagfinn Gangsaas. “Practical
+        Control Law Design for Aircraft Using Multivariable Techniques.”
+        International Journal of Control 59, no. 1 (January 1994): 93-137.
+        https://doi.org/10.1080/00207179408923071.
+
+    [2] Seiler, Peter, Andrew Packard, and Pascal Gahinet. “An Introduction
+        to Disk Margins [Lecture Notes].” IEEE Control Systems Magazine 40,
+        no. 5 (October 2020): 78-95.
+
+    [3] P. Benner, V. Mehrmann, V. Sima, S. Van Huffel, and A. Varga, "SLICOT
+        - A Subroutine Library in Systems and Control Theory", Applied and
+        Computational Control, Signals, and Circuits (Birkhauser), Vol. 1, Ch.
+        10, pp. 505-546, 1999.
+
+    [4] S. Van Huffel, V. Sima, A. Varga, S. Hammarling, and F. Delebecque,
+        "Development of High Performance Numerical Software for Control", IEEE
+        Control Systems Magazine, Vol. 24, Nr. 1, Feb., pp. 60-76, 2004.
+    """
+
+    # Get dimensions of feedback system
+    ny,_ = ss(L).C.shape
+    I = ss([], [], [], np.eye(ny))
+
+    # 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 + (skew - 1)*I/2
+    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 - 1)/2).
+    # Uses SLICOT routine AB13MD to compute. [1,3-4].
+    DM = np.zeros(omega.shape, np.float64)
+    GM = np.zeros(omega.shape, np.float64)
+    PM = np.zeros(omega.shape, np.float64)
+    for ii in range(0,len(omega)):
+        # Disk margin (magnitude) vs. frequency
+        DM[ii] = 1/ab13md(ST_jw[ii], np.array(ny*[1]), np.array(ny*[2]))[0]
+
+        # Gain-only margin (dB) vs. frequency
+        gamma_min = (1 - DM[ii]*(1 - skew)/2)/(1 + DM[ii]*(1 + skew)/2)
+        gamma_max = (1 + DM[ii]*(1 - skew)/2)/(1 - DM[ii]*(1 + skew)/2)
+        GM[ii] = mag2db(np.minimum(1/gamma_min, gamma_max))
+
+        # Phase-only margin (deg) vs. frequency
+        if math.isinf(gamma_max):
+            PM[ii] = 90.0
+        else:
+            PM[ii] = (1 + gamma_min*gamma_max)/(gamma_min + gamma_max)
+            if PM[ii] >= 1.0:
+                PM[ii] = 0.0 # np.arccos(1.0)
+            elif PM[ii] <= -1.0:
+                PM[ii] = float('Inf') # np.arccos(-1.0)
+            else:
+                PM[ii] = np.rad2deg(np.arccos(PM[ii]))
+
+    return (DM, GM, PM)
 
 def margin(*args):
     """margin(sysdata)