Core-audio, алгоритм Герцеля не работает

В настоящее время я создаю приложение, которое определяет величину на заранее заданной частоте (16780 Гц) в реальном времени с микрофона iPhone.

У меня есть звук. данные в буфере, и я пытаюсь обработать их с помощью Goertzel, алгоритм, разработанный для этой задачи. Информация Гертцеля . Здесь и начинается проблема.

Алгоритм дает очень положительные результаты, когда звук записывается с частотой, которая намного ниже (5000 Гц), чем заданная (16780 Гц). На самом деле результат намного более положительный, чем при записи звука правильной частоты.

Вот моя реализация goertzel:

double goertzel(unsigned short *sample, int sampleRate, double Freq, int len )
{

double realW = 2.0 * cos(2.0 * M_PI * Freq / sampleRate);
double imagW = 2.0 * sin(2.0 * M_PI * Freq / sampleRate);
double d1 = 0;
double d2 = 0;
int z;
double y;
for (int i = 0; i < len; i++) {
    y=(double)(signed short)sample[i] +realW * d1 - d2;
    d2 = d1;
    d1 = y;
}
double rR = 0.5 * realW *d1-d2;
double rI = 0.5 * imagW *d1-d2;

return (sqrt(pow(rR, 2)+pow(rI,2)))/len;
} /* end function goertzel */

Вот как я извлекаю звук, если он вообще актуален

-(void)startListeningWithFrequency:(float)frequency;
{
OSStatus status;
//AudioComponentInstance audioUnit;
AudioComponentDescription desc;
desc.componentType = kAudioUnitType_Output;
desc.componentSubType = kAudioUnitSubType_RemoteIO;
desc.componentFlags = 0;
desc.componentFlagsMask = 0;
desc.componentManufacturer = kAudioUnitManufacturer_Apple;

AudioComponent inputComponent = AudioComponentFindNext(NULL, &desc);
status = AudioComponentInstanceNew( inputComponent, &audioUnit);
checkStatus(status);

UInt32 flag = 1;
status = AudioUnitSetProperty(audioUnit, kAudioOutputUnitProperty_EnableIO, kAudioUnitScope_Input,kInputBus, &flag, sizeof(flag));
checkStatus(status);

AudioStreamBasicDescription audioFormat;
audioFormat.mSampleRate         = 44100.00;//44100.00;
audioFormat.mFormatID           = kAudioFormatLinearPCM;
audioFormat.mFormatFlags        = kAudioFormatFlagIsPacked | kAudioFormatFlagIsSignedInteger;
audioFormat.mFramesPerPacket    = 1;
audioFormat.mChannelsPerFrame   = 1;
audioFormat.mBitsPerChannel     = 16;
//  float
audioFormat.mBytesPerPacket     = 2;
audioFormat.mBytesPerFrame      = 2;

status = AudioUnitSetProperty(audioUnit,
                              kAudioUnitProperty_StreamFormat,
                              kAudioUnitScope_Output,
                              kInputBus,
                              &audioFormat, 
                              sizeof(audioFormat));
checkStatus(status);
//status = AudioUnitSetProperty(audioUnit, 
//                            kAudioUnitProperty_StreamFormat, 
//                            kAudioUnitScope_Input, 
//                            kOutputBus, 
//                            &audioFormat, 
//                            sizeof(audioFormat));
checkStatus(status);
AURenderCallbackStruct callbackStruct;
callbackStruct.inputProc = recordingCallback;
callbackStruct.inputProcRefCon = self;
status = AudioUnitSetProperty(audioUnit, 
                              kAudioOutputUnitProperty_SetInputCallback,
                              kAudioUnitScope_Global,
                              kInputBus, &callbackStruct, sizeof(callbackStruct));
checkStatus(status);
/*  UInt32 shouldAllocateBuffer = 1;
AudioUnitSetProperty(audioUnit, kAudioUnitProperty_ShouldAllocateBuffer, kAudioUnitScope_Global, 1, &shouldAllocateBuffer, sizeof(shouldAllocateBuffer));
*/
status = AudioOutputUnitStart(audioUnit);

}
static OSStatus recordingCallback(void *inRefCon, 
                              AudioUnitRenderActionFlags *ioActionFlags, 
                              const AudioTimeStamp *inTimeStamp, 
                              UInt32 inBusNumber, 
                              UInt32 inNumberFrames, 
                              AudioBufferList *ioData) {
AudioBuffer buffer;

buffer.mNumberChannels = 1;
buffer.mDataByteSize = inNumberFrames * 2;
//NSLog(@"%d",inNumberFrames);
buffer.mData = malloc( inNumberFrames * 2 );

// Put buffer in a AudioBufferList
AudioBufferList bufferList;
bufferList.mNumberBuffers = 1;
bufferList.mBuffers[0] = buffer;



OSStatus status;
status = AudioUnitRender(audioUnit, 
                         ioActionFlags, 
                         inTimeStamp, 
                         inBusNumber, 
                         inNumberFrames, 
                         &bufferList);  
checkStatus(status);
//double g = calculateGoertzel((const char *)(&bufferList)->mBuffers[0].mData,16789.0,96000.0);
UInt16 *q = (UInt16 *)(&bufferList)->mBuffers[0].mData;
int N = sizeof(q)/sizeof(UInt16);
double Qr,Qi;
double theta = 2.0*M_PI*16780/44100;
double g = goertzel(q,44100,16780,N);

NSLog(@"goertzel:%f", g);
}

Это возвращает числа в сотнях для частоты намного ниже 16780 Гц, а для частот 16780 Гц возвращает гораздо меньшие числа.

Я очень разочарован, и помощь будет очень признательна.