Basic Filters Implementation in C

Table of contents
  1. Intro
  2. General structure
  3. FIR Filters
    1. Structure
    2. Initialization
    3. Filtering
    4. Cleanup
    5. Example
  4. IIR Filters
    1. Structure
    2. Initialization
    3. Filtering
    4. Cleanup
    5. Example
  5. Biquad Filters
    1. Structure
    2. Initialization
    3. Filtering
    4. Cleanup
    5. Example
  6. Summary

Intro

After a deep dive into filters theory in the previous post. It is time to transform the gained knowledge into real code. I will present the implementation for the FIR, and IIR filters in a general form that allows using them for any application and exploring filter designing. Moreover, a simpler version of biquad filters will be presented for more real-time applications. There will be a lot of code but most of it is pretty straightforward (if you’ve read theory) and easy to use for your application.

General structure

All filter types will have a similar structure with coefficients arrays and buffers for inputs or outputs. Also, for every type, there will be defined special functions to interact with objects. In general, we can divide those functions into 3 categories:

  • Initialization - allocating memory and calculating all coefficients of the filters,
  • Filtering/Applying - applying a filer in the main program (actually filtering a signal),
  • Cleanup - freeing memory and resetting parameters in case of deactivation of the filter.

Moreover, we will define a function for updating filters’ structure which is essential for some filtering (moving Notch filter or RPM filter).

FIR Filters

Let’s start with the simplest one. We need to define a structure combining coefficients and inputs but we don’t want to decide about the order of the filter in advance. Let’s create a variable for the order of the filter lenght and pointers for future array addresses * buffer and * impulse_response. Also, let’s save the last output filtered value to have quick access to that output. The structure below allows us to dynamically allocate memory so it will be suitable for filters of any length. Because we will be using a circular buffer we need buffer_index to keep track of the last measurement.

Structure 

1
2
3
4
5
6
7
8

// filters.h
typedef struct{
	uint8_t length;             //	how many previous samples will be taken into calculation
	float* buffer;              //	buffer for input measurements
	uint8_t buffer_index;       //	variable to control position of the latest measurement
	float* impulse_response;    //	coefficients for inputs values
	float output;               //	final output value
} FIR_Filter_t;

Initialization

Next, we need to initialize our filter and allocate memory for its arrays:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

// filters.h
void FIR_filter_init(FIR_Filter_t* fir, uint8_t order);

// filters.c
void FIR_filter_init(FIR_Filter_t* fir, uint8_t order){
	fir->length = order;
	//	Allocate memory for arrays
	fir->buffer = (float*)malloc(sizeof(*(fir->buffer)) * fir->length);
	fir->impulse_response = (float*)malloc(
		sizeof(*(fir->impulse_response)) * fir->length);
	//	Clear filter buffer
	for (uint8_t i = 0; i < order; i++){
		fir->buffer[i] = 0.f;
		fir->impulse_response[i] = 0.f;
	}
	//	Clear buffer index
	fir->buffer_index = 0;
	//	Clear filter output
	fir->output = 0.0f;
}

Filtering

After that filter is ready to use but its coefficients (impulse_response[]) are all set to $0$. Therefore we need to update them manually one by one. An example is shown at the end. After updating coefficients we can perform a filtering process. We need to calculate a convolution - basically, we multiply all the last measurements with corresponding coefficients (bearing in mind the circular buffer implementation).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

// filters.h
float FIR_filter_apply(FIR_Filter_t* fir, float input)

// filters.c
float FIR_filter_apply(FIR_Filter_t* fir, float input){
	//	Add new data to buffer
	fir->buffer[fir->buffer_index] = input;
	//	Increment buffer_index and loop if necessary
	fir->buffer_index++;
	if (fir->buffer_index == fir->length){
		fir->buffer_index = 0;
	}
	//	Reset old output value
	fir->output = 0.f;
	uint8_t sum_index = fir->buffer_index;
	//	Decrement sum_index and loop if necessary
	for (uint8_t i = 0; i < fir->length; i++){
		if (sum_index > 0){
			sum_index--;
		}
		else{
			sum_index = fir->length - 1;
		}
		//	Compute new output (via convolution)
		fir->output += fir->impulse_response[i] * fir->buffer[sum_index];
	}
	return fir->output;
}

Cleanup

For drone applications, we will set up and initialize filters at the beginning and use them as long as the drone program is running (so we don’t have to bother with freeing allocated memory). But you may need filters only for some time and then they can be discarded. In such a case you need to remember to free the memory allocated during the initialization. Moreover, this is good practice anyway - to write a function freeing memory every time you dynamically allocate memory.

1
2
3
4
5
6
7
8
9
10
11
12
13
14

// filters.h
void FIR_filter_cleanup(FIR_Filter_t* fir);

// filters.c
void FIR_filter_cleanup(FIR_Filter_t* fir){
	// Free allocated memory
	free(fir->buffer);
	free(fir->impulse_response);
	// Reset length and buffer index
	fir->length = 0;
	fir->buffer_index = 0;
	// Reset output
	fir->output = 0.0f;
}

Example

Now it is time to show an example of how to use it in practice:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

// main.c
#include"filters.h"
// ...

int main(){
    // setup
    // ...
    // define all variables and constants:
    #define GYRO_FILTERS_ORDER 6
    const float GYRO_FILTER_X_COEF[GYRO_FILTERS_ORDER] = { 0.135250f, 0.216229f, 0.234301f, 0.234301f, 0.216229f, 0.135250f };
    FIR_Filter_t filter_gyro_X;
    // initialize filter:
    FIR_filter_init(&filter_gyro_X, GYRO_FILTERS_ORDER);
    // update coefficients:
    for (uint8_t i = 0; i < filter_gyro_X.length; i++){
        filter_gyro_X.impulse_response[i] = GYRO_FILTER_X_COEF[i];
    }
    while(true){
        // variable for raw measurement:
        float raw_input_x;
        // perform filtering (you need to provide new values and take care of timing)
        float filtered_value = FIR_filter_apply(&filter_gyro_X, raw_input_x);
        // you can get last filtered value from filter also:
        filtered_value = filter_gyro_X.output;
    }
    // frre memory
    FIR_filter_cleanup(&filter_gyro_X);
    return 0;
}

IIR Filters

IIR filter is pretty similar to FIR but a bit extended since it needs inputs and outputs saved.

Structure 

1
2
3
4
5
6
7
8
9
10

// filters.h
typedef struct{
	uint8_t filter_order;           //  how many previous samples will be taken into calculation
	float* buffer_input;            //  buffer for input measurements
	float* buffer_output;           //  buffer for output values from filter
	uint8_t buffer_index;           //  variable to control position of the latest measurement
	float* forward_coefficients;    //  coefficients for inputs values
	float* feedback_coefficients;   //  coefficients for previous outputs values
	float output;                   //  final output value
} IIR_Filter_t;

Initialization

Initialization is similar but you need to update both coefficients’ arrays (example at the end):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

// filters.h
void IIR_filter_init(IIR_Filter_t* iir, uint8_t order);
// filters.c

void IIR_filter_init(IIR_Filter_t* iir, uint8_t order){
	iir->filter_order = order;
	//	Allocate memory for arrays
	iir->buffer_input = (float*)malloc(
		sizeof(*(iir->buffer_input)) * (iir->filter_order + 1));
	iir->buffer_output = (float*)malloc(
		sizeof(*(iir->buffer_output)) * (iir->filter_order));
	iir->forward_coefficients = (float*)malloc(
		sizeof(*(iir->forward_coefficients)) * (iir->filter_order + 1));
	iir->feedback_coefficients = (float*)malloc(
		sizeof(*(iir->feedback_coefficients)) * (iir->filter_order));
	//	Clear filter buffers and coefficients
	for (uint8_t i = 0; i < (order + 1); i++){
		// FORWARD PART:
		iir->buffer_input[i] = 0.f;
		iir->forward_coefficients[i] = 0.f;
		// FEEDBACK PART:
		iir->buffer_output[i] = 0.f;
		iir->feedback_coefficients[i] = 0.f;
	}
	//	Clear buffer index
	iir->buffer_index = 0;
	//	Clear filter output
	iir->output = 0.0f;
}

Filtering 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

// filters.h
float IIR_filter_apply(IIR_Filter_t* iir, float input);

// filters.c
float IIR_filter_apply(IIR_Filter_t* iir, float input){
	//	Add new data to buffer_input:
	iir->buffer_input[iir->buffer_index] = input;
	//	Reset old output value:
	iir->output = 0.f;
	// save position of the latest sample:
	uint8_t sum_index = iir->buffer_index;
	//	FORWARD PART:
	//	Decrement sum_index and loop if necessary
	for (uint8_t i = 0; i < iir->filter_order + 1; i++){
		//	Compute forward part of a new output:
		iir->output += iir->forward_coefficients[i] * iir->buffer_input[sum_index];
		if (sum_index > 0){
			sum_index--;
		}
		else{
			sum_index = iir->filter_order;
		}
	}
	//	FEEDBACK PART:
	sum_index = iir->buffer_index;
	//	Decrement sum_index and loop if necessary
	for (uint8_t i = 0; i < iir->filter_order; i++){
		if (sum_index > 0){
			sum_index--;
		}
		else{
			sum_index = iir->filter_order - 1;
		}
		// Add feedback part of a new output
		iir->output -= iir->feedback_coefficients[i] * iir->buffer_output[sum_index];
	}
	//	Add new data to buffer_output
	iir->buffer_output[iir->buffer_index] = iir->output;
	//	Increment buffer_index and loop if necessary
	iir->buffer_index++;
	if (iir->buffer_index > iir->filter_order){
		iir->buffer_index = 0;
	}
	return iir->output;
}

Cleanup 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

// filters.h
void IIR_filter_cleanup(IIR_Filter_t* iir);
//filters.c
void IIR_filter_cleanup(IIR_Filter_t* iir){
	// Free allocated memory
	free(iir->buffer_input);
	free(iir->buffer_output);
	free(iir->forward_coefficients);
	free(iir->feedback_coefficients);
	// Reset length and buffer index
	iir->filter_order = 0;
	iir->buffer_index = 0;
	// Reset output
	iir->output = 0.0f;
}

Example 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

// main.c
#include"filters.h"
// ...
int main(){
    // setup
    // ...
    // define all variables and constants:
    #define GYRO_FILTERS_ORDER 2
    float GYRO_FILTER_X_FORW_COEF[GYRO_FILTERS_ORDER + 1] = { 0.02008336f, 0.04016673f, 0.02008336f };
    const float GYRO_FILTER_X_BACK_COEF[GYRO_FILTERS_ORDER] = { -1.56101807f, 0.6413515f };
    IIR_Filter_t filter_gyro_X;
    // initialize filter:
    IIR_filter_init(&filter_gyro_X, GYRO_FILTERS_ORDER);
    // update coefficients:
	for (uint8_t i = 0; i < filter_gyro_X.filter_order + 1; i++){
		filter_gyro_X.forward_coefficients[i] = GYRO_FILTER_X_FORW_COEF[i];
	}
	for (uint8_t i = 0; i < filter_gyro_X.filter_order; i++){
		filter_gyro_X.feedback_coefficients[i] = GYRO_FILTER_X_BACK_COEF[i];
	}
    while(true){
        // variable for raw measurement:
        float raw_input_x;
        // perform filtering (you need to provide new values and take care of timing)
        float filtered_value = IIR_filter_apply(&filter_gyro_X, raw_input_x);
        // you can get last filtered value from filter also:
        filtered_value = filter_gyro_X.output;
    }
    // free memory
    IIR_filter_cleanup(&filter_gyro_X);
    return 0;
}

Biquad Filters

The above implementations allow you great flexibility and you can use them for any filter (low-pass, high-pass, band-pass, etc.). However, for real-time applications, we need something more computationally efficient and maybe less versatile. Moreover, often second-order filters are good enough for simple filtration (IIR filters especially) and at the same time, they are simple enough for convenient design. Biquad filters are just 2nd order IIR filters and we will implement a more condensed version of IIR filter functions for better performance (you can stick with the previous implementation but new ones will be simpler to use and in most cases, biquad filters are good enough anyway).

Structure

Since we know all the required variables we can define them explicitly. Moreover, since we have a defined structure (for low-pass or high-pass etc.) we can easily calculate the required coefficients on the fly (with some knowledge about frequencies). Besides frequencies, we have one parameter to set - the Q-factor. Of course, you can define it as you want but for many scenarios, we can set $ Q=\frac{1}{\sqrt{2}}$ which corresponds 2nd order Butterworth (low-pass or high-pass):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

// filters.h
typedef struct{
	float frequency;    // cut-off/center frequency
	float Q_factor;	    // quality factor
	float a0;
	float a1;
	float a2;
	float b0;
	float b1;
	float b2;
	float x1;   //  last input
	float x2;   //	2nd last input
	float y1;   //	last output
	float y2;   //	2nd last output
} biquad_Filter_t;

typedef enum{
    BIQUAD_LPF,
    BIQUAD_HPF;
    BIQUAD_NOTCH,
    BIQUAD_BPF,
} biquad_Filter_type;

Initialization

For initialization we don’t need to allocate any memory since variables were defined explicitly but we need to calculate coefficients. In most cases, they will stay the same for the rest of the program so we only need to do it once. So function biquad_filter_init in reality is just a wrapper (with a small addition) for the functionbiquad_filter_update which handles all the calculations and updates coefficients.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

// filters.h
void biquad_filter_init(biquad_Filter_t* filter, biquad_Filter_type filter_type, float center_frequency_Hz, float quality_factor, uint16_t sampling_frequency_Hz);
static void biquad_filter_update(biquad_Filter_t* filter, biquad_Filter_type filter_type, float filter_frequency_Hz, float quality_factor, uint16_t sampling_frequency_Hz);

// filters.c
void biquad_filter_init(biquad_Filter_t* filter, biquad_Filter_type filter_type, float filter_frequency_Hz, float quality_factor, uint16_t sampling_frequency_Hz){
	biquad_filter_update(filter, filter_type, filter_frequency_Hz, quality_factor, sampling_frequency_Hz);
	// set previous values as 0:
	filter->x1 = 0;
	filter->x2 = 0;
	filter->y1 = 0;
	filter->y2 = 0;
}

static void biquad_filter_update(biquad_Filter_t* filter, biquad_Filter_type filter_type, float filter_frequency_Hz, float quality_factor, uint16_t sampling_frequency_Hz){
	// save filter info:
	filter->frequency = filter_frequency_Hz;
	filter->Q_factor = quality_factor;
	const float omega = 2.f * M_PI * filter_frequency_Hz / sampling_frequency_Hz;
	const float sn = sinf(omega);
	const float cs = cosf(omega);
	const float alpha = sn / (2.0f * filter->Q_factor);
	// implementation from datasheet:https://www.ti.com/lit/an/slaa447/slaa447.pdf <-- not everything good
	// or even better: http://shepazu.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html <-- probably resource for above
	/*
	general formula for 2nd order filter:
                   b0*z^-2 + b1*z^-1 + b2
	H(z^-1) = ------------------------
                   a0*z^-2 + a1*z^-1 + a2

	main analog prototypes:
	Low-pass:
                     1
	H(s) = ---------------
                s^2 + s/Q + 1
	High-pass:
                    s^2
	H(s) = ---------------
	        s^2 + s/Q + 1
	Notch:
	           s^2 + 1
	H(s) = ---------------
	        s^2 + s/Q + 1
	BPF (skirt gain where Q is max. gain):
	             s
	H(s) = ---------------
	        s^2 + s/Q + 1

	Q is a quality factor
	In this datasheet conversion from s -> z domain is done with Bilinear transform with pre-warping
	*/
	filter->a0 = 1 + alpha;
	switch (filter_type){
	case BIQUAD_LPF:
		filter->b0 = (1 - cs) * 0.5f;
		filter->b1 = 1 - cs;
		filter->b2 = filter->b0;
		filter->a1 = -2 * cs;
		filter->a2 = 1 - alpha;
		break;
	case BIQUAD_HPF:
		filter->b0 = (1 + cs) * 0.5f;
		filter->b1 = -(1 + cs);
		filter->b2 = (1 + cs) * 0.5f;
		filter->a1 = -2 * cs;
		filter->a2 = 1 - alpha;
		break;
	case BIQUAD_NOTCH:
		filter->b0 = 1;
		filter->b1 = -2 * cs;
		filter->b2 = 1;
		filter->a1 = filter->b1;
		filter->a2 = 1 - alpha;
		break;
	case BIQUAD_BPF:
		filter->b0 = alpha;
		filter->b1 = 0;
		filter->b2 = -alpha;
		filter->a1 = -2 * cs;
		filter->a2 = 1 - alpha;
		break;
	}
	// oust a0 coefficient:
	filter->b0 /= filter->a0;
	filter->b1 /= filter->a0;
	filter->b2 /= filter->a0;
	filter->a1 /= filter->a0;
	filter->a2 /= filter->a0;
}

Filtering

Filtering is the same as before in terms of the function prototype but implementation allows for some improvements - check the Implementation paragraph on Wikipedia. In reality, you can use either of these functions and for most cases, it will be good enough (although DF2 is a little better):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

// filters.h
float biquad_filter_apply_DF1(biquad_Filter_t* filter, float input);
float biquad_filter_apply_DF2(biquad_Filter_t* filter, float input);

// filters.c
float biquad_filter_apply_DF1(biquad_Filter_t* filter, float input){
	// direct form 1 (the most straighforward):
	const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
	/* shift x1 to x2, input to x1 */
	filter->x2 = filter->x1;
	filter->x1 = input;
	/* shift y1 to y2, result to y1 */
	filter->y2 = filter->y1;
	filter->y1 = result;
	return result;
}

float biquad_filter_apply_DF2(biquad_Filter_t* filter, float input){
	//	this is transposed direct form 2 is a little more precised for float number implementation:
	const float result = filter->b0 * input + filter->x1;
	filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
	filter->x2 = filter->b2 * input - filter->a2 * result;
    //y1, y2, x1 and x2 are not saved in the filter structure x1 and x2 are used to save s1 and s2 (https://en.wikipedia.org/wiki/Digital_biquad_filter)
    // MOD: if you want you can use y1 and y2 variables to save last outputs:
    // filter->y2 = filter->y1;
    // filter->y1 = result;
	return result;
}

Cleanup

Since we were not allocating any memory with malloc there is no need to free anything.

Example 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

// main.c
#include"filters.h"
// ...
int main(){
    // setup
    // ...
    // define all variables and constants:
    #define BIQUAD_LPF_CUTOFF_GYRO 90 // [Hz]
    #define BIQUAD_LPF_Q (1.f / sqrtf(2)) // Q factor for Low-pass filters
    #define FREQUENCY_MAIN_LOOP 2000 // [Hz]
    biquad_Filter_t filter_gyro_X;
    // initialize filter:
    biquad_filter_init(&filter_gyro_X, BIQUAD_LPF, BIQUAD_LPF_CUTOFF_GYRO, BIQUAD_LPF_Q, FREQUENCY_MAIN_LOOP);
    while(true){
        // variable for raw measurement:
        float raw_input_x;
        // perform filtering (you need to provide new values and take care of timing)
        float filtered_value = biquad_filter_apply_DF2(&filter_gyro_X, raw_input_x);
        // WARNING! you can get last filtered value from filter ONLY if you use biquad_filter_apply_DF1 or modified DF2:
        // filtered_value = filter_gyro_X.y1; // filter_gyro_X.y1 is not used currently
    }
    return 0;
}

Summary

And that’s it. Maybe there should be some screenshots before and after filtering but who cares really? It works, you can trust me or just check it yourself.

For my drone, I was using each of these filters (and even have the option to change between them in the code) but the most convenient are biquad filters since you don’t have to calculate coefficients manually (you can calculate coefficients for IIR and FIR filter as well in code but there is no point if you will use 2nd order filters).

But the most important thing for a good filtration is to precisely apply notch filters and for that, we have RPM filter!