X0=0.0,
transpose=False)
vCmdName = sysCL.OutputName[:3]
vCmd = out[:3]
zName = sysCL.OutputName[-7:]
z = out[-7:]
#v = -(vCmd - vExc)
v = vCmd # FIXIT
#%% Estimate the frequency response function
optSpec = FreqTrans.OptSpect(dftType='czt',
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)
plt.figure()
plt.subplot(3,1,1)
plt.plot(time_s, sig); plt.grid()
plt.ylabel('Signal (nd)');
plt.subplot(3,1,2)
plt.plot(time_s, ampChirp); plt.grid()
plt.ylabel('ampitude (nd)');
plt.subplot(3,1,3)
plt.plot(time_s, freqChirp_rps * rps2hz); plt.grid()
plt.xlabel('Time (s)'); plt.ylabel('Frequency (Hz)')
plt.show()
#%% Plot the Excitation Spectrum
## Compute Spectrum of each channel
optSpect = FreqTrans.OptSpect(freqRate = freqRate_hz)
freq_hz, sigDft, Psd_mag = FreqTrans.Spectrum(sig, optSpect)
Psd_dB = 20*np.log10(Psd_mag)
## Plot Spectrum
plt.figure()
plt.plot(freq_hz[0], Psd_dB[0])
plt.xlabel('frequency (Hz)');
plt.ylabel('Spectrum (dB)');
plt.grid()
plt.show()
#%% Create the output for JSON config
timeFinal_s = time_s[-1]
_, r, _ = control.forced_response(sysR, T = time_s, U = p)
r = r + rExc
uExc = np.zeros_like(rExc)
m = np.random.normal([0.0, 0.0], mSigma, size = (len(time_s), 2)).T
_, n, _ = control.forced_response(sysN, T = time_s, U = m)
inCL = np.concatenate((r, uExc, n))
_, outCL, _ = control.forced_response(sysCL, T = time_s, U = inCL)
uCtrl = outCL[0:2]
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
freqNat_rps = freqNat_hz * hz2rps
objServo = Servo.Servo(1/freqRate_hz, freqNat_rps = freqNat_rps, damp = 0.8)
objServo.freeplay = 1.0
objServo.timeDelay_s = 50 / 1000 # this ends up rounded to an integer (timeDelay_s * freqRate_hz)
objServo.vLim = 560
np.random.seed(584)
rStd = [0.5]
r = np.random.normal([0.0], rStd, size = (len(time_s), 1)).T
nStd = [0.5]
n = np.random.normal([0.0], nStd, size = (len(time_s), 1)).T
#%% Setup FRE
optSpec = FreqTrans.OptSpect(dftType = 'czt', scaleType = 'density', freqRate = freqRate_hz * hz2rps, smooth = ('box', 3), winType = 'rect', detrendType = 'linear')
# Excited Frequencies per input channel
optSpec.freq = freqChan_rps
optSpec.freqInterp = freqExc_rps
# Null Frequencies
optSpec.freqNull = freqGap_rps
optSpec.freqNullInterp = True
#%% Simulate
timeRefine_s = []
ampList = []
pCmdRefine = []
pMeasRefine = []
ax[iChan].set_xlabel('Time (s)')
if True:
fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True)
for iChan in range(0, numChan):
for iElem in sigIndx[iChan]:
ax[iChan].plot(time_s, sigElem[iElem])
ax[iChan].set_ylabel('Amplitude (nd)')
ax[iChan].grid(True)
ax[iChan].set_xlabel('Time (s)')
#%% Plot the Excitation Spectrum
## Compute Spectrum of each channel
optFFT = FreqTrans.OptSpect(freqRate = freqRate_hz * hz2rps, winType = ('tukey', 0.0), smooth = ('box', 1))
optCZT = FreqTrans.OptSpect(dftType = 'czt', freqRate = freqRate_hz * hz2rps, winType = ('tukey', 0.0), smooth = ('box', 1))
freq_fft = []
P_dB_fft = []
freq_czt = []
P_dB_czt = []
for iChan, sig in enumerate(sigList):
freq_rps_fft, _, P_fft = FreqTrans.Spectrum(sig, optFFT)
freq_fft.append(freq_rps_fft * rps2hz)
P_dB_fft.append(20*np.log10(P_fft))
optCZT.freq = freqElem_rps[sigIndx[iChan]]
freq_rps_czt, _, P_czt = FreqTrans.Spectrum(sig, optCZT)
freq_czt.append(freq_rps_czt * rps2hz)
for iSig, sigOut in enumerate(sigOutList):
outSig[iSig] = seg[sigOut]
# outSig[iSig] = seg['Surf'][sigOut]
# plt.plot(oDataSegs[iSeg]['time_s'], outSig[iSig])
outList.append(outSig)
#%% Estimate the frequency response function
# Define the excitation frequencies
freqRate_hz = 50
freqRate_rps = freqRate_hz * hz2rps
optSpec = FreqTrans.OptSpect(dftType='czt',
freqRate=freqRate_rps,
smooth=('box', 3),
winType=('tukey', 0.2),
detrendType='Linear')
# Excited Frequencies per input channel
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
freq_rps = []
plt.figure()
plt.title(str(W_theshold_dB) + 'dB Bandwidth')
#overSamp = numSampList / numSampMin
plt.loglog(numSampList, np.asarray(binList), '-*')
plt.xlabel('Number of Samples')
plt.ylabel('Number of bins')
plt.grid('on')
plt.show
#%% Signal length - Window only
numSampList = np.arange(100, 1001, 100)
numSampMax = int(np.max(numSampList))
numSampMin = int(np.min(numSampList))
overSamp = numSampList / numSampMin
optSpec = FreqTrans.OptSpect(dftType = 'czt', freqRate = 1 * hz2rps, smooth = ('box', 1))
#optSpec.scaleType = 'density'
optSpec.winType = ('tukey', 0.0)
optSpec.freq = np.linspace(0, 0.05*np.pi, numSampMin-1)
sig = np.ones(numSampMax)
noiseSamples = np.random.randn(numSampMax)
noiseLev = 0.0
sig += noiseLev * noiseSamples
P_theshold_dB = -12
plt.figure()
binList = []
for numSamp in numSampList:
numSamp = int(numSamp)
color='r',
label='Hann Approximation')
ax = fig.get_axes()
ax[0].set_xscale('linear')
ax[0].set_xlim(left=0.0, right=10)
ax[0].set_xlabel('Normalized Bin')
ax[0].set_ylim(bottom=-70, top=10)
ax[0].set_ylabel('Normalized Power Magnitude [dB]')
ax[0].grid(True)
ax[0].legend()
#%% Validation with Spectrum
optTemp = FreqTrans.OptSpect(dftType='dftMat',
scaleType='density',
freqRate=freqRate_rps,
smooth=('box', 1))
optTemp.winType = ('tukey', 0.0)
#optTemp.winType = 'bartlett'
optTemp.freq = freqChan_rps
optTemp.freqInterp = freqExc_rps
optTempN = FreqTrans.OptSpect(dftType='dftMat',
scaleType='density',
freqRate=freqRate_rps,
smooth=('box', 1))
optTempN.winType = ('tukey', 0.0)
#optTempN.winType = 'bartlett'
optTempN.freq = np.vstack((freqGap_rps, freqGap_rps, freqGap_rps))
optTemp.freqInterp = freqExc_rps
plt.plot(oDataSegs[iSeg]['time_s'], oDataSegs[iSeg]['vIas_mps'])
# plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][0])
# plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][1])
# plt.plot(oDataSegs[iSeg]['time_s'], vExcList[iSeg][2])
# plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][0])
# plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][1])
# plt.plot(oDataSegs[iSeg]['time_s'], vFbList[iSeg][2])
#%% Estimate the frequency response function
# Define the excitation frequencies
freqRate_hz = 50
freqRate_rps = freqRate_hz * hz2rps
optSpec = FreqTrans.OptSpect(dftType='czt',
freqRate=freqRate_rps,
smooth=('box', 3),
winType=('tukey', 0.2),
detrendType='Linear')
# Excited Frequencies per input channel
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 = []
x, ampChirpX, freqChirp_rps = GenExcite.Chirp(freqInit_rps,
freqFinal_rps,
time_s,
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)
ax[iChan].grid(True)
ax[iChan].set_xlabel('Time (s)')
if True:
fig, ax = plt.subplots(ncols=1, nrows=numChan, sharex=True)
for iChan in range(0, numChan):
for iElem in sigIndx[iChan]:
ax[iChan].plot(time_s, sigElem[iElem])
ax[iChan].set_ylabel('Amplitude (nd)')
ax[iChan].grid(True)
ax[iChan].set_xlabel('Time (s)')
#%% Plot the Excitation Spectrum
## Compute Spectrum of each channel
optFFT = FreqTrans.OptSpect(freqRate=freqRate_hz * hz2rps)
optMAT = FreqTrans.OptSpect(dftType='dftmat', freqRate=freqRate_hz * hz2rps)
optCZT = FreqTrans.OptSpect(dftType='czt', freqRate=freqRate_hz * hz2rps)
freq_fft = []
P_dB_fft = []
freq_mat = []
P_dB_mat = []
freq_czt = []
P_dB_czt = []
for iChan, sig in enumerate(sigList):
freq_rps_fft, _, P_fft = FreqTrans.Spectrum(sig, optFFT)
freq_fft.append(freq_rps_fft * rps2hz)
P_dB_fft.append(20 * np.log10(P_fft))
v = max(v, np.abs(objServo.v))
a = max(a, np.abs(objServo.a))
av = max(av, a*v)
print(amp, p, v, a, av)
pCmdList.append(pCmd)
pOutList.append(pOut)
if True:
plt.figure(1)
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)