FIR filter lib by windowing (LOW_PASS, HIGH_PASS, BAND_PASS, BAND_STOP)

filters.c

/*
* File Name		: filter.c
* Authors		: Trung Kien - letrungkien.k53.hut@gmail.com
* Version		: V 1.0
* Date			: 10 July 2015
* Description	: FIR filter by windowing.
*/

/*
* NOTE: use this command to build: 
* gcc -o filters filters.c -lm -lfftw3
*/

#include <stdio.h>
#include <malloc.h>
#include <fftw3.h>
#include <complex.h>
#include <math.h>

/*
*****************************************************************************
* DEFINES
*****************************************************************************
*/

#define FIR_OLDER 21
#define CLEAN 0
#define MAX 10000

/*
*****************************************************************************
* VARIABLES
*****************************************************************************
*/

enum filterType{ LOW_PASS, HIGH_PASS, BAND_PASS, BAND_STOP};
enum windowType{ RECTANGULAR, BATLETT, HANNING, HAMMING, BLACKMAN};

/*
*****************************************************************************
* FUNCTIONS PROTOTYPE
*****************************************************************************
*/

double *create1TransSinc(int windowLength, double transFreq, double sampFreq, enum filterType type);
double *create2TransSinc(int windowLength, double trans1Freq, double trans2Freq, double sampFreq, enum filterType type);

double *createWindow(double *in, double *out, int windowLength, enum windowType type);

double *createKaiserWindow(double *in, double *out, int windowLength, double beta);
double modZeroBessel(double x);

int outputFFT(char *filename, double *window, int windowLength, double sampFreq);

double fir_filter(double rawData, double *firCoef, int firOrder, unsigned char bReset);

/*
*****************************************************************************
* FUNCTIONS IMPLEMENT
*****************************************************************************
*/

/*
 * Function	: create1TransSinc
 * Propose	: Create Sinc h(x) with 1 transition - an impulse response of LPF or HPF
 * Return	: Pointer to array store weight coefficient
 */
double *create1TransSinc(int windowLength, double transFreq, double sampFreq, enum filterType type)
{
	int n;
	/*	Allocate memory for the window 	*/
	double *window = (double *)malloc(windowLength * sizeof(double));
	if(window == NULL)
	{
		fprintf(stderr, "create1TransSinc: Could not alllocation memory for window\n");
		return NULL;
	}

	if(type != LOW_PASS && type != HIGH_PASS)
	{
		fprintf(stderr, "create1TransSinc: Bad filter type, should be LOW_PASS or HIGH_PASS \n");
		return NULL;
	}
	/* Calculate the normalised transistion frequency. As transFreq should be
	 * less than or equal to sampFreq / 2, ft should be less than 0.5 (as Nyquist theory)
	 */
	double ft = transFreq / sampFreq;
	double m_2 = 0.5 * (windowLength-1);
	int halfLength = windowLength / 2;

	 if(2*halfLength != windowLength)
	 {
	 	double val = 2.0*ft;
	 	if(type == HIGH_PASS)
	 		val = 1 - val;
	 	window[halfLength] = val;
	 }
	 else if (type == HIGH_PASS) {
		fprintf(stderr, "create1TransSinc: For high pass filter, window length must be odd\n");
		return NULL;
	}

	/* This has the effect of inverting all weight values*/
	if (type == HIGH_PASS) ft = -ft;
	
	/* Calculate weight coefficient*/
	for(n = 0; n < halfLength; n++)
	{
	 	double val = sin(2*M_PI*ft*(n - m_2)) / (M_PI * (n - m_2));
	 	window[n] = window[windowLength - n -1] = val;
	 }

	 return window;
}

/*
 * Function	: create2TransSinc
 * Propose	: Create Sinc h(x) with 2 transition - an impulse response of BandPass or BandStop
 * Return	: Pointer to array store weight coefficient
 */
double *create2TransSinc(int windowLength, double trans1Freq, double trans2Freq, double sampFreq, enum filterType type)
{
	int n;

	/* Allocation memory for window */
	double *window = (double*)malloc(windowLength * sizeof(double));
	if(window == NULL)
	{
		fprintf(stderr, "create2TransSinc: Could not alllocation memory for window\n");
		return NULL;
	}
	/* Check the available filter type */
	if(type != BAND_PASS && type != BAND_STOP)
	{
		fprintf(stderr, "create2TransSinc:  Bad filter type, should be BAND_PASS or BAND_STOP  \n");
		return NULL;
	}

	/* Calculate te transition frequencies */
	double ft1 = trans1Freq / sampFreq;
	double ft2 = trans2Freq / sampFreq;

	double m_2 = 0.5 * (windowLength - 1);
	int halfLength = windowLength / 2;

	if(2*halfLength != windowLength)
	{
		double val = 2.0 * (ft2 - ft1);
		if(type == BAND_STOP)
			val = 1 - val;
		window[halfLength] = val;
	}	 
	else
	{
		fprintf(stderr, "create2TransSinc: For BAND_STOP filter, window length must be odd\n");
		return NULL;
	}

	/* Invert value of transFreq for BAND_STOP*/
	if(type == BAND_STOP)
	{
		ft1 = -ft1; ft2 = -ft2;
	}

	/* Calculate weight coefficient*/
	for(n = 0; n < halfLength; n++)
	{
		double val = sin(2 * M_PI * ft2* (n-m_2)) / (M_PI * (n-m_2)) - sin(2 * M_PI * ft1 * (n-m_2)) / (M_PI * (n-m_2));
		window[n] = window[windowLength - n - 1] = val;
	}
	
	return window;
}

/*
 * Function	: createWindow
 * Propose	: Create windows for FIR
 * Return	: Pointer to array store coefficient of windows
 *
 * Window type support in this function as below:
 *
 * RECTANGULAR	: w(n) = 1;
 *
 * BATLETT		: w(n) = 1 - 2*(n -M/2)/ m
 *
 * HANNING		: w(n) = 0.5 - 0.5*cos(2*M_PI*n/M);
 *
 * HAMMING		: w(n) = 0.54 - 0.46*cos(2*M_PI*n/M);
 *
 * BACKMAN		: w(n) = 0.42 - 0.5*cos(2*M_PI*n/M) + 0.08*cos(2*M_PI*n/M);
 *
 * in 			- If not null, each value will be multiplied with the window weight
 * out 			- The output weight values, if NULL and new array will be allocated
 * windowLength - the number of weights
 * windowType 	- The window type
 */
double *createWindow(double *in, double *out, int windowLength, enum windowType type)
{
	/* If out is not allocated memory, let allocation for it*/
	if(out == NULL) 
	{
		out = (double*) malloc(windowLength * sizeof(double)) ;
		if(out == NULL)
		fprintf(stderr, "createWindow: Could not alllocation memory for window \n");
	}

	/*	*/
	
	int n;
	double m = windowLength - 1;
	double m_2 = 0.5 *(m);
	int halfLength = windowLength / 2;

	/*	*/
	switch( type)
	{
		case RECTANGULAR:
			for(n = 0; n < windowLength; n++)
			{
				out[n] = 1.0;
			}
			break;

		case BATLETT:
			for(n = 0; n <= halfLength; n++)
			{
				out[n] = out[windowLength - 1]= 1 - 2 * (m_2 - n) / m; // Because: m_2 > n in [0:halfLength]
			}
			break;

		case HANNING:
			for(n = 0; n <= halfLength; n++)
			{
				out[n] = 0.5 - 0.5 * cos(2 * M_PI * n / (2 * halfLength));
				out[windowLength - n - 1] =  out[n];
			}
			break;

		case HAMMING:
			for(n = 0; n <= halfLength; n++)
			{
				out[n] = 0.54 - 0.6 * cos(2 * M_PI * n / (2 * halfLength));
				out[windowLength - n - 1] = out[n];
			}
			break;

		case BLACKMAN:
			for(n = 0; n <= halfLength; n++)
			{
				out[n] = 0.42 - 0.5* cos(2 * M_PI * n / (2 * halfLength)) + 0.08 * cos(4 * M_PI * n/ (2*halfLength));
				out[windowLength - n - 1] =  out[n];
			}
			break;
	}

	// If input has been given, multiply with out
	if (in != NULL) {
		for (n = 0 ; n < windowLength ; n++) {
			out[n] *= in[n];
		}
	}

	return out;
}

/*
 * Function	: calculateKaiserParams
 * Propose	: Create windows with type Kaiser
 * Param 	: ripple is given in Hz
 			  transWidth is given in Hz
 			  sampFreq is given in Hz
 			  Window Length (windowLength) will be set
 * Return	: NONE
 */
void calculateKaiserParams(double ripple, double transWidth, double sampFreq, int *windowLength, double *beta)
{
	// Calculate delta w
	double dw = 2 * M_PI * transWidth / sampFreq;

	// Calculate ripple dB
	double a = -20.0 * log10(ripple);

	// Calculate filter order
	int m;
	if (a>21) m = ceil((a-7.95) / (2.285*dw));
	else m = ceil(5.79/dw);

	*windowLength = m + 1;

	if (a<=21) *beta = 0.0;
	else if (a<=50) *beta = 0.5842 * pow(a-21, 0.4) + 0.07886 * (a-21);
	else *beta = 0.1102 * (a-8.7);
}

/*
 * Function	: createKaiserWindow
 * Propose	: calculate param for Keise windows
 * Param 	: Transition Width (transWidth) is given in Hz
 			  Sampling Frequency (sampFreq) is given in Hz
 			  Window Length (windowLength) will be set
 * Return	: Pointer to array store coefficient of windows
 */
double *createKaiserWindow(double *in, double *out, int windowLength, double beta)
{
	double m_2 = (double)(windowLength-1) / 2.0;
	double denom = modZeroBessel(beta);					// Denominator of Kaiser function

	// If output buffer has not been allocated, allocate memory now
	if (out == NULL) {
		out = (double *) malloc(windowLength * sizeof(double));
		if (out == NULL) {
			fprintf(stderr, "Could not allocate memory for window\n");
			return NULL;
		}
	}

	int n;
	for (n=0 ; n<windowLength ; n++)
	{
		double val = ((n) - m_2) / m_2;
		val = 1 - (val * val);
		out[n] = modZeroBessel(beta * sqrt(val)) / denom;
	}

	// If input has been given, multiply with out
	if (in != NULL) {
		for (n=0 ; n<windowLength ; n++) {
			out[n] *= in[n];
		}
	}

	return out;
}
/*
 * Function	: modZeroBessel
 * Propose	: calculate modZeroBessel
 */
double modZeroBessel(double x)
{
	int i;

	double x_2 = x/2;
	double num = 1;
	double fact = 1;
	double result = 1;

	for (i=1 ; i<20 ; i++) {
		num *= x_2 * x_2;
		fact *= i;
		result += num / (fact * fact);
//		printf("%f %f %f\n", num, fact, result);
	}

	return result;
}
/*
 * Function	: outputFFT
 * Propose	: Use FFT (form support infftw3 lib) to transform signal to frequence doman then write result to a file
 */
int outputFFT(char *filename, double *window, int windowLength, double sampFreq)
{	
	int i;
	FILE *fp;
	double *in;
	fftw_complex *out;
	fftw_plan plan;
	int result = 0;

	// If the window length is short, zero padding will be used
	int fftSize = (windowLength < 2048) ? 2048 : windowLength;

	// Calculate size of result data
	int resultSize = (fftSize / 2) + 1;

	// Allocate memory to hold input and output data
	in = (double *) fftw_malloc(fftSize * sizeof(double));
	out = (fftw_complex *) fftw_malloc(resultSize * sizeof(fftw_complex));
	if (in == NULL || out == NULL) {
		result = 1;
		fprintf(stderr, "outputFFT: Could not allocate input/output data\n");
		goto finalise;
	}

	// Create the plan and check for success
	plan = fftw_plan_dft_r2c_1d(fftSize, in, out, FFTW_MEASURE); 
	if (plan == NULL) {
		result = 1;
		fprintf(stderr, "outputFFT: Could not create plan\n");
		goto finalise;
	}

	// Copy window and add zero padding (if required)
	for (i=0 ; i<windowLength ; i++) in[i] = window[i];
	for ( ; i<fftSize ; i++) in[i] = 0;
	
	// Perform fft
	fftw_execute(plan);

	// Open file for writing
	fp = fopen(filename, "w");
	if (fp == NULL) {
		result = 1;
		fprintf(stderr, "outputFFT: Could open output file for writing\n");
		goto finalise;
	}

	// Output result
	for (i=0 ; i<resultSize ; i++)
	{
		double freq = sampFreq * i / fftSize;
		double mag = sqrt(out[i][0] * out[i][0] + out[i][1] * out[i][1]);
		double magdB = 20 * log10(mag);
		double phase = atan2(out[i][1], out[i][0]);
		fprintf(fp, "%02d %f %f %f %f\n", i, freq, mag, magdB, phase);
	}

	// Perform any cleaning up
	finalise:
	if (plan != NULL) fftw_destroy_plan(plan);
	if (in != NULL) fftw_free(in);
	if (out != NULL) fftw_free(out);
	if (fp != NULL) fclose(fp);

	return result;
}

/*
 * Function	: fir_filter
 * Propose	: Calculate data before use FFR filter
 * Param 	: rawData, firCoef, bReset
 * Result 	: data filted.
 */
double fir_filter(double rawData, double *firCoef, int firOrder, unsigned char bReset)
{
	double dataFilted = 0.0f;
	int i = 0;
	static double dataBuffer[MAX] = {0.0f};
	if(bReset == CLEAN)
	{
		for(i = 0; i < firOrder; i++)
			dataBuffer[i] = 0.0f;
	}
	else
	{
		/* Put data to the fist position of buffer*/
		dataBuffer[0] = (double)rawData;
		for(i = 0; i <= firOrder; i++)
			dataFilted += firCoef[i] * dataBuffer[i];
		/* Shift ringBufer a postion*/
		for(i = firOrder; i > 0; i--)
			dataBuffer[i] = dataBuffer[i-1];
	}

	return dataFilted;

}

int main(void)
{
	int windowLength = 21;
	double sampFreq = 8000;

	// Low and high pass filters
	double transFreq = 100;

	double *lpf = create1TransSinc(windowLength, transFreq, sampFreq, LOW_PASS);
	//double *lpf_rectang = createWindow(lpf, NULL, windowLength, RECTANGULAR);
	double *lpf_hamming = createWindow(lpf, NULL, windowLength, HAMMING);
	// double *lpf_blackman = createWindow(lpf, NULL, windowLength, BLACKMAN);

	// double *hpf = create1TransSinc(windowLength, transFreq, sampFreq, HIGH_PASS);
	// double *hpf_hamming = createWindow(hpf, NULL, windowLength, HAMMING);

	// outputFFT("lpf-hamming.dat", lpf_hamming, windowLength, sampFreq);
	// outputFFT("lpf-blackman.dat", lpf_blackman, windowLength, sampFreq);
	// outputFFT("hpf-hamming.dat", hpf_hamming, windowLength, sampFreq);

	// Band pass and band stop filters
	// double trans1Freq = 5000;
	// double trans2Freq = 17050;

	// double *bpf = create2TransSinc(windowLength, trans1Freq, trans2Freq, sampFreq, BAND_PASS);
	// double *bpf_hamming = createWindow(bpf, NULL, windowLength, HAMMING);

	// double *bsf = create2TransSinc(windowLength, trans1Freq, trans2Freq, sampFreq, BAND_STOP);
	// double *bsf_hamming = createWindow(bsf, NULL, windowLength, HAMMING);

	// outputFFT("bpf-hamming.dat", bpf_hamming, windowLength, sampFreq);
	// outputFFT("bsf-hamming.dat", bsf_hamming, windowLength, sampFreq);

	// Kaiser Window
	// int kaiserWindowLength;
	// double beta;

	// calculateKaiserParams(0.001, 800, sampFreq, &kaiserWindowLength, &beta);

	// lpf = create1TransSinc(kaiserWindowLength, transFreq, sampFreq, LOW_PASS);
	// double *lpf_kaiser = createKaiserWindow(lpf, NULL, kaiserWindowLength, beta);

	// outputFFT("lpf-kaiser.dat", lpf_kaiser, kaiserWindowLength, sampFreq);


	/* Test implement filter*/
	double rawData[MAX];
	double filtedData[MAX];
	double t = 0;
	int count = 0;

	//filtedData[count] = fir_filter(rawData[count], lpf_hamming, windowLength, CLEAN);
	for(t = 0; t < 0.1; t += 1.0/sampFreq)
	{
		rawData[count] =  50 * sin(2 * M_PI * 100 * t) + rand()%20 * sin(2 * M_PI * 300 * t) + rand()%10;
		filtedData[count] = fir_filter(rawData[count], lpf_hamming, windowLength, 1);
		printf("%lf \t %lf \t %f \t \n", t, rawData[count], filtedData[count]);
		count++;
	}
	return 0;
}