plt.grid(True)
if False:
plt.figure()
plt.plot(pCmdRefineMean, pMeasRefineMean, '.k', label = 'Refinement Estimates')
plt.xlabel('Command')
plt.ylabel('Output')
plt.legend()
plt.grid(True)
#%% Bode Progression
if False:
gainLin_mag, phaseLin_deg = FreqTrans.GainPhase(TLinFinal, magUnit = 'mag', unwrap = False)
gainRefine_mag, phaseRefine_deg = FreqTrans.GainPhase(TuzRefine, magUnit = 'mag', unwrap = False)
gainFinal_mag, phaseFinal_deg = FreqTrans.GainPhase(TuzFinal, magUnit = 'mag', unwrap = False)
gainFinalMean_mag = np.mean(gainFinal_mag, axis=0)
phaseFinalMean_deg = np.mean(phaseFinal_deg, axis=0)
phaseFinalMean_deg = np.mean(phaseFinal_deg, axis=0)
CuzFinalMean = np.mean(CuzFinal, axis=0)
fig = None
fig = FreqTrans.PlotBode(freqExcFinal_rps * rps2hz, gainLin_mag.squeeze(), phaseLin_deg.squeeze(), coher_nd = np.ones_like(gainLin_mag).squeeze(), gainUnc_mag = None, fig = fig, dB = True, linestyle = '-', color='k', label='Linear Model')
fig = FreqTrans.PlotBode(freqExc_rps * rps2hz, gainRefine_mag[0], phaseRefine_deg[0], coher_nd = CuzRefine[0], gainUnc_mag = None, fig = fig, dB = True, linestyle = ':', color='b', label='First iteration')
fig = FreqTrans.PlotBode(freqExc_rps * rps2hz, gainRefine_mag[-1], phaseRefine_deg[-1], coher_nd = CuzRefine[-1], gainUnc_mag = None, fig = fig, dB = True, linestyle = '-', color='b', label='Last Refinement iteration')
for iFinal, t in enumerate(timeFinal_s):
fig = FreqTrans.PlotNyquist(ToEstNom[iOut, iIn], ToEstUncMag[iOut, iIn], fig = fig, fillType = 'circle', marker='.', color = 'b', linestyle='None', label = 'Estimate (MIMO)')
fig = FreqTrans.PlotNyquist(ToEstNom[iOut, iIn, sigIndx[iIn]], ToEstUncMag[iOut, iIn, sigIndx[iIn]], fig = fig, fillType = 'circle', marker='.', color = 'g', linestyle='None', label = 'Estimate (SIMO)')
fig = FreqTrans.PlotNyquist(np.array([-1+0j]), np.array([0.4]), fig = fig, fillType = 'circle', marker='+', color = 'r', linestyle='None')
ax = fig.get_axes()
# handles, labels = ax[0].get_legend_handles_labels()
# handles = [handles[0], handles[3], handles[1], handles[4], handles[2], handles[5]]
# labels = [labels[0], labels[3], labels[1], labels[4], labels[2], labels[5]]
# ax[0].legend(handles, labels)
fig.suptitle('$T_o$ : ' + '$r_{Exc}$[' + str(iIn) + '] to ' + '$z$[' + str(iOut) + ']')
#%% Bode Plot - Output Complimentary Sensitivity Function
# Linear Model Gain and Phase
gainToLinNom_mag, phaseToLinNom_deg = FreqTrans.GainPhase(ToLinNom, magUnit = 'mag', phaseUnit = 'deg', unwrap = True)
gainToLinUnc_mag = FreqTrans.Gain(ToLinUnc, magUnit = 'mag')
cohToLin_mag = np.ones_like(ToLinUnc)
# Estimated
gainToEstNom_mag, phaseToEstNom_deg = FreqTrans.GainPhase(ToEstNom, magUnit = 'mag', phaseUnit = 'deg', unwrap = True)
gainToEstUnc_mag = FreqTrans.Gain(ToEstUnc, magUnit = 'mag') # Estimation Uncertain Response
cohToEst_mag = ToEstCoh # Estimation Coherence
cohToEstMin = np.min(cohToEst_mag, axis = (0, 1))
if False:
numOut, numIn = ToLinNom.shape[0:-1]
ioArray = np.array(np.meshgrid(np.arange(numOut), np.arange(numIn))).T.reshape(-1, 2)
for iPlot, io in enumerate(ioArray):
linestyle='None',
label='Sample (MIMO)')
# ax = fig.get_axes()
# handles, labels = ax[0].get_legend_handles_labels()
# handles = [handles[0], handles[3], handles[1], handles[2], handles[3], handles[5]]
# labels = [labels[0], labels[3], labels[1], labels[2], labels[3], labels[5]]
# ax[0].legend(handles, labels)
fig.suptitle('$T_i$ : ' + '$u_{Exc}$[' + str(iIn) + '] to ' + 'u[' +
str(iOut) + ']')
#%% Bode Plot - Compl Sens Function
# Linear Model Gain and Phase
gainTiLinUnc_mag, phaseTiLinUnc_deg = FreqTrans.GainPhase(TiLinUnc,
magUnit='mag',
phaseUnit='deg',
unwrap=True)
cohTiLin_mag = np.ones_like(TiLinUnc)
# Estimated
gainTiEstNom_mag, phaseTiEstNom_deg = FreqTrans.GainPhase(TiEstNom,
magUnit='mag',
phaseUnit='deg',
unwrap=True)
gainTiEstUnc_mag = FreqTrans.Gain(
TiEstUnc, magUnit='mag') # Estimation Uncertain Response
cohTiEst_mag = TiEstCoh # Estimation Coherence
cohTiEstMin = np.min(cohTiEst_mag, axis=(0, 1))
if False:
numOut, numIn = TiLinNom.shape[0:-1]
T = np.zeros_like(Tev, dtype=complex)
C = np.zeros_like(Tev, dtype=float)
for i in range(T.shape[-1]):
T[..., i] = (Teb[..., i].T @ np.linalg.inv(Tev[..., i].T)).T
sigmaNom_mag, _ = FreqTrans.Sigma(T) # Singular Value Decomp
# Coherence
C = Ceb
T_InputName = exc_names
T_OutputName = fbName
gain_mag, phase_deg = FreqTrans.GainPhase(T,
magUnit='mag',
phaseUnit='deg',
unwrap=True)
rCritNom_mag, _, _ = FreqTrans.DistCrit(T, typeUnc='ellipse')
#rCritNom_mag, rCritUnc_mag, rCrit_mag = FreqTrans.DistCrit(T, TUnc, typeUnc = 'ellipse')
#rCritNom_mag, rCritUnc_mag, rCrit_mag, pCont_mag = FreqTrans.DistCritEllipse(T, TUnc) # Returns closest approach points
#%% Sigma Plot
Cmin = np.min(np.min(C, axis=0), axis=0)
sigmaNom_magMin = np.min(sigmaNom_mag, axis=0)
fig = 20
fig = FreqTrans.PlotSigma(freqLin_hz,
sigmaLin_mag,
coher_nd=np.ones_like(freqLin_hz),
fig=fig,
fmt='k',
fig=fig,
color='r',
marker='+',
label='Critical Region')
fig.suptitle(inPlot[iIn] + ' to ' + outPlot[iOut])
ax = fig.get_axes()
ax[0].set_xlim(-3, 1)
ax[0].set_ylim(-2, 2)
#%% Bode Plots
gainLaEstNomList_mag = []
gainLaEstUncList_mag = []
phaseLaEstNomList_deg = []
for iSeg in range(0, len(oDataSegs)):
gainLaEstNom_mag, phaseLaEstNom_deg = FreqTrans.GainPhase(
LaEstNomList[iSeg], magUnit='mag', phaseUnit='deg', unwrap=True)
gainLaEstUnc_mag = FreqTrans.Gain(LaEstNomList[iSeg], magUnit='mag')
gainLaEstNomList_mag.append(gainLaEstNom_mag)
phaseLaEstNomList_deg.append(phaseLaEstNom_deg)
gainLaEstUncList_mag.append(gainLaEstUnc_mag)
if False:
for iPlot, [iOut, iIn] in enumerate(ioArray):
fig = 20 + iPlot
for iSeg in range(0, len(oDataSegs)):
gain_mag = gainLaEstNomList_mag[iSeg][iOut, iIn]
phase_deg = phaseLaEstNomList_deg[iSeg][iOut, iIn]
coher_nd = LaEstCohList[iSeg][iOut, iIn]
gainUnc_mag = gainLaEstUncList_mag[iSeg][iOut, iIn]
ampInit,
ampFinal,
freqType='linear',
ampType='linear',
initZero=1)
# Simulate the excitation through the system
time_s, y, _ = signal.lsim(sys, x, time_s)
y = np.atleast_2d(y)
# Estimate the transfer function
optSpec = FreqTrans.OptSpect(freqRate=freqRate_rps,
smooth=('box', 1),
winType=('tukey', 0.0))
freq_rps, Txy, Cxy, Pxx, Pyy, Pxy = FreqTrans.FreqRespFuncEst(x, y, optSpec)
gain_dB, phase_deg = FreqTrans.GainPhase(Txy)
freq_hz = freq_rps * rps2hz
freq_hz = np.squeeze(freq_hz)
gain_dB = np.squeeze(gain_dB)
phase_deg = np.unwrap(np.squeeze(phase_deg) * deg2rad) * rad2deg
Cxy = np.squeeze(Cxy)
#%% Multisine signal with CZT
numChan = 1
numCycles = 1
freqMinDes_rps = freqInit_rps * np.ones(numChan)
freqMaxDes_rps = freqFinal_rps * np.ones(numChan)
freqStepDes_rps = (10 / freqRate_hz) * hz2rps
methodSW = 'zip' # "zippered" component distribution
plt.plot(time_s, pCmd, time_s, pOut)
#%% Plot the Excitation Spectrum
optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, freq = freqExc_rps, smooth = ('box', 3), winType = 'rect')
plt.figure(2)
TxyList = []
CxyList = []
PxxList = []
PyyList = []
PxyList = []
for i, pOut in enumerate(pOutList):
pCmd = pCmdList[i]
freq_rps, Txy, Cxy, Pxx, Pyy, Pxy = FreqTrans.FreqRespFuncEst(pCmd, pOut, optSpec)
gain_dB, phase_deg = FreqTrans.GainPhase(Txy)
freq_hz = freq_rps * rps2hz
freq_hz = np.squeeze(freq_hz)
gain_dB = np.squeeze(gain_dB)
phase_deg = np.squeeze(phase_deg)
Cxy = np.squeeze(Cxy)
# Cxy = np.squeeze(np.abs(Cxy)**2)
TxyList.append(Txy)
CxyList.append(Cxy)
PxxList.append(Pxx)
PyyList.append(Pyy)
PxyList.append(Pxy)
ax1 = plt.subplot(3,1,1); plt.grid(True)