% A Matlab script to demonstrate parabolic interpolation of a spectral
% peak. See the following link for details on the parabolic interpolation
% forumulae:
%
% - https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html
%
% by Gary Scavone, McGill University, 2019-2020.

fs = 22050; % sample rate
N = 22050;  % impulse response length and fft size
f0 = 1000;  % resonance center frequency (Hz)
Q = 40;     % resonance Q
B = f0 / Q; % resonance bandwidth in Hz
r = exp(-pi*B/fs);
b0 = 20;
a1 = -2*r*cos(2*pi*f0/fs);
a2 = r^2;

x = [1, zeros(1, N-1)];
y = filter( b0, [1 a1 a2], x ); % resonance impulse response
Y = fft(y); % 1 Hz per fft bin

subplot(2, 1, 1)
plot(y)
grid
xlim([0 2000])
xlabel('Time (samples)');
ylabel('Amplitude');

subplot(2, 1, 2)
f = (0:N-1)*fs/N;
plot(f, 20*log10(abs(Y)))
xlim([0 11000])
grid
xlabel('Frequency (Hz)')
ylabel('Magnitude (dB)')

disp('Complete spectrum ... hit key to proceed with estimation.')
pause

Y = abs(Y(1:ceil(N/2))); % throw away redundant half of fft
Xi = [];
for n = 2:length(Y)-2 % search for peak indices
  if Y(n+1) <= Y(n) && Y(n-1) < Y(n) 
    Xi = [Xi, n];
  end
end

% Perform parabolic interpolation of peaks
Yi = zeros( size( Xi ) );
for n = 1:length(Xi)
  alpha = Y(Xi(n)-1);
  beta = Y(Xi(n));
  gamma = Y(Xi(n)+1);
  Xii = 0.5*(alpha-gamma) / (alpha - 2*beta + gamma); % interpolated bin value (-1/2 to +1/2)
  Yi(n) = beta - 0.25 * (alpha-gamma) * Xii; % interpolated peak value
  if Xii > 0
    Xi(n) = f(Xi(n)) + Xii*(f(Xi(n)+1)-f(Xi(n))); % bin value in Hz
  else
    Xi(n) = f(Xi(n)) + Xii*(f(Xi(n))-f(Xi(n)-1)); % bin value in Hz
  end
  a = 0.5*(alpha - 2*beta + gamma);
  b = Yi(n);
  Yparab = a * (f - Xi(n)).^2 + b;
  hold on
  plot(f, 20*log10(abs(Yparab)))
  xlim([985 1015])
  
  % We can also try to estimate the -3dB bandwidth from the parabola,
  % though the parabolic fit it likely not very good so far from the center
  % frequency. Using the formula of the parabola, y(x) = a*(x-p)^2 + b,
  % with the center point p = 0, y(x) = 10^(-3/20)*Yi and b = Yi, find x.
  Y3dB = 20*log10(abs(Yi(n))) - 3;
  xB = sqrt(Yi*(10^(-3/20)-1)/a);
  plot(Xi(n)-xB, Y3dB, 'o');
  plot(Xi(n)+xB, Y3dB, 'o');
  text(Xi(n)-xB, Y3dB, '  Y = -3dB');
  text(Xi(n)+xB, Y3dB, '  Y = -3dB');
  hold off
end

fprintf('The estimated center frequency is %3.2f. The specified center frequency was %3.2f.\n', Xi, f0);
fprintf('The estimated 3dB bandwidth is %3.2f. The specified 3dB bandwidth was %3.2f.\n', 2*xB, B);