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.

If you have a good Matlab implementation and a bad C implementation, you can add logging to the Matlab version to calculate the values of intermediate variables in the loop, and then compare them against the equivalents from the C version. — Oliver Charlesworth, Jul 20 '12 at 13:49
Hi, this solution certainly works, anyway the real problem with my implementation was that the value was not getting returned correctly as my IDE had linked the files incorrectly .... thanks for your help ... — anshu, Jul 24 '12 at 04:26

score 16 · Accepted Answer · answered Jul 20 '12 at 14:20

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;
}

Hi, this solution certainly works, anyway the real problem with my implementation was that the value was not getting returned correctly as my IDE had linked the files incorrectly .... thanks for your help .... — anshu, Jul 24 '12 at 04:26

score 1 · Answer 2 · answered Oct 08 '13 at 20:14

1

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

answered Oct 08 '13 at 20:14

Wadester

343
2
5

score 1 · Answer 3 · answered Dec 31 '16 at 11:11

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;
}

Hmm I keep seeing this magnitude but I'm not sure how that returns the frequency? Is there a formula to calculate the frequency (in Hz) using the magnitude? — developer01, Mar 25 '22 at 22:55

Implementation of Goertzel algorithm in C

3 Answers3

Linked