"""
This File has the sole purpose of creating the ASC DHARD Y Model that is then exported to the buzz repository.
"""
import numpy as np
import matplotlib.pyplot as plt
import control
import scipy
from wield.bunch import Bunch
from wield.utilities import file_io
from wield.utilities.mpl import mplfigB
from buzz import ssutil, iodutil
import scipy.optimize
from wield.control import SISO
from wield.control.SISO import bode as SISObode
import pytest
from wield.pytest import ( # noqa
tjoin,
fjoin,
dprint,
)
[docs]
def nullSS(namespace='N', gain=1):
"""Create a state space model for the null block. It has one input and one output.
Args:
namespace (str, optional): The namespace to use for the model i.e. the letter that comes at the beginning of the input and output names. Defaults to 'N'.
gain (float, optional): The gain of the null block. Defaults to 1.
Returns:
MIMO Statespace: Returns Wield MIMO statespace
"""
A = np.zeros([0,0]) # Construct the A state matrix
B = np.zeros([1,0]) # Construct the B state matrix
C = np.zeros([0,1]) # Construct the C state matrix
D = np.array([[gain]]) # Construct the D state matrix
iod = dict({namespace+'.in': 0, namespace+'.out': 0}) # Input output dictionary
return ssutil.wieldSS(A, B, C, D, iod=iod)
[docs]
def addSS(namespace='T', dt=1e-4, sub=False, outSufx = None):
"""Create a state space model for an adder. It has two inputs and one output.
Args:
namespace (string): The namespace to use for the model i.e. the letter that comes at the beginning of the input and output names. Defaults to 'T'.
dt (float, optional): The time step. Defaults to 1e-4.
sub (bool, optional): If true, the model will be a subtractor. Defaults to False.
Returns:
wield.bunch: Returns the local name space for the function including the state space model as `mod`, input output dictionary as `iod`, and namespace.
"""
A = np.zeros([0,0]) # Construct the A state matrix
B = np.zeros([2,0]) # Construct the B state matrix
C = np.zeros([0,1]) # Construct the C state matrix
if sub:
D = np.array([[1, -1]]) # Construct the D state matrix for a subtractor
else:
D = np.array([[1, 1]]) # Construct the D state matrix
if outSufx == None:
iod = {namespace+'.in.1': 0, namespace+'.in.2': 1, namespace+'.out': 0} # Input output dictionary
else:
iod = {namespace+'.in.1': 0, namespace+'.in.2': 1, namespace+'.out.'+outSufx: 0} # Input output dictionary
return ssutil.wieldSS(A, B, C, D, iod=iod)
[docs]
def delaySS(namespace='D', delay=1e-3, order=1, numins=1, dt=1e-4):
"""Create a state space model for the delay block. It has one input and one output.
Args:
namespace (str, optional): The namespace for the delay block. Defaults to 'D'.
delay (_type_, optional): The delay in seconds. Defaults to 1e-3.
order (int, optional): Order of the filter. Defaults to 1.
numins (int, optional): Number of inputs. Defaults to 1.
dt (_type_, optional): The discretization time step. Defaults to 1e-4.
Returns:
wield.bunch: Returns the local name space for the function including the state space model as `mod`, input output dictionary as `iod`, and namespace.
"""
if delay==0:
print('Delay is zero, using nullSS')
D_mod = nullSS().mod
else:
# take the poles of this normalized bessel filter (delay=1s)
z, p, k = scipy.signal.besselap(order, norm="delay")
# now rescale for desired delay
roots = p / delay * 2
if order % 2 == 0:
k = 1
else:
k = -1
D_mod = SISO.zpk(-roots.conjugate(), roots, k).ss
# D_res = control.zpk(-roots.conjugate(), roots, k)
# D_mod = control.tf2ss(D_res)
B_blocked = D_mod.B
D_blocked = D_mod.D
for ii in range(numins-1):
B_blocked = np.block([[B_blocked, D_mod.B]])
D_blocked = np.block([[D_blocked, D_mod.D]])
mod = control.ss(D_mod.A, B_blocked, D_mod.C, D_blocked) # Create the system
mod_dis = control.c2d(mod, dt) # Discretize the system
iod = dict({namespace+'.in': 0, namespace+'.out': 0}) # Input output dictionary
for ii in range(numins-1):
iod[namespace+'.in.'+str(ii+1)] = ii+1
return Bunch(locals())
[docs]
def delayDrive(namespace='D', Sp=None, delay=1e-3, order=1, numins=1, dt=1e-4):
D = delaySS(namespace=namespace, delay=delay, order=order, numins=numins, dt=dt)
if Sp is None or Sp == 1:
#assert(False)
return D
#rename the input for pending merge
D.iod[namespace + '.in.2'] = D.iod[namespace + '.in']
del D.iod[namespace + '.in']
#check that it only has 1 input and one output (THIS IS A HACKY WAY TO TEST)
assert(len(Sp.iod) == 2)
print(Sp.iod)
Sp.iod.clear()
Sp.iod[namespace + '.in'] = 0
Sp.iod[namespace + '.out.1'] = 0
print(Sp.iod)
Dx = ssutil.multiconnect([Sp, D], [[namespace+'.in.2', namespace+'.out.1']])
print(Dx.iod)
return Dx
[docs]
def FFlatSS(gain=1, return_name=False):
if return_name:
name = "FOM Flat"
return name
return SISO.zpk([], [], gain).asSS.mimo("F2.out", "F2.in")
[docs]
def FBNSsimpSS(return_name=False, lo_ord=4, return_scale=False, as_zpk = False):
if return_name:
name = "FOM BNS Simple"
return name
BNS_zpk = file_io.load(fjoin('FOMs/FBNSsimp_FOM.yml'))
print(BNS_zpk['z'])
F_z = np.array(BNS_zpk["z"], dtype=np.complex128)
F_p = np.array(BNS_zpk["p"], dtype=np.complex128)
F_k = BNS_zpk["k"]
F_zpk = SISO.zpk(F_z, F_p, F_k)
F = F_zpk.asSS.mimo("FBNS.out", "FBNS.in")
F, scale = ssutil.normalize_gain(F, norm=1, return_scale=True)
if as_zpk:
return F_zpk * scale
if return_scale:
return scale
else:
return F
[docs]
def FBNSSS_Hand(return_name=False, return_scale = False, as_zpk=False):
"""
"""
if return_name:
name = "FOM BNS (Hand Fit)"
return name
BNS_zpk = file_io.load(fjoin('ExampleModels_newFOM/BNS_FOM_handfit.yml'))
F_z = np.array(BNS_zpk["z"], dtype=np.complex128)
F_p = np.array(BNS_zpk["p"], dtype=np.complex128)
F_k = BNS_zpk["k"]
F_zpk = SISO.zpk(F_z, F_p, F_k)
F = F_zpk.asSS.mimo("FBNS.out", "FBNS.in")
F, scale = ssutil.normalize_gain(F, norm=1, return_scale=True)
if as_zpk:
return F_zpk * scale
if return_scale:
return scale
else:
return F
[docs]
def FBNSSS_DARM(
return_name=False,
return_scale=False,
as_zpk=False,
normalize=True,
):
"""
"""
if return_name:
name = "FOM BNS (Hand Fit)"
return name
#BNS_zpk = file_io.load(fjoin('ExampleModels_DARMFOM/BNS_FOM_from_DARM.yml'))
BNS_zpk = file_io.load(fjoin('ExampleModels_DARMFOM/BNS_FOM_IR.yml'))
F_z = np.array(BNS_zpk["z"], dtype=np.complex128)
F_p = np.array(BNS_zpk["p"], dtype=np.complex128)
F_k = BNS_zpk["k"]
F_zpk = SISO.zpk(F_z, F_p, F_k)
F = F_zpk.asSS.mimo("FBNS.out", "FBNS.in")
if not normalize:
return F
F, scale = ssutil.normalize_gain(F, norm=1, return_scale=True)
if as_zpk:
return F_zpk * scale
if return_scale:
return scale
else:
return F
[docs]
def FBNSSS_Fit(gain=1, return_name=False, return_scale = False):
"""
"""
if return_name:
name = "FOM BNS"
return name
if gain != 1:
NotImplementedError("This function does not support gain scaling")
BNS_zpk = file_io.load(fjoin('ExampleModels_newFOM/BNS_FOM.yml'))
print(BNS_zpk['z'])
F_z = np.array(BNS_zpk["z"], dtype=np.complex128)
#BNS_zpk["p"].append(BNS_zpk["p"][-1])
F_p = np.array(BNS_zpk["p"], dtype=np.complex128)
F_k = BNS_zpk["k"]
#print(len(F_z))
#print(len(F_p))
F_mod = SISO.zpk(F_z, F_p, F_k).asSS
F_iod = {"FBNS.in": 0, "FBNS.out": 0}
F = ssutil.wieldSS(F_mod.A, F_mod.B, F_mod.C, F_mod.D, iod=F_iod)
F, scale = ssutil.normalize_gain(F, norm=1, return_scale=True)
if return_scale:
return scale
else:
return F
[docs]
@pytest.mark.parametrize(
'folder', [
'ExampleModels_working',
'ExampleModels_working_F3',
'ExampleModels_newFOM',
'ExampleModels_newFOM_F3',
'ExampleModels_DARMFOM',
'ExampleModels_DARMFOM_F3',
]
)
@pytest.mark.gitlabCI
@pytest.mark.ADY
def test_Make_ADY(folder):
"""
This is the main function that is called to create the model for the ASC DHARD Y.
"""
use_old_balancing = False
# not helpful it seems
use_alt_balancing = False
if use_old_balancing:
from wield.control.utilities import algorithm_choice
# This fix is needed since updating several wield.controls algorithms
algorithm_choice.algorithm_adjust_default('zpk2ss', 'zpk2ss_chain_poly_nobal', 200)
S = ssutil.loadSys(fjoin(folder, 'ADY_E_from_fit.mat'))
S = S.balance(which='ABC')
if use_old_balancing:
S = ssutil.balance_sys_gain(S, verbose=True)
if use_alt_balancing:
S = ssutil.balance_sys_gain(S, method='bfsqrt', verbose=True)
S = S.schur_form()
O = ssutil.loadSys(fjoin(folder, 'ADY_M_from_fit.mat'))
# O = nullSS(namespace='O', gain=5e-5)
print('O.A: ', O.A)
O = O.balance(which='ABC')
if use_old_balancing:
O = ssutil.balance_sys_gain(O, verbose=True)
if use_alt_balancing:
O = ssutil.balance_sys_gain(O, method='bfsqrt', verbose=True)
O = O.schur_form()
# TODO - make solver handle unobserved modes
# can be tested by adding one here
# the plant of the working folder has a nontrivial statespace for some reason even though
# it has a flat response with no poles or zeros
# this ensures an un-observable mode that the solver DOES NOT LIKE
O = O.siso('O.out', 'O.in').asZPK.asSS.mimo('O.out', 'O.in')
P_fname = 'ADY_plant_from_fit.mat'
P = ssutil.loadSys(fjoin(folder, P_fname))
P = P.balance(which='ABC')
#if use_old_balancing:
# P = ssutil.balance_sys_gain(P, verbose=True)
if use_alt_balancing:
P = ssutil.balance_sys_gain(P, method='bfsqrt', verbose=True)
P = P.schur_form()
# print("plant shape:")
# P.print_nonzero()
# this rescale is still helping the solver (it shouldn't!)
w2_rescale = 1e6
if use_old_balancing:
w2_rescale = 1e6
S = (w2_rescale * S.siso('S.out', 'S.in')).mimo('S.out', 'S.in')
O = (w2_rescale * O.siso('O.out', 'O.in')).mimo('O.out', 'O.in')
if '_newFOM' in folder or '_newFOM_F3' in folder:
FBNS_func = FBNSSS_Hand
FBNS = FBNS_func()
FBNS_zpk = FBNS_func(as_zpk=True)
FBNS_scale = FBNS_func(return_scale=True)
elif 'DARMFOM' in folder:
FBNS_func = FBNSSS_DARM
FBNS = FBNS_func()
FBNS_zpk = FBNS_func(as_zpk=True)
FBNS_scale = FBNS_func(return_scale=True)
else:
FBNS_func = FBNSsimpSS
FBNS = FBNS_func()
FBNS_zpk = FBNS_func(as_zpk=True)
FBNS_scale = FBNS_func(return_scale=True)
omega_limits = [1e-2, 1e4]
axB = ssutil.bode(FBNS.siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits, label='Simple FBNS')
axB = ssutil.bode(FBNSSS_Hand().siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits, axB=axB, label='Hand Fit FBNS')
axB = ssutil.bode(FBNSSS_Fit().siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits, axB=axB, label='FBNS From Data')
axB.save(tjoin('BNS_FOMS_Comp.pdf'))
axB.save(tjoin('BNS_FOMS_Comp.png'))
print("OBS STATESPACE-----------------------------------------------------------")
print(O.print_nonzero())
print("ZP: ", O.siso('O.out', 'O.in').asZPK.zeros, O.siso('O.out', 'O.in').asZPK.poles)
print("/OBS STATESPACE-----------------------------------------------------------")
print("SEI STATESPACE-----------------------------------------------------------")
print(S.print_nonzero())
print("/SEI STATESPACE-----------------------------------------------------------")
print("FOM STATESPACE-----------------------------------------------------------")
if use_old_balancing:
FBNS = ssutil.balance_sys_gain(FBNS, verbose=True)
print(FBNS.print_nonzero())
print("/FOM STATESPACE-----------------------------------------------------------")
FFlat = FFlatSS()
if use_old_balancing:
FFlat = ssutil.balance_sys_gain(FFlat, verbose=True)
# Inject the coupling into the BNS FOM, normalize it to Hinf=1 and then collect its scale
Coup = ssutil.loadSys(fjoin(folder, 'A2L_coupling_with_cal.mat')) # This is the Angle to length coupling it would be multiplied by the BNS FOM
if use_old_balancing:
Coup = ssutil.balance_sys_gain(Coup, verbose=True)
if use_alt_balancing:
Coup = ssutil.balance_sys_gain(Coup, method='bfsqrt', verbose=True)
Coup = Coup.schur_form()
# Coup = SISO.zpk([], [], 1e-14).asSS.mimo('COUP.out', 'COUP.in')
FBNS_orig = FBNS
FBNS = ssutil.multiconnect([Coup, FBNS], [['FBNS.in', 'COUP.out']])
#FBNS = ssutil.truncate_io(FBNS, ['COUP.in'], ['FBNS.out', 'COUP.out']) #commented out to display DARM Spectrum
FBNS = ssutil.rename_io(FBNS, 'COUP.out', 'FBNS.DARM.out') # to see DARM Spectrum
FBNS = ssutil.rename_io(FBNS, 'FBNS.in', 'FBNS.noC.in') # add a bypass coup for the FBNS
FBNS = ssutil.rename_io(FBNS, 'COUP.in', 'FBNS.in')
FBNS_Trash, Coup_scale = ssutil.normalize_gain(ssutil.truncate_io(FBNS, ['FBNS.in'], ['FBNS.out']), norm=1, return_scale=True, method='fq_resp')
del FBNS_Trash
FBNS = ssutil.scale_io(FBNS, 'FBNS.out', Coup_scale) # using this to scale preserves the DARM.out output
# TODO, comment this for breakage test and uncomment for typical operation
#FBNS = ssutil.scale_io(FBNS, 'FBNS.noC.in', 1/Coup_scale) # using this to scale preserves the DARM.out output
FBNS = ssutil.scale_io(FBNS, 'FBNS.noC.in', 1/Coup_scale * 1/FBNS_scale) # using this to scale preserves the DARM.out output
omega_limits_noise = [1e-1, 1e3]
axN = ssutil.bode(ssutil.asSISO(ssutil.truncate_io(FBNS, ['FBNS.noC.in'], ['FBNS.out'])), omega_limits=omega_limits_noise, label='FBNS Scaled')
axN = ssutil.bode(ssutil.asSISO(FBNS_func(normalize=False)), axB=axN, omega_limits=omega_limits_noise, linestyle='--',label='Unscaled BNS')
axN.save(tjoin('FBNS_bode_after_scl.png'))
axN.save(tjoin('FBNS_bode_after_scl.pdf'))
# FBNS_alt = Coup.siso('COUP.out', 'COUP.in') * FBNS.siso('FBNS.out', 'FBNS.in')
print("FOM STATESPACE-with Coupling---------------------------------------------")
print(FBNS.inputs)
print(FBNS.print_nonzero())
print("/FOM STATESPACE-with coupling---------------------------------------------")
FBNS_scale = FBNS_scale * Coup_scale
np.savetxt(fjoin(folder, 'FBNS_scale.txt'), [FBNS_scale])
print('FBNS scale', FBNS_scale)
current_range = np.loadtxt(fjoin(folder, 'FBNS_Current_range.txt')) # Saved by the normalization in the make function
io_scales = Bunch(
FBNS = FBNS_scale, # will need to apply this to the BNS FOM
w2_rescale = w2_rescale, # will need to divide by this on both FOMs
# will need to be applied only to the Flat fom since the A2L_coupling_with_cal includes it as a factor
ct2rad = 5.2e-11, # Calibration from https://alog.ligo-wa.caltech.edu/aLOG/index.php?callRep=69551
strain2m = 1/3995, # Representing the 4km arm length. Strain is h = dL/L where L = 3995 m. The coupling is "I" is
# in rad to displacement so we need to convert it to strain for the BNS FOM which is in strain
current_range = current_range,
coup_scale = Coup_scale,
)
io_scales.RMS_cal_F1 = 1/io_scales.FBNS * io_scales.strain2m / io_scales.w2_rescale
io_scales.RMS_cal_F2 = io_scales.ct2rad / io_scales.w2_rescale
file_io.save(fjoin(folder, 'io_scales.yml'), dict(io_scales))
omega_limits = [1e-2, 1e4]
omega_limits_noise = [1e-1, 1e3]
axN = ssutil.bode(ssutil.asSISO(S), omega_limits=omega_limits_noise, label='Seismic')
axN = ssutil.bode(ssutil.asSISO(O), axB=axN, omega_limits=omega_limits_noise, label='Meas. Noise')
axN = ssutil.bode(ssutil.asSISO(P), axB=axN, omega_limits=omega_limits_noise, label='Plant')
axN = ssutil.bode(ssutil.asSISO(S.siso('S.out', 'S.in') * P.siso('P.out', 'P.in')), axB=axN, omega_limits=omega_limits_noise, label='S*P')
axN.save(tjoin('Noise_bode.png'))
axN.save(tjoin('Noise_bode.pdf'))
# control.bode(S.mod, dB=False, Hz=True, omega_limits=omega_limits, label='Seismic')
# control.bode(O.mod, dB=False, Hz=True, omega_limits=omega_limits, label='Meas. Noise')
# control.bode(P.mod, dB=False, Hz=True, omega_limits=omega_limits, label='Plant')
# control.bode((S.siso('S.out', 'S.in') * P.siso('P.out', 'P.in')).mimo('out', 'in').mod, dB=False, Hz=True, omega_limits=omega_limits, label='S*P')
# plt.legend()
# plt.savefig(tjoin('Noise_bode.pdf'), bbox_inches='tight')
# plt.savefig(tjoin('Noise_bode.png'), bbox_inches='tight', dpi=300)
# plt.close()
omega_limits_fom = np.array([1e-1, 1e5])*2*np.pi
axFOM = ssutil.bode(FBNS.siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits_fom, label='FBNS')
axFOM = ssutil.bode(FFlat.siso(iodutil.listoutputs(FFlat)[0], iodutil.listinputs(FFlat)[0]), axB=axFOM, omega_limits=omega_limits_fom, label='FFlat')
axFOM.save(tjoin('FOM_bode.pdf'))
axFOM.save(tjoin('FOM_bode.png'))
# control.bode(FBNS.mod, dB=True, Hz=True, omega_limits=omega_limits, label='FBNS')
# control.bode(FFlat.mod, dB=True, Hz=True, omega_limits=omega_limits, label='FFlat')
# plt.legend()
# plt.savefig(tjoin('FOM_bode.pdf'))
# plt.savefig(tjoin('FOM_bode.png'))
# plt.close()
#make the SO
#conlist =[['T_SO.in.1', 'O.out'], ['T_SO.in.2','S.out']]
#T_SO = addSS(namespace='T_SO', sub=True)
#SO = ssutil.multiconnect([S, O, T_SO], conlist)
#SO = ssutil.truncate_io(SO, ['O.in', 'S.in'], ['T_SO.out', 'S.out'])
#SO = ssutil.rename_io(SO, 'T_SO.out', 'O.out')
#control.bode(SO.mod[SO.iod['S.out'], SO.iod['S.in']], dB=True, Hz=True, omega_limits=omega_limits, label='S to S')
#control.bode(SO.mod[SO.iod['O.out'], SO.iod['O.in']], dB=True, Hz=True, omega_limits=omega_limits, label='O to O')
#control.bode(SO.mod[SO.iod['O.out'], SO.iod['S.in']], dB=True, Hz=True, omega_limits=omega_limits, label='S to O', linestyle='--', linewidth=5)
#control.bode(S.mod, dB=True, Hz=True, omega_limits=[1e-2, 1e3], label='Orig. S', linestyle='dotted', linewidth=4)
#control.bode(O.mod, dB=True, Hz=True, omega_limits=[1e-2, 1e3], label='Orig. O', linestyle='dotted', linewidth=4)
#plt.legend(fontsize=8)
#plt.savefig(tjoin('SO_bode.pdf'))
#plt.close()
SPOFF_kw = {}
if '_F3' in folder:
extras = 'F3_withP'
SPOFF_kw['include_F1'] = False
FBNS_eff_in = 'F3.in'
FBNS_eff_out = 'F3.out'
else:
extras = None
FBNS_eff_in = 'FBNS.in'
FBNS_eff_out = 'FBNS.out'
# TODO, this will break the solve to use noC.in. but debugging breakage
SPOFF = ssutil.makeSPOFF(
S, P, O,
FBNS,
FFlat,
diagnostic=True,
extras=extras,
balance=use_old_balancing,
# F1in='FBNS.noC.in',
F1in='FBNS.in',
F1out='FBNS.out',
**SPOFF_kw
)
SPOFF = SPOFF.topological_sort()
SPOFF = SPOFF.balance()
# SPOFF = SPOFF.schur_form()
#print('SPOFF', SPOFF.iod)
#SPOFF = ssutil.balance_sys_gain(SPOFF)
print("SPOFF STATESPACE-----------------------------------------------------------")
try_block_diagonalize = False
if try_block_diagonalize:
SPOFF = SPOFF.schur_form()
print(SPOFF.print_nonzero())
sr = np.zeros(SPOFF.ss.Nstates, dtype=float)
SPOFF_b2 = SPOFF.balance(which='A', Dr=sr)
SPOFF_b2 = SPOFF
from wield.control.ss_bare.ssprint import print_dense_nonzero_A, print_dense_nonzero, nz_int
def flog2(arr):
#arr = np.asarray(arr)
#mant, exp = np.frexp(arr)
exp = np.round(np.log2(abs(arr)))
return exp
sc = np.block([[flog2(sr)]])
print("scaling?")
print(print_dense_nonzero_A(sc, scaling=nz_int))
Tr = np.zeros_like(SPOFF_b2.A)
SPOFF_b2 = SPOFF_b2.block_diagonalize(condition_number=1e6, Tright=Tr)
print("after balance2")
print(SPOFF_b2.print_nonzero())
print("Transfer Matrix")
print(print_dense_nonzero_A(Tr))
# SPOFF = SPOFF_b2
print("POLES")
print(SPOFF_b2.p)
print("/SPOFF STATESPACE-----------------------------------------------------------")
omega_limits=[1e-2, 1e3]
axB = mplfigB(Nrows=2)
F_Hz = np.geomspace(1e-2, 20, 1000)
with SISObode.multi_bode(axB=axB, F_Hz = F_Hz) as bode:
bode(P['P.out', 'P.in'], label='P alone', linewidth=2, color='dodgerblue')
bode(SPOFF['P.out', 'P.in'], label='P to P in SPOFF', color='orange')
bode(SPOFF['P.out', 'S.in'], label='S to P', color='green')
bode(SPOFF['S.out', 'S.in'], label='S to S', linestyle='dotted', linewidth=4, color='green')
bode(SPOFF['P.out', 'SA.in'], linestyle='--', label='P Recombined', linewidth=4, color='dodgerblue')
bode(SPOFF['S.out', 'SA.in'], label='P that moved to S', color='dodgerblue', linestyle='dotted', linewidth=6)
bode(SPOFF['G.out', 'S.in'], label='Orig. S (from SPOFF)', color='red')
bode(S['S.out', 'S.in'], label='Orig. S alone', linestyle='dotted', linewidth=4, color='red')
axB.ax1.legend(fontsize=5)
axB.save(tjoin('SPOFF_diagnostic_bode.pdf'))
axB.save(tjoin('SPOFF_diagnostic_bode.png'))
axB = mplfigB(Nrows=2)
F_Hz = np.geomspace(1e-2, 20, 1000)
with SISObode.multi_bode(axB=axB, F_Hz = F_Hz) as bode:
bode(sys=SPOFF['P.out', 'P.in'], label='P to P')
bode(sys=SPOFF['P.out', 'S.in'], label='S to P')
bode(sys=SPOFF['S.out', 'S.in'], label='S to S', linestyle=':', linewidth=2)
bode(sys=S['S.out', 'S.in'], label='S to S (orig)', linestyle=':', linewidth=2)
bode(sys=SPOFF['T.out', 'O.in'], label='O to Meas. Out')
bode(sys=SPOFF['T.out', 'S.in'], label='S to Meas. Out')
axB.ax1.legend(fontsize=5)
axB.save(tjoin('SPOFF_bode.pdf'))
axB.save(tjoin('SPOFF_bode.png'))
#print('SPOFF', SPOFF.iod)
#print(SPOFF['T.out', 'S.in']._zp[1])
#truncate_inputs = ['P.in', 'S.in', 'O.in', 'T.in.1', 'T.in.2', 'FBNS.in', 'F2.in', 'Zinf.in']
#truncate_outputs = ['P.out', 'O.out', 'S.out', 'T.out', 'FBNS.out', 'F2.out']
#SPOFF = ssutil.truncate_io(SPOFF, truncate_inputs, truncate_outputs)
control.bode(SPOFF.mod[SPOFF.iod['F2.out'], SPOFF.iod['P.in']], dB=True, Hz=True, omega_limits=omega_limits, label='P to Flat FOM')
control.bode(SPOFF.mod[SPOFF.iod[FBNS_eff_out], SPOFF.iod['P.in']], dB=True, Hz=True, omega_limits=omega_limits, label='P to BNS FOM')
control.bode(SPOFF.mod[SPOFF.iod['P.out'], SPOFF.iod['S.in']], dB=True, Hz=True, omega_limits=omega_limits, label='S to P')
control.bode(SPOFF.mod[SPOFF.iod['S.out'], SPOFF.iod['S.in']], dB=True, Hz=True, omega_limits=omega_limits, label='S to S', linestyle='dotted', linewidth=4)
control.bode(SPOFF.mod[SPOFF.iod['T.out'], SPOFF.iod['O.in']], dB=True, Hz=True, omega_limits=omega_limits, label='O to Meas. Out')
control.bode(SPOFF.mod[SPOFF.iod['T.out'], SPOFF.iod['S.in']], dB=True, Hz=True, omega_limits=omega_limits, label='S to Meas. Out')
plt.legend(fontsize=5)
plt.savefig(tjoin('SPOFF_all_bode.pdf'))
plt.savefig(tjoin('SPOFF_all_bode.png'))
plt.close()
# THIS MODIFIES THE ORIGINAL SYSTEM!!!!! WHAT - LEE
#SPOFF_bal = ssutil.balance_sys_gain(SPOFF)
axFOM_S = ssutil.bode(SPOFF.siso(FBNS_eff_out, FBNS_eff_in), omega_limits=omega_limits_fom, label='FBNS', color='red')
axFOM_S = ssutil.bode(SPOFF.siso(iodutil.listoutputs(FFlat)[0], iodutil.listinputs(FFlat)[0]), axB=axFOM_S, omega_limits=omega_limits_fom, label='FFlat', color='dodgerblue')
#axFOM_S = ssutil.bode(SPOFF_bal.siso(FBNS_eff_out, FBNS_eff_in), omega_limits=omega_limits_fom, axB=axFOM_S, label='FBNS SPOFF bal', color='green')
#axFOM_S = ssutil.bode(SPOFF_bal.siso(iodutil.listoutputs(FFlat)[0], iodutil.listinputs(FFlat)[0]), axB=axFOM_S, omega_limits=omega_limits_fom, label='FFlat SPOFF bal', color='green')
axFOM_S = ssutil.bode(FBNS.siso('FBNS.out', 'FBNS.in'), axB=axFOM_S, omega_limits=omega_limits_fom, label='FBNS orig.', linestyle='dotted', color='red', linewidth=4)
axFOM_S = ssutil.bode(FBNS_orig.siso('FBNS.out', 'FBNS.in'), axB=axFOM_S, omega_limits=omega_limits_fom, label='FBNS orig.s', linestyle='dotted', color='purple', linewidth=4)
axFOM_S = ssutil.bode(Coup.siso('COUP.out', 'COUP.in'), axB=axFOM_S, omega_limits=omega_limits_fom, label='FBNS orig.s', linestyle='-', color='magenta', linewidth=4)
axFOM_S = ssutil.bode(FBNS_zpk, axB=axFOM_S, omega_limits=omega_limits_fom, label='FBNS orig_zpk.', linestyle='dotted', color='orange', linewidth=4)
axFOM_S = ssutil.bode(FFlat.siso(iodutil.listoutputs(FFlat)[0], iodutil.listinputs(FFlat)[0]), axB=axFOM_S, omega_limits=omega_limits_fom, label='FFlat orig.', color='dodgerblue', linestyle='dotted', linewidth=4)
axFOM_S.save(tjoin('FOM_bode_from_SPOFF.pdf'))
axFOM_S.save(tjoin('FOM_bode_from_SPOFF.png'))
#SPOFF = ssutil.balance_sys_gain(SPOFF)
P_load2 = ssutil.loadSys(fjoin(folder, P_fname))
# assert(np.allclose(P_load2.A, P.A))
# assert(np.allclose(P_load2.B, P.B))
# assert(np.allclose(P_load2.C, P.C))
# assert(np.allclose(P_load2.D, P.D))
print("SYS?")
SPOFF.print_nonzero()
ssutil.savesys(SPOFF, tjoin('ADY_Sanex.mat'))
ssutil.savesys(P, tjoin('ADY_plant.mat'))
ssutil.savesys(SPOFF, fjoin(folder, 'ADY_Sanex.mat'))
ssutil.savesys(P, fjoin(folder, 'ADY_plant.mat'))
return