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)