FFT Frequency Estimation Using Barry Quinn's Second Estimator (C++)

quinnsecond.cpp

//
//  Frequency estimation using Barry Quinn's Second Estimator
//  Adapted from following links:
//  DSP Guru site:
//  http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak
//  Vadym Markov's adaptation:
//  https://github.com/vadymmarkov/Beethoven/blob/master/Source/Estimation/Strategies/QuinnsSecondEstimator.swift
//
//  sampleRate ... your audio sample rate
//  k          ... bin index that shows biggest FFT output magnitude
//  N          ... number of FFT points
//  out[k][0]  ... real part of FFT output at bin k
//  out[k][1]  ... imaginary part of FFT output at bin k

// ...
// perform complex FFT here...

int k = peakPosIndex;		
double divider = pow(out[k][0], 2.0) + pow(out[k][1], 2.0);
double ap = (out[k+1][0] * out[k][0] + out[k+1][1] * out[k][1]) / divider;
double dp = -ap  / (1.0 - ap);
double am = (out[k-1][0] * out[k][0] + out[k-1][1] * out[k][1]) / divider;

double dm = am / (1.0 - am);
double d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);

double adjustedBinLocation = (double)k + d;
double peakFreqAdjusted = (sampleRate * adjustedBinLocation /(double)N );

// peakFreqAdjusted holds the peak frequency you want
// ...

// helper function used for frequency estimation		
double tau(double x)
{
	double p1 = log(3 * pow(x, 2.0) + 6 * x + 1);
	double part1 = x + 1 - sqrt(2.0/3.0);
	double part2 = x + 1 + sqrt(2.0/3.0);
	double p2 = log(part1 / part2);	
	return (1.0/4.0 * p1 - sqrt(6) / 24 * p2);
}