c++ - mbed の相互相関

Question

相互相関を取得するのに問題があります。誰かが私の間違いを教えてもらえますか?

人が話している角度を取得するために、遅延を取得しようとしています。sparkfun microphone私のプロジェクトは、との 2 ピースを使用したサウンド ローカリゼーションに基づいていlpc1768ます。

#include "mbed.h"
#include "math.h"

#define SAMPLE_PERIOD 60
#define SAMPLE 10
#define PI  3.1416
#define sos 34300 //cm/s
#define SAMPLEDELAY 20

Serial pc(USBTX, USBRX);
DigitalOut myled(LED1);
AnalogIn  rightmic(p16);
AnalogIn  leftmic(p19);
BusOut    unused(p15,p17,p18,p20); 
Timer t,t1;

float max1=0.0; 
double max;
double min;
float min1 = 3.3;
float left_results[SAMPLE];
float right_results[SAMPLE];
int k,ndelays,delay;
int Max=0;  
int indexMax=0;
int delayMax;
int count;

void correlate(float left_results[] , float right_results[])
{
        ndelays=2*SAMPLEDELAY+1;
        float correlation[ndelays];
        delayMax=2*SAMPLEDELAY;
        for(int j=0;j<ndelays;j++)
        {
            correlation[j]=0;
            delay=-SAMPLEDELAY+j;
            for(int i=0;i<SAMPLE;i++)
            {
                k=i+delay;
                if(k<0) //negative
                    k+=SAMPLE; 
                if(k>(SAMPLE-1)) //positive
                    k-=SAMPLE;
                correlation[j]+=(left_results[i]*right_results[i]);
            }
            if(correlation[j]>Max)
            { 
                Max=correlation[j];
                indexMax=j;
                delayMax=delay;
            }
            pc.printf("Correlation[%i]=%f\n\r>",j,correlation[j]);
            pc.printf("Max=%f\n\r>",Max);
            pc.printf("delayMax=%d\n\r>",delayMax);
        }
         pc.printf("Correlation: delay=%d\n\r>",delayMax); 
}
int main() 
{   
    max = 0.0;
    min = 3.3;
    t.start();
    for (int i=0; i<SAMPLE; i++)
    {
        while(t.read_ms()<i*SAMPLE_PERIOD)// wait until the next sample time....
        {
            left_results[i]=leftmic.read();
            right_results[i] = rightmic.read();

            if (right_results[i] > max) 
                max = right_results[i];
            if (right_results[i] < min) 
                min = right_results[i];
            if (left_results[i] > max) 
                max = left_results[i];
            if (left_results[i] < min) 
                min = left_results[i];        
        }

    t.reset();
    max=max*3.3;
    min=min*3.3;
    pc.printf(" max=%0.3f, min=%0.3f\n\r",max,min);
    max=0.0;
    min=3.3;
    } 
    correlate(left_results , right_results);
}