Fs = 1000;
samples = 25000;
t = (0:samples-1)'*1/Fs;
signal = cos(2*pi*50*t)+cos(2*pi*100*t)+1e-3*rand(samples,1);
NFFT = 1000;
Overlap = 0.75;
w = hanning(NFFT);
fc = 50;
wc = 2*pi*fc;
Q = 5;
w0 = 2*pi*fc/Fs;
alpha = sin(w0)/(2*Q);
b0 = 1;
b1 = -2*cos(w0);
b2 = 1;
a0 = 1 + alpha;
a1 = -2*cos(w0);
a2 = 1 - alpha;
num1=[1/wc^2 0 1];
den1=[1/wc^2 1/(wc*Q) 1];
num1z=[b0 b1 b2];
den1z=[a0 a1 a2];
freq = linspace(fc-1,fc+1,200);
[g1,p1] = bode(num1,den1,2*pi*freq);
g1db = 20*log10(g1);
[gd1,pd1] = dbode(num1z,den1z,1/Fs,2*pi*freq);
gd1db = 20*log10(gd1);
figure(1);
plot(freq,g1db,'b',freq,gd1db,'+r');
title(' Notch: H(s) = (s^2 + 1) / (s^2 + s/Q + 1)');
legend('analog','digital');
xlabel('Fréquence (Hz)');
ylabel(' dB')
signal_filtered = filtfilt(num1z,den1z,signal);
[freq,fft_spectrum] = myfft_peak(signal, Fs, NFFT, Overlap);
sensor_spectrum_dB = 20*log10(fft_spectrum);
df = mean(diff(freq));
[pks,locs]= findpeaks(sensor_spectrum_dB,'SortStr','descend','NPeaks',2);
[freq,fft_spectrum_filtered] = myfft_peak(signal_filtered, Fs, NFFT, Overlap);
sensor_spectrum_filtered_dB = 20*log10(fft_spectrum_filtered);
figure(2),semilogx(freq,sensor_spectrum_dB,'b',freq,sensor_spectrum_filtered_dB,'r');grid
title(['Averaged FFT Spectrum / Fs = ' num2str(Fs) ' Hz / Delta f = ' num2str(freq(2)-freq(1)) ' Hz ']);
legend('before notch filter','after notch filter');
xlabel('Frequency (Hz)');ylabel(' dB')
text(locs+.02,pks,num2str(freq(locs)))
function [freq_vector,fft_spectrum] = myfft_peak(signal, Fs, nfft, Overlap)
signal = signal(:);
samples = length(signal);
if samples<nfft
s_tmp = zeros(nfft,1);
s_tmp((1:samples)) = signal;
signal = s_tmp;
samples = nfft;
end
window = hanning(nfft);
window = window(:);
offset = fix((1-Overlap)*nfft);
spectnum = 1+ fix((samples-nfft)/offset);
fft_spectrum = 0;
for i=1:spectnum
start = (i-1)*offset;
sw = signal((1+start):(start+nfft)).*window;
fft_spectrum = fft_spectrum + (abs(fft(sw))*4/nfft);
end
fft_spectrum = fft_spectrum/spectnum;
if rem(nfft,2)
select = (1:(nfft+1)/2)';
else
select = (1:nfft/2+1)';
end
fft_spectrum = fft_spectrum(select);
freq_vector = (select - 1)*Fs/nfft;
end
Best Answer