我正在 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 结果，那么我会得到完美的结果。在网上找到的一些在线教程的帮助下，我使用 C 实现了算法。如果该函数正确地实现了 goertzel 算法，我只想了解你们的看法。

谢谢，安舒