diff --git a/control/margins.py b/control/margins.py index d7c7992be..e51aa40af 100644 --- 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 index 43cd68ae3..23ef00aac 100644 --- 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 new file mode 100644 index 000000000..e7938f4ac --- /dev/null +++ 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 index d657fd431..8432f7fcf 100644 --- 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 new file mode 100644 index 000000000..1b9934156 --- /dev/null +++ b/examples/disk_margins.py @@ -0,0 +1,548 @@ +"""disk_margins.py + +Demonstrate disk-based stability margin calculations. + +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. + +[5] Deodhare, G., & Patel, V. (1998, August). A "Modern" Look at Gain + and Phase Margins: An H-Infinity/mu Approach. In Guidance, Navigation, + and Control Conference and Exhibit (p. 4134). +""" + +import os +import control +import matplotlib.pyplot as plt +import numpy as np + +def plot_allowable_region(alpha_max, skew, ax=None): + """Plot region of allowable gain/phase variation, given worst-case disk margin. + + Parameters + ---------- + alpha_max : float (scalar or list) + worst-case disk margin(s) across all frequencies. May be a scalar or list. + skew : float (scalar or list) + skew parameter(s) 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 + ax : axes to plot bounding curve(s) onto + + 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. + """ + + # Create axis if needed + if ax is None: + ax = plt.gca() + + # Allow scalar or vector arguments (to overlay plots) + if np.isscalar(alpha_max): + alpha_max = np.asarray([alpha_max]) + else: + alpha_max = np.asarray(alpha_max) + + if np.isscalar(skew): + skew=np.asarray([skew]) + else: + skew=np.asarray(skew) + + # Add a plot for each (alpha, skew) pair present + theta = np.linspace(0, np.pi, 500) + legend_list = [] + for ii in range(0, skew.shape[0]): + legend_str = "$\\sigma$ = %.1f, $\\alpha_{max}$ = %.2f" %(\ + skew[ii], alpha_max[ii]) + legend_list.append(legend_str) + + # Complex bounding curve of stable gain/phase variations + f = (2 + alpha_max[ii] * (1 - skew[ii]) * np.exp(1j * theta))\ + /(2 - alpha_max[ii] * (1 + skew[ii]) * np.exp(1j * theta)) + + # Allowable combined gain/phase variations + gamma_dB = control.ctrlutil.mag2db(np.abs(f)) # gain margin (dB) + phi_deg = np.rad2deg(np.angle(f)) # phase margin (deg) + + # Plot the allowable combined gain/phase variations + out = ax.plot(gamma_dB, phi_deg, alpha=0.25, label='_nolegend_') + ax.fill_between(ax.lines[ii].get_xydata()[:,0],\ + ax.lines[ii].get_xydata()[:,1], alpha=0.25) + + plt.ylabel('Phase Variation (deg)') + plt.xlabel('Gain Variation (dB)') + plt.title('Range of Gain and Phase Variations') + plt.legend(legend_list) + plt.grid() + plt.tight_layout() + + return out + +def test_siso1(): + # + # Disk-based stability margins for example + # SISO loop transfer function(s) + # + + # Frequencies of interest + omega = np.logspace(-1, 2, 1001) + + # Loop transfer gain + L = control.tf(25, [1, 10, 10, 10]) + + print("------------- Python control built-in (S) -------------") + GM_, PM_, SM_ = control.stability_margins(L)[:3] # python-control default (S-based...?) + print(f"SM_ = {SM_}") + print(f"GM_ = {GM_} dB") + print(f"PM_ = {PM_} deg\n") + + print("------------- Sensitivity function (S) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=1.0, returnall=True) # S-based (S) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(1) + plt.subplot(3, 3, 1) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('S-Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(1) + plt.subplot(3, 3, 4) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(1) + plt.subplot(3, 3, 7) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Complementary sensitivity function (T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=-1.0, returnall=True) # T-based (T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(1) + plt.subplot(3, 3, 2) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('T_Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(1) + plt.subplot(3, 3, 5) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(1) + plt.subplot(3, 3, 8) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Balanced sensitivity function (S - T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=0.0, returnall=True) # balanced (S - T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(1) + plt.subplot(3, 3, 3) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('Balanced Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(1) + plt.subplot(3, 3, 6) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(1) + plt.subplot(3, 3, 9) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + # Disk margin plot of admissible gain/phase variations for which + DM_plot = [] + DM_plot.append(control.disk_margins(L, omega, skew=-2.0)[0]) + DM_plot.append(control.disk_margins(L, omega, skew=0.0)[0]) + DM_plot.append(control.disk_margins(L, omega, skew=2.0)[0]) + plt.figure(10); plt.clf() + plot_allowable_region(DM_plot, skew=[-2.0, 0.0, 2.0]) + + return + +def test_siso2(): + # + # Disk-based stability margins for example + # SISO loop transfer function(s) + # + + # Frequencies of interest + omega = np.logspace(-1, 2, 1001) + + # Laplace variable + s = control.tf('s') + + # Loop transfer gain + L = (6.25 * (s + 3) * (s + 5)) / (s * (s + 1)**2 * (s**2 + 0.18 * s + 100)) + + print("------------- Python control built-in (S) -------------") + GM_, PM_, SM_ = control.stability_margins(L)[:3] # python-control default (S-based...?) + print(f"SM_ = {SM_}") + print(f"GM_ = {GM_} dB") + print(f"PM_ = {PM_} deg\n") + + print("------------- Sensitivity function (S) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=1.0, returnall=True) # S-based (S) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(2) + plt.subplot(3, 3, 1) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('S-Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(2) + plt.subplot(3, 3, 4) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(2) + plt.subplot(3, 3, 7) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Complementary sensitivity function (T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=-1.0, returnall=True) # T-based (T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(2) + plt.subplot(3, 3, 2) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('T-Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(2) + plt.subplot(3, 3, 5) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(2) + plt.subplot(3, 3, 8) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Balanced sensitivity function (S - T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=0.0, returnall=True) # balanced (S - T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(2) + plt.subplot(3, 3, 3) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('Balanced Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(2) + plt.subplot(3, 3, 6) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(2) + plt.subplot(3, 3, 9) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + # Disk margin plot of admissible gain/phase variations for which + # the feedback loop still remains stable, for each skew parameter + DM_plot = [] + DM_plot.append(control.disk_margins(L, omega, skew=-1.0)[0]) # T-based (T) + DM_plot.append(control.disk_margins(L, omega, skew=0.0)[0]) # balanced (S - T) + DM_plot.append(control.disk_margins(L, omega, skew=1.0)[0]) # S-based (S) + plt.figure(20) + plot_allowable_region(DM_plot, skew=[-1.0, 0.0, 1.0]) + + return + +def test_mimo(): + # + # Disk-based stability margins for example + # MIMO loop transfer function(s) + # + + # Frequencies of interest + omega = np.logspace(-1, 3, 1001) + + # Loop transfer gain + P = control.ss([[0, 10],[-10, 0]], np.eye(2), [[1, 10], [-10, 1]], 0) # plant + K = control.ss([], [], [], [[1, -2], [0, 1]]) # controller + L = P * K # loop gain + + print("------------- Sensitivity function (S) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=1.0, returnall=True) # S-based (S) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(3) + plt.subplot(3, 3, 1) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('S-Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(3) + plt.subplot(3, 3, 4) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(3) + plt.subplot(3, 3, 7) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Complementary sensitivity function (T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=-1.0, returnall=True) # T-based (T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(3) + plt.subplot(3, 3, 2) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('T-Based Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(3) + plt.subplot(3, 3, 5) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(3) + plt.subplot(3, 3, 8) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + print("------------- Balanced sensitivity function (S - T) -------------") + DM, GM, PM = control.disk_margins(L, omega, skew=0.0, returnall=True) # balanced (S - T) + print(f"min(DM) = {min(DM)} (omega = {omega[np.argmin(DM)]})") + print(f"GM = {GM[np.argmin(DM)]} dB") + print(f"PM = {PM[np.argmin(DM)]} deg") + print(f"min(GM) = {min(GM)} dB") + print(f"min(PM) = {min(PM)} deg\n") + + plt.figure(3) + plt.subplot(3, 3, 3) + plt.semilogx(omega, DM, label='$\\alpha$') + plt.ylabel('Disk Margin (abs)') + plt.legend() + plt.title('Balanced Margins') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 2]) + + plt.figure(3) + plt.subplot(3, 3, 6) + plt.semilogx(omega, GM, label='$\\gamma_{m}$') + plt.ylabel('Gain Margin (dB)') + plt.legend() + #plt.title('Gain-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 40]) + + plt.figure(3) + plt.subplot(3, 3, 9) + plt.semilogx(omega, PM, label='$\\phi_{m}$') + plt.ylabel('Phase Margin (deg)') + plt.legend() + #plt.title('Phase-Only Margin') + plt.grid() + plt.xlim([omega[0], omega[-1]]) + plt.ylim([0, 90]) + plt.xlabel('Frequency (rad/s)') + + # Disk margin plot of admissible gain/phase variations for which + # the feedback loop still remains stable, for each skew parameter + DM_plot = [] + DM_plot.append(control.disk_margins(L, omega, skew=-1.0)[0]) # T-based (T) + DM_plot.append(control.disk_margins(L, omega, skew=0.0)[0]) # balanced (S - T) + DM_plot.append(control.disk_margins(L, omega, skew=1.0)[0]) # S-based (S) + plt.figure(30) + plot_allowable_region(DM_plot, skew=[-1.0, 0.0, 1.0]) + + return + +if __name__ == '__main__': + #test_siso1() + #test_siso2() + test_mimo() + if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + #plt.tight_layout() + plt.show()