GeezerVison

import os, sys, math
import numpy as np
import control

import math
import matplotlib
import matplotlib.pyplot as plt
from warnings import warn

import numpy as np
import scipy as sp

# Frequencies of interest
omega = np.logspace(-1, 3, 1001)

# Laplace variable
s = control.tf('s')

# MIMO loop transfer gain for the spinning satellite example
# https://www.mathworks.com/help/robust/ug/mimo-stability-margins-for-spinning-satellite.html
P = control.ss([[0, 10],[-10, 0]], np.eye(2), [[1, 10], [-10, 1]], [[0, 0],[0, 0]]) # plant
C = control.ss([],[],[], [[1, -2], [0, 1]]) # controller
L = P*C # output loop gain

print(f"------------- 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.legend()
plt.title('Disk Margin')
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])

print(f"------------- 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.legend()
plt.title('Disk Margin')
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])

print(f"------------- 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.legend()
plt.title('Disk Margin')
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])

------------- Sensitivity function (S) -------------
min(DM) = 0.3697398698410271 (omega = 0.1)
GM = 2.732761944304522 dB
PM = 21.30709883385843 deg
min(GM) = 2.732761944304522 dB
min(PM) = 21.30709883385843 deg

------------- Complementary sensitivity function (T) -------------
min(DM) = 0.3683035923739884 (omega = 0.1)
GM = 2.7236493433653735 dB
PM = 21.223368586091564 deg
min(GM) = 2.7236493433653735 dB
min(PM) = 21.223368586091564 deg

------------- Balanced sensitivity function (S - T) -------------
min(DM) = 0.3769872636944355 (omega = 0.1)
GM = 3.3140985364257256 dB
PM = 21.349285542574695 deg
min(GM) = 3.3140985364257256 dB
min(PM) = 21.349285542574695 deg

.
.
.
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])