最近在做一个声音测量距离的系统,平台为Android,要用广义互相关来求信号的时延。
里面用到了FFT,FFT是参考这位仁兄的:
http://blog.csdn.net/syrchina/article/details/6670517
如何通过FFT求信号时延看这里:
http://zlgc.seu.edu.cn/jpkc2/ipkc/signal/new/course/two/2-6-11.htm
废话不多说了:上代码,下面的是FFT的
public class FFT {
private int N_FFT=0; //傅里叶变换的点数
private int M_of_N_FFT=0; //蝶形运算的级数,N = 2^M
private int Npart2_of_N_FFT=0; //创建正弦函数表时取PI的1/2
private int Npart4_of_N_FFT=0; //创建正弦函数表时取PI的1/4
private float SIN_TABLE_of_N_FFT [] = null;
private static final double PI = 3.14159265358979323846264338327950288419716939937510;
public FFT(int FFT_N)
{
Init_FFT(FFT_N);
}
public Complex[] complexLization(float data[])
{
Complex[] w = new Complex[data.length];
for(int i=0;i<data.length;i++)
{
w[i].real = data[i];
w[i].imag = 0.0f;
}
return w;
}
public float[] magnitude(Complex[] data)
{
float[] r = new float[data.length];
for(int i=0;i<data.length;i++)
{
r[i] = (float) Math.sqrt(data[i].real * data[i].real + data[i].imag * data[i].imag);
}
return r;
}
//初始化FFT程序
//N_FFT是 FFT的点数,必须是2的次方
private void Init_FFT(int N_of_FFT)
{
int i=0;
int temp_N_FFT=1;
N_FFT = N_of_FFT; //傅里叶变换的点数 ,必须是 2的次方
M_of_N_FFT = 0; //蝶形运算的级数,N = 2^M
for (i=0; temp_N_FFT<N_FFT; i++)
{
temp_N_FFT = 2*temp_N_FFT;
M_of_N_FFT++;
}
//printf("\n%d\n",M_of_N_FFT);
Npart2_of_N_FFT = N_FFT/2; //创建正弦函数表时取PI的1/2
Npart4_of_N_FFT = N_FFT/4; //创建正弦函数表时取PI的1/4
//data_of_N_FFT = (ptr_complex_of_N_FFT)malloc(N_FFT * sizeof(complex_of_N_FFT));
//data_of_N_FFT =
//ptr_complex_of_N_FFT SIN_TABLE_of_N_FFT=NULL;
//SIN_TABLE_of_N_FFT = (ElemType *)malloc((Npart4_of_N_FFT+1) * sizeof(ElemType));
SIN_TABLE_of_N_FFT = new float[Npart4_of_N_FFT+1];
CREATE_SIN_TABLE(); //创建正弦函数表
}
//创建正弦函数表
private void CREATE_SIN_TABLE()
{
int i=0;
for (i=0; i<=Npart4_of_N_FFT; i++)
{
SIN_TABLE_of_N_FFT[i] = (float) Math.sin(PI*i/Npart2_of_N_FFT);//SIN_TABLE[i] = sin(PI2*i/N);
}
}
private float Sin_find(float x)
{
int i = (int)(N_FFT*x);
i = i>>1;
if (i>Npart4_of_N_FFT)//注意:i已经转化为0~N之间的整数了!
{
//不会超过N/2
i = Npart2_of_N_FFT - i;//i = i - 2*(i-Npart4);
}
return SIN_TABLE_of_N_FFT[i];
}
private float Cos_find(float x)
{
int i = (int)(N_FFT*x);
i = i>>1;
if (i<Npart4_of_N_FFT)//注意:i已经转化为0~N之间的整数了!
{
//不会超过N/2
//i = Npart4 - i;
return SIN_TABLE_of_N_FFT[Npart4_of_N_FFT - i];
}
else//i>Npart4 && i<N/2
{
//i = i - Npart4;
return -SIN_TABLE_of_N_FFT[i - Npart4_of_N_FFT];
}
}
private void ChangeSeat(Complex DataInput[])
{
int nextValue,nextM,i,k,j=0;
Complex temp;
nextValue=N_FFT/2; //变址运算,即把自然顺序变成倒位序,采用雷德算法
nextM=N_FFT-1;
for (i=0;i<nextM;i++)
{
if (i<j) //如果i<j,即进行变址
{
temp=DataInput[j];
DataInput[j]=DataInput[i];
DataInput[i]=temp;
}
k=nextValue; //求j的下一个倒位序
while (k<=j) //如果k<=j,表示j的最高位为1
{
j=j-k; //把最高位变成0
k=k/2; //k/2,比较次高位,依次类推,逐个比较,直到某个位为0
}
j=j+k; //把0改为1
}
}
//FFT运算函数
public void FFT(Complex []data)
{
int L=0,B=0,J=0,K=0;
int step=0, KB=0;
//ElemType P=0;
float angle;
Complex W = new Complex();
Complex Temp_XX = new Complex();
ChangeSeat(data);//变址
//CREATE_SIN_TABLE();
for (L=1; L<=M_of_N_FFT; L++)
{
step = 1<<L;//2^L
B = step>>1;//B=2^(L-1)
for (J=0; J<B; J++)
{
//P = (1<<(M-L))*J;//P=2^(M-L) *J
angle = (float)J/B; //这里还可以优化
W.imag = -Sin_find(angle); //用C++该函数课声明为inline
W.real = Cos_find(angle); //用C++该函数课声明为inline
//W.real = cos(angle*PI);
//W.imag = -sin(angle*PI);
for (K=J; K<N_FFT; K=K+step)
{
KB = K + B;
//Temp_XX = XX_complex(data[KB],W);
//用下面两行直接计算复数乘法,省去函数调用开销
Temp_XX.real = data[KB].real * W.real-data[KB].imag*W.imag;
Temp_XX.imag = W.imag*data[KB].real + data[KB].imag*W.real;
data[KB].real = data[K].real - Temp_XX.real;
data[KB].imag = data[K].imag - Temp_XX.imag;
data[K].real = data[K].real + Temp_XX.real;
data[K].imag = data[K].imag + Temp_XX.imag;
}
}
}
}
//IFFT运算函数
public void IFFT(Complex []data)
{
int L=0,B=0,J=0,K=0;
int step=0, KB=0;
//ElemType P=0;
float angle=0.0f;
Complex W = new Complex();
Complex Temp_XX = new Complex();
ChangeSeat(data);//变址
//CREATE_SIN_TABLE();
for (L=1; L<=M_of_N_FFT; L++)
{
step = 1<<L;//2^L
B = step>>1;//B=2^(L-1)
for (J=0; J<B; J++)
{
//P = (1<<(M-L))*J;//P=2^(M-L) *J
angle = (float)J/B; //这里还可以优化
W.imag = Sin_find(angle); //用C++该函数课声明为inline
W.real = Cos_find(angle); //用C++该函数课声明为inline
//W.real = cos(angle*PI);
//W.imag = -sin(angle*PI);
for (K=J; K<N_FFT; K=K+step)
{
KB = K + B;
//Temp_XX = XX_complex(data[KB],W);
//用下面两行直接计算复数乘法,省去函数调用开销
Temp_XX.real = data[KB].real * W.real-data[KB].imag*W.imag;
Temp_XX.imag = W.imag*data[KB].real + data[KB].imag*W.real;
data[KB].real = data[K].real - Temp_XX.real;
data[KB].imag = data[K].imag - Temp_XX.imag;
data[K].real = data[K].real + Temp_XX.real;
data[K].imag = data[K].imag + Temp_XX.imag;
}
}
}
}
}
下面的是一个辅助类:
public class Complex {
public float real;
public float imag;
public Complex()
{
this.real = 0;
this.imag = 0;
}
}
下面是自相关求解过程源码:
public class Correlation {
private Complex[] s1 = null;
private Complex[] s2 = null;
private int lag = 0;
public Correlation(float sig1[] , float sig2[])
{
int maxLen = sig1.length > sig2.length ? sig1.length : sig2.length;
maxLen = (int) (Math.log(maxLen)/Math.log(2.0)) + 1;//求出FFT的幂次
maxLen = (int) Math.pow(2, maxLen);
s1 = new Complex[maxLen];
s2 = new Complex[maxLen];
for(int i=0;i<maxLen;i++)
{
//这一步已经完成了补零的工作了
s1[i] = new Complex();
s2[i] = new Complex();
}
for(int i=0;i<sig1.length;i++)
{
s1[i].real = sig1[i];
//System.out.println(s1[i].real);
}
for(int i=0;i<sig2.length;i++)
{
s2[i].real = sig2[i];
//System.out.println(s2[i].real);
}
//求出信号的FFT
float[] rr = new float[maxLen];
FFT fft = new FFT(maxLen);
fft.FFT(s1);
fft.FFT(s2);
conj(s2);
mul(s1,s2);
fft.IFFT(s1);
float max = s1[maxLen-1].real;
for(int i=0;i>maxLen;i++)
{
if(s1[i].real > max)
{
lag = i;
max = s1[i].real;
}
//System.out.println(s1[i].real);
}
}
public int getLag()
{
return lag;
}
//求两个复数的乘法,结果返回到第一个输入
public void mul(Complex[] s1,Complex[] s2)
{
float temp11=0,temp12=0;
float temp21=0,temp22=0;
for(int i=0;i<s1.length;i++)
{
temp11 = s1[i].real ; temp12 = s1[i].imag;
temp21 = s2[i].real ; temp22 = s2[i].imag;
s1[i].real = temp11 * temp21 - temp12 * temp22;
s1[i].imag = temp11 * temp22 + temp21 * temp12;
//s1[i].real = s1[i].real * s2[i].real - s1[i].imag * s2[i].imag;
//s1[i].imag = s1[i].real * s2[i].imag + s1[i].imag * s2[i].real;
}
}
//求信号的共轭
public void conj(Complex s[])
{
for(int i=0;i<s.length;i++)
{
s[i].imag = 0.0f - s[i].imag;
}
}
}
下面的是测试代码:
public class Test {
public static void main(String args[])
{
//fft的测试函数代码
/*int f0 = 10;
int fs = 100;
int FFT_LENGHT = 512;
float[] testdata = new float[FFT_LENGHT];
Complex[] r = new Complex[FFT_LENGHT*2];
for(int i=0;i<r.length;i++)
{
r[i] = new Complex();
}
FFT fft = new FFT(FFT_LENGHT*2);
for(int i=0;i<FFT_LENGHT;i++)
{
r[i].real = (float) Math.sin(2*3.1415926*i*f0/fs);
r[i].imag = 0.0f;
//testdata[i] = (float) Math.sin(2*3.1415926*i*f0/fs);
}
//r = fft.complexLization(testdata);
fft.FFT(r);
testdata = fft.magnitude(r);
for(int i=0;i<FFT_LENGHT;i++)
{
System.out.println(testdata[i]);
}*/
//测试互相关函数
int f0=10;
int fs=150;
int K = 50;
int lag = 50;
int dataLength = 16;
float[] s1 = new float[dataLength];
float[] s2 = new float[dataLength*2];
for(int i=0;i<dataLength;i++)
{
//s1[i] = (float) Math.sin(2*3.14*(i*2.0/K/fs+f0)*i/fs);
s1[i] = (float) Math.sin(2*3.14*f0*i/fs);
//System.out.println(s1[i]);
}
for(int i=0;i<dataLength*1;i++)
{
s2[i] = 0;
}
for(int i=dataLength*1;i<dataLength*2;i++)
{
s2[i] = s1[i-dataLength*1];
}
Correlation correlation = new Correlation(s1, s2);
//System.out.println("结果:"+correlation.getLag());
}
}
再来个对应结果的图吧:
从图从左往右数,最高的刚好是第16个点,再看看测试代码里面的两个信号就知道刚好延迟了一个信号周期,刚好16个点,结果太完美了。
先写到这里,后面有 成果了再上传其它的代码。