hello
see example below :
the notch filter simu is a bonus
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load signal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% dummy data
Fs = 10000;
samples = 25000;
t = (0:samples-1)'*1/Fs;
signal = sin(2*pi*1000*t)+sin(2*pi*2000*t)+sin(2*pi*3000*t)+1e-3*rand(samples,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FFT parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
NFFT = 1000; %
Overlap = 0.75;
w = hanning(NFFT); % Hanning window / Use the HANN function to get a Hanning window which has the first and last zero-weighted samples.
%% notch filter section %%%%%%
% H(s) = (s^2 + 1) / (s^2 + s/Q + 1)
fc = 1000; % notch freq
wc = 2*pi*fc;
Q = 5; % adjust Q factor for wider (low Q) / narrower (high Q) notch
% at f = fc the filter has gain = 0
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;
% analog notch (for info)
num1=[1/wc^2 0 1];
den1=[1/wc^2 1/(wc*Q) 1];
% digital notch (for info)
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')
% now let's filter the signal
signal_filtered = filtfilt(num1z,den1z,signal);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% display : averaged FFT spectrum before / after notch filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[freq,fft_spectrum] = myfft_peak(signal, Fs, NFFT, Overlap);
sensor_spectrum_dB = 20*log10(fft_spectrum);% convert to dB scale (ref = 1)
% demo findpeaks
df = mean(diff(freq));
[pks,locs]= findpeaks(sensor_spectrum_dB,'SortStr','descend','NPeaks',3);
[freq,fft_spectrum_filtered] = myfft_peak(signal_filtered, Fs, NFFT, Overlap);
sensor_spectrum_filtered_dB = 20*log10(fft_spectrum_filtered);% convert to dB scale (ref = 1)
figure(2),plot(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(freq(locs)+df,pks,num2str(freq(locs)))
function [freq_vector,fft_spectrum] = myfft_peak(signal, Fs, nfft, Overlap)
% FFT peak spectrum of signal (example sinus amplitude 1 = 0 dB after fft).
% Linear averaging
% signal - input signal,
% Fs - Sampling frequency (Hz).
% nfft - FFT window size
% Overlap - buffer overlap % (between 0 and 0.95)
signal = signal(:);
samples = length(signal);
% fill signal with zeros if its length is lower than nfft
if samples<nfft
s_tmp = zeros(nfft,1);
s_tmp((1:samples)) = signal;
signal = s_tmp;
samples = nfft;
end
% window : hanning
window = hanning(nfft);
window = window(:);
% compute fft with overlap
offset = fix((1-Overlap)*nfft);
spectnum = 1+ fix((samples-nfft)/offset); % Number of windows
% % for info is equivalent to :
% noverlap = Overlap*nfft;
% spectnum = fix((samples-noverlap)/(nfft-noverlap)); % Number of windows
% main loop
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); % X=fft(x.*hanning(N))*4/N; % hanning only
end
fft_spectrum = fft_spectrum/spectnum; % to do linear averaging scaling
% one sidded fft spectrum % Select first half
if rem(nfft,2) % nfft odd
select = (1:(nfft+1)/2)';
else
select = (1:nfft/2+1)';
end
fft_spectrum = fft_spectrum(select);
freq_vector = (select - 1)*Fs/nfft;
end
