"""
This File has the sole purpose of creating the ASC DHARD Y Model that is then exported to the buzz repository.
"""
import os
import contextlib
import numpy as np
import matplotlib.pyplot as plt
import control
import pytest
import scipy
from scipy.signal import cheby2
from scipy import signal
from wield.bunch import Bunch
#from wield.control import MIMO
from wield.control import SISO
from wield.control.ss_bare.ss import BareStateSpace as RawStateSpace
from wield.control.ss_bare.ssprint import print_dense_nonzero
from wield.utilities.file_io import load, load_ls, save
from wield.utilities.mpl import mplfigB
from wield.pytest import (
tjoin,
fjoin,
dprint,
)
from icecream import ic
from buzz import ssutil, iodutil
import scipy.optimize
from wield.control import SISO
[docs]
def bode(
sys,
axB=None,
F_Hz=None,
omega_limits=None,
include_zp = True,
label=None,
**kwargs
):
"""
sys should be a wield.control.SISO object
"""
if axB is None:
axB = mplfigB(Nrows = 2)
if include_zp:
z, p = sys._zp
z = z[z.imag > 0]
p = p[p.imag > 0]
F_include = np.sort(np.concatenate(
[
z.imag,
p.imag,
z.imag - abs(z.real) - 1e-6,
z.imag + abs(z.real) + 1e-6,
p.imag - abs(p.real) - 1e-6,
p.imag + abs(p.real) + 1e-6,
]
)) / (2 * np.pi)
F_include = F_include[F_include > 0]
if omega_limits is None:
omega_limits = (
(F_include[0] + 1e-6) / 3,
F_include[-1] * 3
)
if F_Hz is None:
F_Hz = np.geomspace(omega_limits[0], omega_limits[1], 1000)
if include_zp:
print("F_include", F_include)
F_Hz = np.sort(np.concatenate([F_Hz, F_include]))
fr = sys.fresponse(f=F_Hz)
axB.ax0.loglog(*fr.fplot_mag, label=label, **kwargs)
if hasattr(axB, 'ax1'):
axB.ax1.semilogx(*fr.fplot_deg225, label=label, **kwargs)
return axB
[docs]
@contextlib.contextmanager
def multi_bode(
axB,
F_Hz = None,
include_zp = True,
**kwargs
):
"""
Plot multiple bode plots at once.
The main advantage is that the scale of the axes can be better auto-determined to capture all known poles and zeros.
The input arguments should all be dictionaries containing the bode kwargs.
the usage should be
the kwargs on the top call are passed into all sub calls
with multi_bode(axB=axB, **kw_cmn) as bode:
bode(sisoA, label='A', **kw)
bode(sisoB, label='B', **kw)
bode(sisoC, label='C', **kw)
axB.save('figname.pdf')
"""
bode_arg_kwarg_list = []
F_includes = []
def bode_sys(
sys,
**kwargs
):
"""
Just extract and return the bode system
"""
return sys
def bode_plot(*args, **kwargs):
sys = bode_sys(*args, **kwargs)
z, p = sys._zp
z = z[z.imag > 0]
p = p[p.imag > 0]
F_include = np.sort(np.concatenate(
[
z.imag,
p.imag,
z.imag - abs(z.real) - 1e-6,
z.imag + abs(z.real) + 1e-6,
p.imag - abs(p.real) - 1e-6,
p.imag + abs(p.real) + 1e-6,
]
)) / (2 * np.pi)
F_include = F_include[F_include > 0]
F_includes.append(F_include)
bode_arg_kwarg_list.append(
(args, kwargs)
)
yield bode_plot
F_include = np.sort(np.concatenate(F_includes))
if F_Hz is None:
F_Hz = np.geomspace(
(F_include[0] + 1e-6) / 3,
F_include[-1] * 3,
300
)
F_include = F_include[F_include < F_Hz[-1]]
F_include = F_include[F_include > F_Hz[0]]
if include_zp:
F_Hz = np.sort(np.concatenate([F_Hz, F_include]))
for arg, kwarg in bode_arg_kwarg_list:
kw = dict(**kwargs)
kw.update(kwarg)
bode(
*arg,
axB=axB,
F_Hz=F_Hz,
omega_limits=None,
include_zp = False,
**kw
)
return
[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
#siso_rep = SISO.zpk(-roots.conjugate(), roots, k)
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
F2_A = np.zeros([0,0]) # Construct the A state matrix
F2_B = np.zeros([1,0]) # Construct the B state matrix
F2_C = np.zeros([0,1]) # Construct the C state matrix
F2_D = np.array([[gain]]) # Construct the D state matrix
F2_mod = control.ss(F2_A, F2_B, F2_C, F2_D)
F2_iod = {"F2.in": 0, "F2.out": 0}
F2 = ssutil.makeSys(F2_mod, F2_iod)
return F2
[docs]
def FBNSsimpSS(gain=1, return_name=False, lo_ord=4, return_scale=False):
if return_name:
name = "FOM BNS Simple"
return name
if gain != 1:
NotImplementedError("This function does not support gain scaling")
F_Fq_lo = 40 * 2 * np.pi
# aggressive version
# F_Fq_lo = 30 * 2 * np.pi
F_Fq_hi = 300 * 2 * np.pi
hp_order = 1 #lo_ord
lp_order = 2
F_p = []
F_z = []
for ord in range(hp_order):
F_p.append(-F_Fq_lo + 0*1j*F_Fq_lo)
F_p.append(-F_Fq_lo - 0*1j*F_Fq_lo)
F_z.append(0)
F_z.append(0)
for ord in range(lp_order):
F_p.append(-F_Fq_hi)
F_k = 1
c_zpk = cheby2(8, 160, 4*2*np.pi, btype='high', analog=True, output='zpk')
# aggressive version
# c_zpk = cheby2(8, 120, 4*2*np.pi, btype='high', analog=True, output='zpk')
F_z.extend(c_zpk[0])
F_p.extend(c_zpk[1])
F_k *= c_zpk[2]
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]
def FBNSSS_Hand(gain=1, return_name=False, return_scale = False):
"""
"""
if return_name:
name = "FOM BNS (Hand Fit)"
return name
if gain != 1:
NotImplementedError("This function does not support gain scaling")
#BNS_zpk = load('fjoin(ExampleModels_newFOM/BNS_FOM_handfit_gentleslope.yml'))
BNS_zpk = load(fjoin('ExampleModels_newFOM/BNS_FOM_handfit.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]
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 = 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]
def transposeSys(sys, FOM_ns):
# add Z_inf as an input
sys_inf_in_badname = ssutil.duplicate_io(sys, 'T.in.1')
sys_inf_in = ssutil.rename_io(sys_inf_in_badname, 'T.in.1.2', 'Zinf.in')
#print(sys.iod)
inputs= ['D.in', 'O.in', 'S.in', 'Zinf.in']
outputs_no_inf = [fns + '.out' for fns in FOM_ns] + ['T.out']
sys_red = ssutil.truncate_io(sys_inf_in, inputs, outputs_no_inf)
# Transpose the system
sys_transpose = ssutil.transpose(sys_red)
# Reorder the inputs and outputs
sys_transpose = ssutil.reorder_io(sys_transpose,
[fns + '.in' for fns in FOM_ns] + ['T.in'], [
'Zinf.out',
'S.out',
'O.out',
'D.out'
])
return sys_transpose
[docs]
def ws_tf2ss(z, p, k, gain=1, mfactor=1, angular=True):
"""
Takes a python control zpk and returns a python control statespace.
Uses more numerically stable wield computations to convert between.
"""
ws_zpk = SISO.zpk(z, p, k * gain, angular=True, fiducial_rtol=1e-5, fiducial_atol=1e-10)
ws_SS = ws_zpk.asSS * mfactor
# print("ZPK: ", ws_SS.asZPK.z, ws_SS.asZPK.p)
# return control.ss(ws_SS.A.T, ws_SS.C.T, ws_SS.B.T, ws_SS.D.T)
return control.ss(ws_SS.A, ws_SS.B, ws_SS.C, ws_SS.D)
[docs]
@pytest.mark.parametrize('folder', ['ExampleModels_rescale', 'ExampleModels', 'ExampleModels_working', 'ExampleModels_newFOM', 'ExampleModels_newFOM_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.
"""
S = ssutil.loadSys(fjoin(folder, 'ADY_E_from_fit.mat'))
O = ssutil.loadSys(fjoin(folder, 'ADY_M_from_fit.mat'))
P_fname = 'ADY_plant_from_fit.mat'
P = ssutil.loadSys(fjoin(folder, P_fname))
# this logic to add scaling helps the solver
if '_rescale' in folder:
S = (1e5 * S.siso('S.out', 'S.in')).mimo('S.out', 'S.in')
O = (1e5 * 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_scale = FBNS_func(return_scale=True)
else:
FBNS_func = FBNSsimpSS
FBNS = FBNS_func()
#print("\n\n\nUsing Simplified FBNS Model\n\n\n")
FBNS_orig = FBNS
FBNS = (FBNS.siso('FBNS.out', 'FBNS.in') * FBNS.siso('FBNS.out', 'FBNS.in')).mimo('FBNS.out', 'FBNS.in')
FBNS_scale = FBNS_func(return_scale=True)**2 # square the gain because the fom is squared
omega_limits=[1e-2, 1e4]
axB = ssutil.bode(FBNS.siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits, label='Squared Simple FBNS')
axB = ssutil.bode(FBNS_orig.siso('FBNS.out', 'FBNS.in'), omega_limits=omega_limits, axB=axB, 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'))
FFlat = FFlatSS()
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
FBNS = ssutil.multiconnect([Coup, FBNS], [['FBNS.in', 'COUP.out']])
# FBNS = ssutil.truncate_io(FBNS, ['COUP.in'], ['FBNS.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, 'COUP.in', 'FBNS.in')
FBNS_Trash, Coup_scale = ssutil.normalize_gain(ssutil.truncate_io(FBNS, ['FBNS.in'], ['FBNS.out']), norm=1, return_scale=True)
FBNS = ssutil.scale_io(FBNS, 'FBNS.out', Coup_scale) # using this to scale preserves the DARM.out output
FBNS_scale = FBNS_scale * Coup_scale
np.savetxt(fjoin(folder, 'FBNS_scale.txt'), [FBNS_scale])
print('FBNS scale', FBNS_scale)
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()
if '_F3' in folder:
extras = 'F3_withP'
else:
extras = None
SPOFF = ssutil.makeSPOFF(S, P, O, FBNS, FFlat, diagnostic=True, extras=extras, balance=True)
#print('SPOFF', SPOFF.iod)
#SPOFF = ssutil.balance_sys_gain(SPOFF)
omega_limits=[1e-2, 1e3]
axB = mplfigB(Nrows=2)
F_Hz = np.geomspace(1e-2, 20, 1000)
with 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 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.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()
SPOFF_bal = ssutil.balance_sys_gain(SPOFF)
axFOM_S = ssutil.bode(SPOFF.siso('FBNS.out', 'FBNS.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.out', 'FBNS.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(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))
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