Implementation of Goertzel algorithm in C

Question

I am implementing BFSK frequency hopping communication system on a DSP processor. It was suggested by some of the forum members to use Goertzel algorithm for the demodulation of frequency hopping at specific frequencies. I have tried implementing the goertzel algorithm in C. the code is follows:

float goertzel(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,result,real,imag;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * M_PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }
    real = (q1 - q2 * cosine);
    imag = (q2 * sine);
    result = sqrtf(real*real + imag*imag);
    return result;
}

When I use the function to calculate the result at specific frequencies for a given dataset, I am not getting the correct results. However, if I use the same dataset and calculate the goertzel result using MATLAB goertzel() function, then I get the results perfectly. I am implemented the algorithm using C, with the help of some online tutorials that I found over the internet. I just want to get the view of you guys if the function is implementing the goertzel algorithm correctly.

Answer 1

If you are saying that the Matlab implementation is good because its results match the result for that frequency of a DFT or FFT of your data, then it's probably because the Matlab implementation is normalizing the results by a scaling factor as is done with the FFT.

Change your code to take this into account and see if it improves your results. Note that I also changed the function and result names to reflect that your goertzel is calculating the magnitude, not the complete complex result, for clarity:

float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;

    float   scalingFactor = numSamples / 2.0;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * M_PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q1 - q2 * cosine) / scalingFactor;
    imag = (q2 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    return magnitude;
}

Answer 2

Often you can just use the square of the magnitude in your computations, for example for tone detection.

Some excellent examples of Goertzels are in the Asterisk PBX DSP code Asterisk DSP code (dsp.c) and in the spandsp library SPANDSP DSP Library

Answer 3

Consider two input sample wave-forms:

1) a sine wave with amplitude A and frequency W

2) a cosine wave with the same amplitude and frequency A and W

Goertzel algorithm should yield the same results for two mentioned input wave-forms but the provided code results in different return values. I think the code should be revised as follows:

float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;

    float   scalingFactor = numSamples / 2.0;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * M_PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q2 = q1;
        q1 = q0;
        q0 = coeff * q1 - q2 + data[i];
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q0 - q1 * cosine) / scalingFactor;
    imag = (-q1 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    return magnitude;
}