c - C での Goertzel アルゴリズムの実装

Question

DSP プロセッサに BFSK 周波数ホッピング通信システムを実装しています。一部のフォーラム メンバーから、特定の周波数での周波数ホッピングの復調に Goertzel アルゴリズムを使用することが提案されました。私は、C で goertzel アルゴリズムを実装しようとしました。コードは次のとおりです。

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

関数を使用して特定のデータセットの特定の頻度で結果を計算すると、正しい結果が得られません。ただし、同じデータセットを使用し、MATLAB goertzel() 関数を使用して goertzel の結果を計算すると、完全に結果が得られます。インターネットで見つけたいくつかのオンラインチュートリアルの助けを借りて、C を使用してアルゴリズムを実装しています。関数が goertzel アルゴリズムを正しく実装しているかどうか、皆さんの意見を聞きたいだけです。