(Prog) Расчет FIR-фильтра через ДПФ метод Overlap-Add (Си)

Описание

Расчет FIR-фильтра (КИХ-фильтра) с использованием теоремы о свертке. Метод использует разбиение длинного сигнала на маленькие части (той же длины что и малая последовательность). Затем от каждого сигнала находится спектр сигнала (ДПФ/DFT), затем спектры умножаются и результат опять возвращается во временную область (ОДПФ/IDFT). Полученные малые кусочки результирующего сигнала соединяются по методу Overlap-Add, для формирования окончательного ответа.

Исходник на C

#define _USE_MATH_DEFINES

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#define NL 1000
#define TRUE 1
#define FALSE 0

/*
  Discrete Fourier Transform
*/

int dft(long int length, double real_sample[], double imag_sample[])
{
  long int i, j;
  double arg;
  double cosarg,sinarg;
  double *temp_real=NULL,*temp_imag=NULL;

  temp_real = (double *)calloc(length, sizeof(double));
  temp_imag = (double *)calloc(length, sizeof(double));
  if (temp_real == NULL || temp_imag == NULL)
  {
    return(FALSE);
  }

  for(i=0; i<length; i+=1) 
  {
    temp_real[i] = 0;
    temp_imag[i] = 0;
    arg = -1.0 * 2.0 * M_PI * (double)i / (double)length;
    for(j=0; j<length; j+=1) 
    {
      cosarg = cos(j * arg);
      sinarg = sin(j * arg);
      temp_real[i] += (real_sample[j] * cosarg - imag_sample[j] * sinarg);
      temp_imag[i] += (real_sample[j] * sinarg + imag_sample[j] * cosarg);
    }
  }

  /* Copy the data back */
  for (i=0; i<length; i+=1) 
  {
    real_sample[i] = temp_real[i];
    imag_sample[i] = temp_imag[i];
  }

  free(temp_real);
  free(temp_imag);
  return(TRUE);
}


/*
  Inverse Discrete Fourier Transform
*/

int inverse_dft(long int length, double real_sample[], double imag_sample[])
{
  long int i, j;
  double arg;
  double cosarg,sinarg;
  double *temp_real=NULL,*temp_imag=NULL;

  temp_real = (double *)calloc(length, sizeof(double));
  temp_imag = (double *)calloc(length, sizeof(double));
  if (temp_real == NULL || temp_imag == NULL)
  {
    return(FALSE);
  }

  for(i=0; i<length; i+=1) 
  {
    temp_real[i] = 0;
    temp_imag[i] = 0;
    arg = 2.0 * M_PI * (double)i / (double)length;
    for(j=0; j<length; j+=1) 
    {
      cosarg = cos(j * arg);
      sinarg = sin(j * arg);
      temp_real[i] += (real_sample[j] * cosarg - imag_sample[j] * sinarg);
      temp_imag[i] += (real_sample[j] * sinarg + imag_sample[j] * cosarg);
    }
  }

  /* Copy the data back */
  for (i=0; i<length; i+=1) 
  {
    real_sample[i] = temp_real[i] / (double)length;
    imag_sample[i] = temp_imag[i] / (double)length;
  }

  free(temp_real);
  free(temp_imag);
  return(TRUE);
}

/*
	Вычисляет свертку двух последовательностей (примерно одинаковой длины) используя ДПФ
	Вместо ДПФ можно использовать один из БПФ алгоритмов
	Длина выхода N = (len1+len2-1)
*/

int calc_simple_conv (
	double *in1, 
	int len1, 
	double *in2, 
	int len2, 
	double *out)
{
	int i, N;
	double *xRe;
	double *xIm;
	double *yRe;
	double *yIm;
	double *rRe;
	double *rIm;

	N = len1+len2-1;
	xRe = (double *)calloc(N, sizeof(double));
	xIm = (double *)calloc(N, sizeof(double));
	yRe = (double *)calloc(N, sizeof(double));
	yIm = (double *)calloc(N, sizeof(double));
	rRe = (double *)calloc(N, sizeof(double));
	rIm = (double *)calloc(N, sizeof(double));

	for (i = 0; i < len1; i++) {
		xRe[i] = (double)in1[i];
		xIm[i] = 0.0;
	}
	for (i = len1; i < N; i++) {
		xRe[i] = 0.0;
		xIm[i] = 0.0;
	}
	for (i = 0; i < len2; i++) {
		yRe[i] = (double)in2[i];
		yIm[i] = 0.0;
	}
	for (i = len2; i < N; i++) {
		yRe[i] = 0.0;
		yIm[i] = 0.0;
	}

	dft(N, xRe, xIm);
	dft(N, yRe, yIm);

	for (i = 0; i < N; i++)
	{
		rRe[i] = xRe[i]*yRe[i] - xIm[i]*yIm[i];
		rIm[i] = xIm[i]*yRe[i] + xRe[i]*yIm[i];
	}

	inverse_dft(N, rRe, rIm);

	for (i = 0; i < N; i++) {
		out[i] = rRe[i];
	}

	free(xRe);
	free(xIm);
	free(yRe);
	free(yIm);
	free(rRe);
	free(rIm);
	return 0;
}

/*
	Простейший подход - действуем по определению свертки
*/

int calc_fir_filter_deafult(
	int *vec1,
	int len1,
	int *vec2,
	int len2,
	int *out) 
{
	int i, j;
	for (i = 0; i < len1+len2-1; i++) {
		out[i] = 0;
		for (j = (i-len2+1 > 0)?(i-len2+1):0; j <= i; j++) {
			if (j >= len1)
				break;
			out[i] += vec1[j]*vec2[i-j];
		}
	}
	return 0;
}

/*
	Подход через ДПФ в случае когда один массив мал, а второй очень большой или бесконечный (типичный случай КИХ-фильтра (FIR-filter)).
	Конвеерная реализация по методу overlap-add.
	http://en.wikipedia.org/wiki/Overlap%E2%80%93add_method

	vec1 - маленький массив
	vec2 - большой массив
*/

int calc_fir_filter_dft_overlap_add(
	int *vec1,
	int len1,
	int *vec2,
	int len2,
	int *out) 
{
	int i, j, N;
	double *xRe;
	double *yRe;
	double *rRe;
	double *rTmpRe;

	if (len1 > len2) {
		printf("Error: len1 > len2\n");
		exit(0);
	}

	N = len1+len2-1;
	xRe = (double *)calloc(len1, sizeof(double));
	yRe = (double *)calloc(len1, sizeof(double));
	rTmpRe = (double *)calloc(len1+len1-1, sizeof(double));
	rRe = (double *)calloc(N, sizeof(double));

	for (i = 0; i < len1; i++) {
		xRe[i] = (double)vec1[i];
	}
	for (i = 0; i < N; i++) {
		rRe[i] = 0.0;
	}

	for (i = 0; i < len2; i += len1)
	{
		for (j = 0; j < len1; j++) {
			yRe[j] = (double)vec2[i+j];
		}
		calc_simple_conv(xRe, len1, yRe, len1, rTmpRe);
		for (j = 0; j < len1+len1-1; j++) {
			rRe[i+j] += rTmpRe[j];
		}
	}

	printf("Result vector: (");
	for (i = 0; i < N; i++) {
		if (i > 0)
			printf(", ");
		printf("%.0lf", rRe[i]);
		out[i] = (int)(rRe[i]+ 0.5);
	}
	printf(")\n");

	free(xRe);
	free(yRe);
	free(rRe);
	free(rTmpRe);
	return 0;
}

int main ()
{
	int i, z;
	double maxVal;

	int filter[NL];
	int filterLen;
	int testArray[100*NL];
	int arrayLen;
	int dataBit;
	int res_check[10*NL];
	int res_experimental[10*NL];

	freopen("input.txt", "r", stdin);
	
	scanf("%d", &z);
	while(z--) {
		scanf("%d %d %d", &dataBit, &filterLen, &arrayLen);
		for (i = 0; i < filterLen; i++) {
			scanf("%d", &(filter[i]));
		}
		for (i = 0; i < arrayLen; i++) {
			scanf("%d", &(testArray[i]));
		}
	}
	
	calc_fir_filter_deafult(filter, filterLen, testArray, arrayLen, res_check);
	calc_fir_filter_dft_overlap_add(filter, filterLen, testArray, arrayLen, res_experimental);
	
	// Check correctness
	for (i = 0; i < filterLen+arrayLen-1; i++) {
		if (res_check[i] != res_experimental[i]) {
			printf("Some error at position %d\n", i);
			break;
		}
	}
	if (i == filterLen+arrayLen-1) {
		printf("Congratulations! No errors were found at calcualtions!\n");
	} else {
		printf("Some errors were found at calcualtions!\n");
	}

	return 0;
}

Входные/выходные данные

Формат входных/выходных данных для FIR-фильтра

Пример (входные данные)

1
4 4 8
3 0 1 2
4 3 5 1 5 6 3 2

Пример (выходные данные)

Result vector: (12, 9, 19, 14, 26, 29, 16, 22, 15, 8, 4)
Congratulations! No errors were found at calcualtions!