ARM DSP 库下使用 FFT 实现自相关

使用 FFT 实现自相关

通用的自相关算法

void MyXcorr(float* input, unsigned int len, float* output)
{
  unsigned int i, j;
  double sum;
  for(i = 0; i < 2 * len; i++)
  {
    sum = 0;
    for(j = 0; j < len; j++)
    {
      sum = sum + input[j] * input[j + i];
    }
    sum = sum / len;
    output[i] = sum;
  }
}

使用 FFT 实现的自相关算法

//注意：最大支持2048个点
void XcorrWithFFT_f32(float* a, unsigned int aLen, float* b, unsigned int bLen, float* output)
{
#if 1 //no flip, maybe with a bug, but faster
  static arm_cfft_instance_f32 s_structCFFTF32;
  float32_t* aSource = NULL;
  float32_t* bSource = NULL;
  float32_t* cMul    = NULL;
  unsigned int totalLen, i;
  
  totalLen = aLen + bLen;
  
  //malloc
  aSource = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  bSource = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  cMul = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  if((NULL == aSource) || (NULL == bSource) || (NULL == cMul))
  {
    printf("XcorrWithFFT: fail to malloc\r\n");
    while(1){}
  }
  
  //copy and corver with zero for A
  for(i = 0; i < aLen; i++){aSource[(2 * i) + 0] = a[i]; aSource[(2 * i) + 1] = 0;}
  while(i < totalLen){aSource[(2 * i) + 0] = 0;aSource[(2 * i) + 1] = 0;i++;}
  
  //copy and corver with zero for B
  for(i = 0; i < bLen; i++){bSource[(2 * i) + 0] = b[i]; bSource[(2 * i) + 1] = 0;}
  while(i < totalLen){bSource[(2 * i) + 0] = 0;bSource[(2 * i) + 1] = 0;i++;}
  
  //A FFT
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, aSource, 0, 1);
  
  //B FFT
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, bSource, 0, 1);
  
  //MUX
  arm_cmplx_mult_cmplx_f32(aSource, bSource, cMul, totalLen);
  
  //IFFT
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, cMul, 1, 1);
  
  //output, only real part, the imaginary part is always zero
  //arm_cmplx_mag_f32(cMul, output, totalLen);
  for(i = 0; i < totalLen; i++){output[i] = cMul[(2 * i) + 0];}
  
  //free
  MyFree(aSource);
  MyFree(bSource);
  MyFree(cMul);

#else //with flip
  static arm_cfft_instance_f32 s_structCFFTF32;
  float32_t* aSource = NULL;
  float32_t* bSource = NULL;
  float32_t* bConj   = NULL;
  float32_t* cMul    = NULL;
  unsigned int totalLen, i;
  
  totalLen = aLen + bLen;
  
  //malloc
  aSource = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  bSource = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  bConj = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  cMul = MyMalloc(SRAMIN, 2 * totalLen * sizeof(float32_t));
  if((NULL == aSource) || (NULL == bSource) || (NULL == bConj) || (NULL == cMul))
  {
    printf("XcorrWithFFT: fail to malloc\r\n");
    while(1){}
  }
  
  //copy and corver with zero for A
  for(i = 0; i < aLen; i++){aSource[(2 * i) + 0] = a[i]; aSource[(2 * i) + 1] = 0;}
  while(i < totalLen){aSource[(2 * i) + 0] = 0;aSource[(2 * i) + 1] = 0;i++;}
  
  //copy and corver with zero for B, and flip B
  for(i = 0; i < bLen; i++){bSource[(2 * i) + 0] = b[i]; bSource[(2 * i) + 1] = 0;}
  while(i < totalLen){bSource[(2 * i) + 0] = 0;bSource[(2 * i) + 1] = 0;i++;}
  
  //A FFT
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, aSource, 0, 1);
  
  //B FFT and conj
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, bSource, 0, 1);
  arm_cmplx_conj_f32(bSource, bConj, totalLen);
  
  //MUX
  arm_cmplx_mult_cmplx_f32(aSource, bConj, cMul, totalLen);
  
  //IFFT
  arm_cfft_init_f32(&s_structCFFTF32, totalLen);
  arm_cfft_f32(&s_structCFFTF32, cMul, 1, 1);
  
  //output, only real part, zhe imaginary part is always zero
  for(i = 0; i < totalLen; i++){output[(i + (totalLen / 2)) % totalLen] = cMul[(2 * i) + 0];}
  
  //free
  MyFree(aSource);
  MyFree(bSource);
  MyFree(bConj);
  MyFree(cMul);
#endif
}

hrz