def SigmaTemporal(THist):
numSec, numOut, numIn, numFreq = THist.shape
sHist = np.zeros((numSec, numOut, numFreq))
for iSec in range(numSec):
sHist[iSec, ...] = FreqTrans.Sigma(THist[iSec, ...])
return sHist
SaEstNomElem = SaEstNom[..., i]
SaEstUncElem = SaEstUnc[..., i]
SaEstNomInvElem = inv(SaEstNomElem)
LaEstNom[..., i] = -np.eye(3) + SaEstNomInvElem
LaEstUnc[..., i] = -inv(np.eye(3) + SaEstNomInvElem @ SaEstUncElem) @ (
SaEstNomInvElem @ SaEstUncElem @ SaEstNomInvElem)
# LaEstCoh[...,i] = -np.eye(3) + inv(SaEstCoh[...,i])
LaEstCoh[..., i] = SaEstCoh[..., i]
LaEstSNR = np.abs(LaEstNom / LaEstUnc)**2
LaEstMeanSNR = np.mean(LaEstSNR, axis=-1)
#%% Sigma Plot
I3 = np.repeat([np.eye(3)], LaLinNom.shape[-1], axis=0).T
svLaLinNom_mag = FreqTrans.Sigma(I3 +
LaLinNom) # sigma(I + Li) = 1 / sigma(Si)
svLaLinNomMin_mag = np.min(svLaLinNom_mag, axis=0)
svLaLinUnc_mag = FreqTrans.Sigma(LaLinUnc)
svLaLinUncMax_mag = np.max(svLaLinUnc_mag, axis=0)
svLaLinLower_mag = svLaLinNomMin_mag - svLaLinUncMax_mag
# I3 = np.repeat([np.eye(3)], SaEstNom.shape[-1], axis=0).T
# svLaEstNom_mag = FreqTrans.Sigma(I3 + LaEstNom)
svLaEstNom_mag = 1 / FreqTrans.Sigma(SaEstNom) # sv(I + La) = 1 / sv(Sa)
svLaEstNomMin_mag = np.min(svLaEstNom_mag, axis=0)
svLaEstUnc_mag = FreqTrans.Sigma(LaEstUnc)
svLaEstUncMax_mag = np.max(svLaEstUnc_mag, axis=0)
svLaEstLower_mag = svLaEstNomMin_mag - svLaEstUncMax_mag
fig = FreqTrans.PlotBode(freq_hz[iIn, sigIndx[iIn]], gainToEstNom_mag[iOut, iIn, sigIndx[iIn]], phaseToEstNom_deg[iOut, iIn, sigIndx[iIn]], coher_nd = cohToEst_mag[iOut, iIn, sigIndx[iIn]], gainUnc_mag = gainToEstUnc_mag[iOut, iIn, sigIndx[iIn]], fig = fig, dB = True, color='g', label='Estimate [SIMO]')
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) + ']')
#%% Sigma - Output Stability
I2 = np.repeat([np.eye(2)], LoLinNom.shape[-1], axis=0).T
# Linear Model SVD magnitude
svLoLinNom_mag = FreqTrans.Sigma(I2 + LoLinNom) # sigma(I + Lo)
svLoLinNomMin_mag = np.min(svLoLinNom_mag, axis=0)
svLoLinUnc_mag = FreqTrans.Sigma(LoLinUnc)
svLoLinUncMax_mag = np.max(svLoLinUnc_mag, axis=0) # Overly Conservative
cohLoLin_mag = np.ones_like(LoLinUnc)
# Estimate SVD magnitude
# svLiEstNom_mag = FreqTrans.Sigma(I2 + LoEstNom)
svLoEstNom_mag = 1 / FreqTrans.Sigma(SoEstNom) # sigma(I + Lo) = 1 / sigma(So)
svLoEstNomMin_mag = np.min(svLoEstNom_mag, axis=0)
svLoEstUnc_mag = FreqTrans.Sigma(LoEstUnc) # Uncertain SVD magnitude
svLoEstUncMax_mag = np.max(svLoEstUnc_mag, axis=0) # Overly Conservative
plt.plot(freq_hz[iIn], mag2db(Snn[iOut, iIn]), '.b', label = 'Estimate Null [MIMO]');
plt.plot(freqGap_rps*rps2hz, mag2db(SnnN[iOut]), '.g:', label = 'Estimate from Null')
plt.ylabel('Power Spectral Density [dB]')
plt.xlabel('Frequency [Hz]')
plt.legend()
plt.xlim([4,8])
fig.set_tight_layout(True)
fig.set_size_inches([6.4,2.4])
if False:
FreqTrans.PrintPrettyFig(fig, 'OpenMimoNullInterp.pgf')
#%% Sigma Plot
# Linear Model SVD magnitude
svTLinNom_mag = FreqTrans.Sigma(TLinNom)
svTLinUnc_mag = FreqTrans.Sigma(TLinUnc)
svTLinNomMin_mag = np.min(svTLinNom_mag, axis=0)
svTLinUncMax_mag = np.max(svTLinUnc_mag, axis=0) # Overly Conservative
cohTLin_mag = np.ones_like(TLinNom)
cohTLinMin = np.min(cohTLin_mag, axis = (0, 1))
# Estimate SVD magnitude
svTEstNom_mag = FreqTrans.Sigma(TEstNom)
svTEstUnc_mag = FreqTrans.Sigma(TEstUnc) # Uncertain SVD magnitude
svTEstNomMin_mag = np.min(svTEstNom_mag, axis=0)
svTEstUncMax_mag = np.max(svTEstUnc_mag, axis=0) # Overly Conservative
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_i$ : ' + '$u_{Exc}$[' + str(iIn) + '] to ' + 'u[' +
str(iOut) + ']')
#%% Sigma Plot
I2 = np.repeat([np.eye(2)], len(freqLin_rps), axis=0).T
# Linear Model SVD magnitude
svLiLinNom_mag = FreqTrans.Sigma(I2 + LiLinNom) # sigma(I + Li)
svLiLinNomMin_mag = np.min(svLiLinNom_mag, axis=0)
svLiLinUnc_mag = FreqTrans.Sigma(LiLinUnc)
svLiLinUncMax_mag = np.max(svLiLinUnc_mag, axis=0) # Overly Conservative
cohLiLin_mag = np.ones_like(LiLinUnc)
# Compute Structured Singular Values
svLiLinSSV_mag, LiLinCrit = FreqTrans.SigmaStruct(I2 + LiLinNom,
np.abs(LiLinUnc))
LiLinCrit = LiLinCrit - I2
# Estimate SVD magnitude
I2 = np.repeat([np.eye(2)], len(freqExc_rps), axis=0).T
# svLiEstNom_mag = FreqTrans.Sigma(I2 + LiEstNom) # sigma(I + Li)
gain_mag.append(gainElem_mag)
gainUnc_mag.append(gainElemUnc_mag)
phase_deg.append(FreqTrans.Phase(T[iSeg], phaseUnit='deg', unwrap=True))
nom_mag, unc_mag, diff_mag = FreqTrans.DistCritCirc(T[iSeg],
TUnc[iSeg],
pCrit=-1 + 0j,
typeNorm='RSS')
rCritNom_mag.append(nom_mag)
rCritUnc_mag.append(unc_mag)
rCrit_mag.append(diff_mag)
sNom_seg, sUnc_seg = FreqTrans.Sigma(
T[iSeg], TUnc[iSeg]) # Singular Value Decomp, U @ S @ Vh == T[...,i]
sNom.append(sNom_seg)
sUnc.append(sUnc_seg)
#%% Spectrograms
if False:
#%%
iSgnlExc = 0
iSgnlOut = 0
freqRate_rps = 50 * hz2rps
optSpec = FreqTrans.OptSpect(dftType='dftmat',
freq=freqExc_rps[iSgnlExc],
freqRate=freqRate_rps,
smooth=('box', 3),
winType=('tukey', 0.0),
inList = [sysOL.InputName.index(s) for s in ['cmdP', 'cmdQ', 'cmdR']]
outList = [sysOL.OutputName.index(s) for s in ['fbP', 'fbQ', 'fbR']]
sysSimOL = sysOL[outList, :]
sysSimOL = sysOL[:, inList]
# Linear System Response
gainLin_mag, phaseLin_rad, _ = control.freqresp(sysSimOL, omega=freqLin_rps)
TxyLin = gainLin_mag * np.exp(1j * phaseLin_rad)
gainLin_dB = 20 * np.log10(gainLin_mag)
phaseLin_deg = np.unwrap(phaseLin_rad) * rad2deg
rCritLin_mag = np.abs(TxyLin - (-1 + 0j))
sigmaLin_mag, _ = FreqTrans.Sigma(TxyLin)
#%% Excitation
numExc = 3
numCycles = 1
ampInit = 4.0 * deg2rad
ampFinal = ampInit
freqMinDes_rps = 0.1 * hz2rps * np.ones(numExc)
freqMaxDes_rps = 10 * hz2rps * np.ones(numExc)
freqStepDes_rps = (10 / freqRate_hz) * hz2rps
methodSW = 'zip' # "zippered" component distribution
# Generate MultiSine Frequencies
freqExc_rps, sigIndx, time_s = GenExcite.MultiSineComponents(
freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps,
methodSW)
rCritNom_mag = []
rCrit_mag = []
sNom = []
for iSeg in range(0, len(oDataSegs)):
gainElem_mag, _, _ = FreqTrans.DistCritCirc(T[iSeg], pCrit = 0+0j, typeNorm = 'RSS')
gain_mag.append(gainElem_mag)
phase_deg.append(FreqTrans.Phase(T[iSeg], phaseUnit = 'deg', unwrap = True))
nom_mag, _, _= FreqTrans.DistCritCirc(T[iSeg], pCrit = -1+0j, typeNorm = 'RSS')
rCritNom_mag.append(nom_mag)
sNom_seg, _ = FreqTrans.Sigma(T[iSeg]) # Singular Value Decomp, U @ S @ Vh == T[...,i]
sNom.append(sNom_seg)
#%% Sigma Plot
fig = None
for iSeg in range(0, len(oDataSegs)):
Cmin = np.min(np.min(C[iSeg], axis = 0), axis = 0)
sNomMin = np.min(sNom[iSeg], axis=0)
fig = FreqTrans.PlotSigma(freq_hz[0], sNomMin, coher_nd = Cmin, fig = fig, fmt = rtsmSegList[iSeg]['fmt'] + '*-', label = oDataSegs[iSeg]['Desc'])
fig = FreqTrans.PlotSigma(freq_hz[0], 0.4 * np.ones_like(freq_hz[0]), fmt = '--r', fig = fig)
ax = fig.get_axes()
ax[0].set_xlim(0, 10)
# fig = FreqTrans.PlotBode(freq_hz[iIn, sigIndx[iIn]], gainTiEstNom_mag[iOut, iIn, sigIndx[iIn]], phaseTiEstNom_deg[iOut, iIn, sigIndx[iIn]], coher_nd = cohTiEst_mag[iOut, iIn, sigIndx[iIn]], gainUnc_mag = gainTiEstUnc_mag[iOut, iIn, sigIndx[iIn]], fig = fig, dB = True, color='g', label='Estimate [SIMO]')
ax = fig.get_axes()
handles, labels = ax[0].get_legend_handles_labels()
handles = [(handles[0], handles[2]), (handles[1], handles[3])]
labels = [labels[0], labels[2]]
ax[0].legend(handles, labels)
fig.suptitle('$T_i$ : ' + '$r_{Exc}$[' + str(iIn) + '] to ' + '$z$[' +
str(iOut) + ']')
#%% Sigma - Output Stability
I2 = np.repeat([np.eye(2)], LiLinNom.shape[-1], axis=0).T
# Linear Model SVD magnitude
svLiLinNom_mag = FreqTrans.Sigma(I2 + LiLinNom) # sigma(I + Lo)
svLiLinNomMin_mag = np.min(svLiLinNom_mag, axis=0)
svLiLinUnc_mag = FreqTrans.Sigma(LiLinUnc)
svLiLinUncMax_mag = np.max(svLiLinUnc_mag, axis=0) # Overly Conservative
cohLiLin_mag = np.ones_like(LiLinUnc)
# Estimate SVD magnitude
# svLiEstNom_mag = FreqTrans.Sigma(I2 + LiEstNom)
svLiEstNom_mag = 1 / FreqTrans.Sigma(SiEstNom) # sigma(I + Li) = 1 / sigma(Si)
svLiEstNomMin_mag = np.min(svLiEstNom_mag, axis=0)
svLiEstUnc_mag = FreqTrans.Sigma(LiEstUnc) # Uncertain SVD magnitude
svLiEstUncMax_mag = np.max(svLiEstUnc_mag, axis=0) # Overly Conservative
print(np.sum(SxxNull, axis=-1) / np.sum(Sxx, axis=-1))
T_InputNames = sigExcList
T_OutputNames = sigFbList
# Compute Gain, Phase, Crit Distance
#%% Sigma Plot
svLaEstNomList = []
svLaEstUncList = []
for iSeg in range(0, len(oDataSegs)):
# I3 = np.repeat([np.eye(3)], SaEstNomList.shape[-1], axis=0).T
# svLaEstNom_mag = FreqTrans.Sigma( I3 + LaEstNomList[iSeg] ) # Singular Value Decomp
svLaEstNom_mag = 1 / FreqTrans.Sigma(
SaEstNomList[iSeg]) # sv(I + La) = 1 / sv(Sa)
svLaEstUnc_mag = FreqTrans.Sigma(
LaEstUncList[iSeg]) # Singular Value Decomp
svLaEstNomList.append(svLaEstNom_mag)
svLaEstUncList.append(svLaEstUnc_mag)
if True:
fig = None
for iSeg in range(0, len(oDataSegs)):
cohLaEst = LaEstCohList[iSeg]
# cohLaEstMin = np.min(cohLaEst, axis = (0,1))
cohLaEstMin = np.mean(cohLaEst, axis=(0, 1))
svNom = svLaEstNomList[iSeg]
svNomMin = np.min(svNom, axis=0)
LiEstCoh = np.zeros_like(SaEstCoh)
inv = np.linalg.inv
for i in range(SaEstNom.shape[-1]):
SaEstNomElem = SaEstNom[..., i]
SaEstNomInvElem = inv(SaEstNomElem)
LiEstNom[..., i] = -np.eye(3) + SaEstNomInvElem
# LiEstCoh[...,i] = -np.eye(3) + inv(SaEstCoh[...,i])
LiEstCoh[..., i] = SaEstCoh[..., i]
LiEstNomList.append(LiEstNom)
LiEstCohList.append(LiEstCoh)
svLiEstNomList_seg = FreqTrans.Sigma(LiEstNom) # Singular Value Decomp
svLiEstNomList.append(svLiEstNomList_seg)
T_InputNames = sigExcList
T_OutputNames = sigFbList
# Compute Gain, Phase, Crit Distance
gainLiEstNomList_mag = []
phaseLiEstNomList_deg = []
rCritLiEstNomList_mag = []
for iSeg in range(0, len(oDataSegs)):
gain_mag, phase_deg = FreqTrans.GainPhase(LiEstNomList[iSeg],
magUnit='mag',
phaseUnit='deg',
unwrap=True)
