Remove debugging statements, update comments, add unit tests.

josiahdelange · josiahdelange · commit cffc3e505bb8 · 2025-04-22T20:56:44.000-04:00
diff --git a/control/margins.py b/control/margins.py
@@ -653,20 +653,23 @@ def disk_margins(L, omega, skew = 0.0, returnall = False):
         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)
-    DGM = np.zeros(omega.shape, np.float64)
-    DPM = np.zeros(omega.shape, np.float64)
+    # the structured singular value, a.k.a. "mu", of (S + (skew - I)/2).
+    DM = np.zeros(omega.shape, np.float64) # disk margin vs frequency
+    DGM = np.zeros(omega.shape, np.float64) # disk-based gain margin vs. frequency
+    DPM = np.zeros(omega.shape, np.float64) # disk-based phase margin vs. frequency
     for ii in range(0,len(omega)):
         # Disk margin (a.k.a. "alpha") vs. frequency
         if L.issiso() and (ab13md == None):
-            DM[ii] = np.minimum(1e5,
-                1.0/bode(ST_jw, omega = omega[ii], plot = False)[0])
+            # For the SISO case, the norm on (S + (skew - I)/2) is
+            # unstructured, and can be computed as Bode magnitude
+            DM[ii] = 1.0/bode(ST_jw, omega = omega[ii], plot = False)[0]
         else:
-            DM[ii] = np.minimum(1e5,
-                1.0/ab13md(ST_jw[ii], np.array(ny*[1]), np.array(ny*[2]))[0])
+            # 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.
+            DM[ii] = 1.0/ab13md(ST_jw[ii], np.array(ny*[1]), np.array(ny*[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))
diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py
@@ -14,7 +14,7 @@
 
 from control import ControlMIMONotImplemented, FrequencyResponseData, \
     StateSpace, TransferFunction, margin, phase_crossover_frequencies, \
-    stability_margins
+    stability_margins, disk_margins, tf, ss
 
 s = TransferFunction.s
 
@@ -372,3 +372,107 @@ 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)
+
+    # Laplace variable
+    s = tf('s')
+
+    # 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)
+
+    # Laplace variable
+    s = tf('s')
+
+    # Loop transfer gain
+    P = ss([[0, 10],[-10, 0]], np.eye(2), [[1, 10], [-10, 1]], [[0, 0],[0, 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
+
+    # 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
+
+def test_siso_disk_margin_return_all():
+    # Frequencies of interest
+    omega = np.logspace(-1, 2, 1001)
+
+    # Laplace variable
+    s = tf('s')
+
+    # 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)
+
+    # Laplace variable
+    s = tf('s')
+
+    # Loop transfer gain
+    P = ss([[0, 10],[-10, 0]], np.eye(2),\
+        [[1, 10], [-10, 1]], [[0, 0],[0, 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
+
+    # 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
+
