freqRate=freqRate_rps,
smooth=('box', 3),
winType=('tukey', 0.0),
detrendType='Linear',
interpType='linear')
# Excited Frequencies per input channel
optSpec.freq = freqChan_rps
optSpec.freqInterp = freqExc_rps
# Null Frequencies
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
freq_rps, Txy, Cxy, Pxx, Pyy, Pxy, TxyUnc, PxxNull, Pnn = FreqTrans.FreqRespFuncEstNoise(
vExc, v, optSpec)
# _ , Txy, Cxy, _ , Pvv, Pev = FreqTrans.FreqRespFuncEst(vExc, v, optSpec)
freq_hz = freq_rps * rps2hz
print(np.sum(PxxNull, axis=-1) / np.sum(Pxx, axis=-1))
I3 = np.repeat([np.eye(3)], Txy.shape[-1], axis=0).T
TaEstNom = -Txy
TaEstUnc = TxyUnc
TaEstCoh = Cxy
SaEstNom = I3 - TaEstNom # Sa = I - Ta
SaEstUnc = -TaEstUnc
SaEstCoh = TaEstCoh
#%% Estimate the frequency response function
optSpec = FreqTrans.OptSpect(dftType = 'czt', scaleType = 'density', freqRate = freqRate_rps, smooth = ('box', 3), winType = 'bartlett', detrendType = 'linear')
# Excited Frequencies per input channel
optSpec.freq = freqChan_rps
optSpec.freqInterp = freqExc_rps
# Null Frequencies
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
#freq_rps, Tuz, Cuz, Suu, Szz, Suz = FreqTrans.FreqRespFuncEst(uExc, z, optSpec)
freq_rps, Tuz, Cuz, Suu, Szz, Suz, Tun, SuuNull, Snn = FreqTrans.FreqRespFuncEstNoise(uExc, z, optSpec)
freq_hz = freq_rps * rps2hz
TEstNom = Tuz
TEstUnc = Tun
TEstCoh = Cuz
#%% Sampling
numOut, numIn, nFreq = TEstNom.shape
nSamp = 20
shapeSamp = (numOut, numIn, nFreq, nSamp)
rSamp = np.sqrt(np.random.uniform(0, 1, shapeSamp))
phSamp = np.random.uniform(-np.pi, np.pi, shapeSamp)
samp = rSamp * np.exp(1j * phSamp)
z = outCL[2:4]
#%% Estimate the frequency response function
optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_rps, smooth = ('box', 3), winType = 'bartlett', detrendType = 'linear')
# Excited Frequencies per input channel
optSpec.freq = freqChan_rps
optSpec.freqInterp = freqExc_rps
# Null Frequencies
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
freq_rps, Txy, Cxy, Sxx, Syy, Sxy, TxyUnc, SxxNull, Snn = FreqTrans.FreqRespFuncEstNoise(rExc, z, optSpec)
freq_hz = freq_rps * rps2hz
I2 = np.repeat([np.eye(2)], Txy.shape[-1], axis=0).T
ToEstNom = Txy # Txy = Sxy / Sxx
ToEstUnc = -TxyUnc # TxyUnc = np.abs(Sxn / Sxx)
ToEstCoh = Cxy # Cxy = np.abs(Sxy)**2 / (Sxx * Syy)
SoEstNom, SoEstUnc, SoEstCoh = FreqTrans.TtoS(ToEstNom, ToEstUnc, ToEstCoh)
LoEstNom, LoEstUnc, LoEstCoh = FreqTrans.StoL(SoEstNom, SoEstUnc, SoEstCoh)
print(np.sum(SxxNull, axis = -1) / np.sum(Sxx, axis = -1))
print(np.sum(Snn, axis = -1) / np.sum(Syy, axis = -1))
ToEstSNR = np.abs(ToEstNom / ToEstUnc)**2
pCmd = sigList + r
pOut = np.zeros_like(pCmd[0])
for i, s in enumerate(pCmd[0]):
pOut[i] = objServo.Update(s)
# Add outputs
pMeas = pOut + n # pMeas = G (x + r) + n = Gx + (Gr + n); SNR = Gx / (Gr + n)
ampList.append(np.mean(ampExc_nd))
pCmdRefine.append(pCmd)
pMeasRefine.append(pMeas)
# Estimate the FRF
# freq_rps, Tuz, Cuz, Suu, Szz, Suz = FreqTrans.FreqRespFuncEst(pCmd, pMeas, optSpec)
freq_rps, Tuz, Cuz, Suu, Szz, Suz, Tun, SuuNull, Snn = FreqTrans.FreqRespFuncEstNoise(pCmd, pMeas, optSpec)
freq_hz = freq_rps * rps2hz
Cuz = Cuz.clip(0, 1)
TuzRefine.append(Tuz.squeeze())
CuzRefine.append(Cuz.squeeze())
SuuRefine.append(Suu.squeeze())
SzzRefine.append(Szz.squeeze())
SuzRefine.append(Suz.squeeze())
TunRefine.append(Tun.squeeze())
SuuNullRefine.append(SuuNull.squeeze())
SnnRefine.append(Snn.squeeze())
# Increase Amplitude
iAmpIncr = [True] * np.ones_like(ampChan_nd, dtype=bool)
# Null Frequencies
freqGap_rps = optSpec.freq.flatten()[0:-1] + 0.5 * np.diff(
optSpec.freq.flatten())
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
freq_rps = []
freq_hz = []
T = []
TUnc = []
C = []
for iSeg, seg in enumerate(oDataSegs):
freq, Tey, Cey, Pee, Pyy, Pey, TeyUnc, Pee_N, Pyy_N = FreqTrans.FreqRespFuncEstNoise(
eList[iSeg], outList[iSeg], optSpec)
freq, Teu, Ceu, _, Puu, Peu = FreqTrans.FreqRespFuncEst(
eList[iSeg], uList[iSeg], optSpec)
# Form the Frequency Response
freq_hz = freq * rps2hz
# Form the Frequency Response
T_seg = np.zeros_like(Tey)
TUnc_seg = np.zeros_like(Tey)
for i in range(T_seg.shape[-1]):
T_seg[..., i] = (Tey[..., i] @ np.linalg.inv(Teu[..., i]))
TUnc_seg[..., i] = (TeyUnc[..., i] @ np.linalg.inv(Teu[..., i]))
T.append(T_seg)
optSpec = FreqTrans.OptSpect(dftType='czt',
freqRate=freqRate_rps,
smooth=('box', 3),
winType=('tukey', 0.0),
detrendType='linear')
# Excited Frequencies per input channel
optSpec.freq = freqChan_rps
optSpec.freqInterp = freqExc_rps
# Null Frequencies
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
freq_rps, Txu, Cxu, Sxx, Suu, Sxy, TxuUnc, SxxNull, Snn = FreqTrans.FreqRespFuncEstNoise(
uExc, uCtrl, optSpec)
freq_hz = freq_rps * rps2hz
I2 = np.repeat([np.eye(2)], Txu.shape[-1], axis=0).T
TiEstNom = -Txu # Si = I - Ti
TiEstUnc = TxuUnc # TxuUnc = np.abs(Sxu / Sxx)
TiEstCoh = Cxu # Cxy = np.abs(Sxu)**2 / (Sxx * Suu)
# Ti to Si, Si to Li
SiEstNom, SiEstUnc, SiEstCoh = FreqTrans.TtoS(TiEstNom, TiEstUnc, TiEstCoh)
LiEstNom, LiEstUnc, LiEstCoh = FreqTrans.StoL(SiEstNom, SiEstUnc, SiEstCoh)
print(np.sum(SxxNull, axis=-1) / np.sum(Sxx, axis=-1))
TiEstSNR = np.abs(TiEstNom / TiEstUnc)**2
SiEstSNR = np.abs(SiEstNom / SiEstUnc)**2
LiEstSNR = np.abs(LiEstNom / LiEstUnc)**2
optSpec.freq = np.asarray(freqExc_rps)
optSpec.freqInterp = np.sort(optSpec.freq.flatten())
# Null Frequencies
freqGap_rps = optSpec.freq.flatten()[0:-1] + 0.5 * np.diff(
optSpec.freq.flatten())
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
# FRF Estimate
T = []
TUnc = []
C = []
for iSeg, seg in enumerate(oDataSegs):
freq_rps, Teb, Ceb, Pee, Pbb, Peb, TebUnc, Pee_N, Pbb_N = FreqTrans.FreqRespFuncEstNoise(
vExcList[iSeg], vFbList[iSeg], optSpec)
_, Tev, Cev, _, Pvv, Pev, TevUnc, _, Pvv_N = FreqTrans.FreqRespFuncEstNoise(
vExcList[iSeg], vCmdList[iSeg], optSpec)
freq_hz = freq_rps * rps2hz
# Form the Frequency Response
T_seg = np.empty_like(Tev)
TUnc_seg = np.empty_like(Tev)
for i in range(T_seg.shape[-1]):
T_seg[..., i] = Teb[..., i] @ np.linalg.inv(Tev[..., i])
TUnc_seg[..., i] = TebUnc[..., i] @ np.linalg.inv(Tev[..., i])
T.append(T_seg)
TUnc.append(np.abs(TUnc_seg))
optSpec.freqNullInterp = True
# FRF Estimate
TaEstNomList = []
TaEstUncList = []
TaEstCohList = []
SaEstNomList = []
SaEstUncList = []
SaEstCohList = []
LaEstNomList = []
LaEstUncList = []
LaEstCohList = []
for iSeg, seg in enumerate(oDataSegs):
freq_rps, Txy, Cxy, Sxx, Syy, Sxy, TxyUnc, SxxNull, Snn = FreqTrans.FreqRespFuncEstNoise(
vExcList[iSeg], vFbList[iSeg], optSpec)
freq_hz = freq_rps * rps2hz
TaEstNom = -Txy # Sa = I - Ta
TaEstUnc = TxyUnc # TxuUnc = np.abs(Sxu / Sxx)
TaEstCoh = Cxy # Cxy = np.abs(Sxu)**2 / (Sxx * Suu)
SaEstNom, SaEstUnc, SaEstCoh = FreqTrans.TtoS(TaEstNom, TaEstUnc, TaEstCoh)
LaEstNom, LaEstUnc, LaEstCoh = FreqTrans.StoL(SaEstNom, SaEstUnc, SaEstCoh)
TaEstNomList.append(TaEstNom)
TaEstUncList.append(TaEstUnc)
TaEstCohList.append(TaEstCoh)
SaEstNomList.append(SaEstNom)
SaEstUncList.append(SaEstUnc)
SaEstCohList.append(SaEstCoh)
