gain-margin.py

from control.matlab import tf, bode, rlocus, nyquist
import matplotlib.pyplot as plt
import numpy as np
from scipy.interpolate import interp1d

def indices_before_sign_flip(x: np.array):
    return np.where(np.diff(np.sign(x)))[0]

def find_180deg_crossing_freq(phase, freq):
    target_phase = -1*np.pi # for some reason +pi doesn't work
    phase = np.unwrap(phase) # normalize -pi/2 < x < pi/2
    crossings = indices_before_sign_flip(phase - target_phase)
    assert len(crossings)==1, "frequency response phase should cross 180deg exactly once"
    before_crossing_index = crossings[0]
    # assuming linear before/after the crossing, estimate magnitude at the crossing point
    phase2freq_interp_func = interp1d(phase[before_crossing_index:before_crossing_index+2],
                        freq[before_crossing_index:before_crossing_index+2],
                        kind='linear')
    return phase2freq_interp_func(target_phase)

def sys2gain_margin(sys):
    mag, phase, freq = bode(sys, plot=False)
    crossing_freq = find_180deg_crossing_freq(phase, freq)
    crossing_mag, crossing_phase, _ = bode(sys, crossing_freq, plot=False)
    print(f"freq={crossing_freq}, mag={crossing_mag[0]}, phase={crossing_phase[0]}")
    return 1/crossing_mag[0]

sys = tf([1], [1, 14, 44, 40])  
print(f"gain margin: {sys2gain_margin(sys)}")