N.B. this is new and still being tested.
This sketch demonstrates several examples of single-channel filters for processing sensor data.
The filter functions are purely numeric operations and not dependent on any Arduino-specific features. This supports offline testing, as they can be compiled for debugging and evaluation on a normal desktop computer. The code is designed to be included directly in an Arduino sketch, either as additional files or directly pasted into a sketch.
Please note this code follows Arduino conventions and uses global variables for filter state. For this reason, these functions will need to be customized for each application. This sample code is intended to be integrated into a sketch, not serve as a standalone library.
For more efficient implementations and a much broader variety of signal processing algorithms, I recommend exploring the Arduino library collections.
The sketch files may be downloaded in a single archive file as FilterDemos.zip, or browsed in raw form in the source folder. The individual files are documented below.
Main Sketch
The main sketch file is FilterDemos.ino. It includes an event loop to sample a sonar range finder sensor and apply several signal processing filters. The output is printed in a form suitable for real-time plotting using the IDE Serial Plotter.
// FilterDemos.ino : Arduino program to demonstrate a variety of single-channel signal filters. // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. // The actual filter functions are kept in a separate .ino files which will // automatically be compiled with this one by the Arduino IDE. The filters are // purely numerical and can compiled for testing on a normal desktop computer. #include <stdlib.h> // The baud rate is the number of bits per second transmitted over the serial port. const long BAUD_RATE = 115200; //================================================================ // Hardware definitions. You will need to customize this for your specific hardware. const int sonarTriggerPin = 7; // Specify a pin for a sonar trigger output. const int sonarEchoPin = 8; // Specify a pin for a sonar echo input. //================================================================ // Global variables. #include "statistics.h" CentralMeasures stats; // class defined in statistics.h //================================================================ // Standard Arduino initialization function to configure the system. void setup() { // initialize the Serial port Serial.begin( BAUD_RATE ); // Initialize the digital input/output pins. pinMode(sonarTriggerPin, OUTPUT); pinMode(sonarEchoPin, INPUT); } //================================================================ // Standard Arduino polling function. This function is called repeatedly to // handle all I/O and periodic processing. This loop should never be allowed to // stall or block so that all tasks can be constantly serviced. void loop() { // Calculate the interval in microseconds since the last polling cycle. static unsigned long last_time = 0; unsigned long now = micros(); unsigned long interval = now - last_time; last_time = now; // Poll the sonar at regular intervals. static long sonar_timer = 0; sonar_timer -= interval; if (sonar_timer < 0) { sonar_timer += 100000; // 10 Hz sampling rate // read the sonar; zeros represent a no-ping condition int raw_ping = ping_sonar(); // suppress zeros in the input, just repeating the last input int nz_ping = suppress_value(raw_ping, 0); // apply a median filter to suppress individual outliers int median = median_3_filter(nz_ping); // convert the value from microseconds to centimeters float cm = fmap(median, 0.0, 5900.0, 0.0, 100.0); // track central measures such as average and variance stats.add(cm); stats.compute_stats(); // apply the low-pass, high-pass, band-pass, and band-stop filters float lowpass_cm = lowpass(cm); float highpass_cm = highpass(cm); float bandpass_cm = bandpass(cm); float bandstop_cm = bandstop(cm); // apply the range data to the ring buffer and filter ring_buffer_put(cm); // calculate the finite differencing derivative float velocity = ring_buffer_deriv(0.1); // calculate the median filter over the ring buffer float ring_median = ring_median_filter(); // fit a trajectory curve to recent sample history float traj[3]; trajfit(cm, traj); // emit some data to plot // Serial.print(raw_ping); Serial.print(" "); // ping time in microseconds // Serial.print(median); Serial.print(" "); // median-filtered ping time // Serial.print(cm); Serial.print(" "); // centimeter-scaled of median-filtered // Serial.print(velocity); Serial.print(" "); // velocity using finite differencing Serial.print(ring_median); Serial.print(" "); // median-filtered distance using ring buffer Serial.print(traj[0]); Serial.print(" "); // quadratic position Serial.print(traj[1]); Serial.print(" "); // quadratic velocity // Serial.print(stats.average); Serial.print(" "); // position average over all data Serial.println(); } } //================================================================ // Run a measurement cycle on the sonar range sensor. Returns the round-trip // time in microseconds. int ping_sonar(void) { // Generate a short trigger pulse. digitalWrite(sonarTriggerPin, HIGH); delayMicroseconds(10); digitalWrite(sonarTriggerPin, LOW); // Measure the echo pulse length. The ~6 ms timeout is chosen for a maximum // range of 100 cm assuming sound travels at 340 meters/sec. With a round // trip of 2 meters distance, the maximum ping time is 2/340 = 0.0059 // seconds. You may wish to customize this for your particular hardware. const unsigned long TIMEOUT = 5900; unsigned long ping_time = pulseIn(sonarEchoPin, HIGH, TIMEOUT); return ping_time; } //================================================================
statistics.h
class CentralMeasures
Object encapsulating a set of accumulators for calculating the mean, variance, min, and max of a stream of values. All instance variables are public to simplify reading out current properties. This class is suitable for static initialization as per Arduino programming conventions.
// statistics.ino : compute some basic central measures in an accumulator // No copyright, 2009-2020, Garth Zeglin. This file is explicitly placed in the public domain. #include <math.h> #include <float.h> #ifndef MAXFLOAT #define MAXFLOAT FLT_MAX #endif class CentralMeasures { public: long samples; // running sum of value^0, i.e., the number of samples float total; // running sum of value^1, i.e., the accumulated total float squared; // running sum of value^2, i.e., the accumulated sum of squares float min; // smallest input seen float max; // largest input seen float last; // most recent input // computed statistics float average; // mean value float variance; // square of the standard deviation // Constructor to initialize an instance of the class. CentralMeasures(void) { samples = 0; total = squared = 0.0; min = MAXFLOAT; max = -MAXFLOAT; last = 0.0; average = variance = 0.0; } // add a new sample to the accumulators; does not update the computed statistics void add(float value) { total += value; squared += value*value; if (value < min) min = value; if (value > max) max = value; samples += 1; last = value; } void compute_stats(void) { if ( samples > 0 ) { average = total / samples; if (samples > 1) { // The "standard deviation of the sample", which is only correct // if the population is normally distributed and a large sample // is available, otherwise tends to be too low: // sigma = sqrtf( samples * squared - total*total ) / ((float)samples); // Instead compute the "sample standard deviation", an unbiased // estimator for the variance. The standard deviation is the // square root of the variance. variance = ( samples * squared - total*total) / ((float) samples * (float)(samples - 1)) ; } } } };
linear.ino
float fmap
(float x, float in_min, float in_max, float out_min, float out_max)
Floating-point version of map(). The standard Arduino map() function only operates using integers; this extends the idea to floating point. The Arduino function can be found in the WMath.cpp file within the Arduino IDE distribution. Note that constrain() is defined as a preprocessor macro and so doesn’t have data type limitations.
// linear.ino : platform-independent linear transforms // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. //================================================================ // Floating-point version of map(). The standard Arduino map() function only // operates using integers; this extends the idea to floating point. The // Arduino function can be found in the WMath.cpp file within the Arduino IDE // distribution. Note that constrain() is defined as a preprocessor macro and // so doesn't have data type limitations. float fmap(float x, float in_min, float in_max, float out_min, float out_max) { float divisor = in_max - in_min; if (divisor == 0.0) { return out_min; } else { return (x - in_min) * (out_max - out_min) / divisor + out_min; } } //================================================================
hysteresis.ino
int suppress_value
(int input, int value)
Suppress a specific value in an input stream. One integer of state is required.
// hysteresis.ino : platform-independent non-linear filters // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. //================================================================ // Quantize an input stream into a binary state. Dual thresholds are needed to // implement hysteresis: the input needs to rise above the upper threshold to // trigger a high output, then drop below the input threshold to return to the // low output. One bit of state is required. bool hysteresis(int input, int lower=300, int upper=700) { // The previous state is kept in a static variable; this means this function // can only be applied to a single input stream. static bool output = false; // previous binary output if (output) { if (input < lower) output = false; } else { if (input > upper) output = true; } return output; } //================================================================ // Suppress a specific value in an input stream. One integer of state is required. int suppress_value(int input, int value) { static int previous = 0; if (input != value) previous = input; return previous; } //================================================================ // Debounce an integer stream by suppressing changes from the previous value // until a specific new value has been observed a minimum number of times. Three // integers of state are required. int debounce(int input, int samples) { static int current_value = 0; static int new_value = 0; static int count = 0; if (input == current_value) { count = 0; } else { if (count == 0) { new_value = input; count = 1; } else { if (input == new_value) { count += 1; if (count >= samples) { current_value = new_value; count = 0; } } else { new_value = input; count = 1; } } } return current_value; } //================================================================
smoothing.ino
// smoothing.ino : platform-independent first-order smoothing filter // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. // Smooth an input signal using a first-order filter. One floating point state // value is required. The smaller the coefficient, the smoother the output. float smoothing(float input, float coeff=0.1) { // The previous state is kept in a static variable; this means this function // can only be applied to a single input stream. static float value = 0.0; // filtered value of the input float difference = input - value; // compute the error value += coeff * difference; // apply a constant coefficient to move the smoothed value toward the input return value; }
moving_average.ino
// moving_average.ino : windowed moving average filter with integer data (platform-independent) // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. // Smooth a signal by averaging over multiple samples. The recent time history // (the 'moving window') is kept in an array along with a running total. The // state memory includes one long, an integer, and an array of integers the // length of the window. int moving_average(int input) { // The window size determines how many samples are held in memory and averaged together. const int WINDOW_SIZE = 5; static int ring[WINDOW_SIZE]; // ring buffer for recent time history static int oldest = 0; // index of oldest sample static long total = 0; // sum of all values in the buffer // subtract the oldest sample from the running total before overwriting total = total - ring[oldest]; // save the new sample by overwriting the oldest sample ring[oldest] = input; // advance to the next position, wrapping around as needed oldest = oldest + 1; if (oldest >= WINDOW_SIZE) oldest = 0; // add the new input value to the running total total = total + input; // calculate the average; this is integer arithmetic so fractional parts will be truncated return total / WINDOW_SIZE; }
median.ino
int median_3_filter
(int c)
Reduce signal outliers using a non-linear median filter with a fixed width of three samples. Two integer values of state are required. The input signal is typically delayed by one sample period.
// median.ino : platform-independent three-sample median filter // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. // Return the median of three integers, i.e. the middle value of the three. // There are six possible sorted orderings from which to choose: ABC, ACB, BAC, // BCA, CAB, CBA. Note that equality can make some of these cases equivalent. int median_of_three(int a, int b, int c) { if (a < b) { if (b < c) return b; // ABC else if (a < c) return c; // ACB else return a; // CAB } else { if (a < c) return a; // BAC else if (b < c) return c; // BCA else return b; // CBA } } //================================================================ // Reduce signal outliers using a non-linear median filter with a fixed width of // three samples. Two integer values of state are required. The input signal // is typically delayed by one sample period. int median_3_filter(int c) { // The previous state is kept in a static variable; this means this function // can only be applied to a single input stream. static int a = 0, b = 0; // previous two inputs in sample order int median = median_of_three(a, b, c); a = b; b = c; return median; } //================================================================
lowpass.ino
float lowpass
(float input)
Reduce high-frequency components to smooth a signal. Four float values of state are required.
This file was automatically generated using filter_gen.py and the SciPy toolkit. In general you will need to regenerate the filter for your particular application needs.
The plot shows the low-pass signal transfer ratio as a function of frequency. Please note this is plotted on a linear scale for clarity; on a logarithmic scale (dB) the rolloff slope becomes straight.
// Low-Pass Butterworth IIR digital filter, generated using filter_gen.py. // Sampling rate: 10 Hz, frequency: 1.0 Hz. // Filter is order 4, implemented as second-order sections (biquads). // Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.butter.html float lowpass(float input) { float output = input; { static float z1, z2; // filter section state float x = output - -1.04859958*z1 - 0.29614036*z2; output = 0.00482434*x + 0.00964869*z1 + 0.00482434*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.32091343*z1 - 0.63273879*z2; output = 1.00000000*x + 2.00000000*z1 + 1.00000000*z2; z2 = z1; z1 = x; } return output; }
highpass.ino
float highpass
(float input)
Reduce low-frequency components to remove constant and slowly-changing components of a signal. Four float values of state are required.
This file was automatically generated using filter_gen.py and the SciPy toolkit. In general you will need to regenerate the filter for your particular application needs.
The plot shows the high-pass signal transfer ratio as a function of frequency. Please note this is plotted on a linear scale for clarity; on a logarithmic scale (dB) the rolloff slope becomes straight.
// High-Pass Butterworth IIR digital filter, generated using filter_gen.py. // Sampling rate: 10 Hz, frequency: 1.0 Hz. // Filter is order 4, implemented as second-order sections (biquads). // Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.butter.html float highpass(float input) { float output = input; { static float z1, z2; // filter section state float x = output - -1.04859958*z1 - 0.29614036*z2; output = 0.43284664*x + -0.86569329*z1 + 0.43284664*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.32091343*z1 - 0.63273879*z2; output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2; z2 = z1; z1 = x; } return output; }
bandpass.ino
float bandpass
(float input)
Remove the components of a signal which fall outside a specific frequency band. Eight float values of state are required.
This file was automatically generated using filter_gen.py and the SciPy toolkit. In general you will need to regenerate the filter for your particular application needs.
The plot shows the band-pass signal transfer ratio as a function of frequency. Please note this is plotted on a linear scale for clarity; on a logarithmic scale (dB) the rolloff slopes become straight.
// Band-Pass Butterworth IIR digital filter, generated using filter_gen.py. // Sampling rate: 10 Hz, frequency: [0.5, 1.5] Hz. // Filter is order 4, implemented as second-order sections (biquads). // Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.butter.html float bandpass(float input) { float output = input; { static float z1, z2; // filter section state float x = output - -1.10547167*z1 - 0.46872661*z2; output = 0.00482434*x + 0.00964869*z1 + 0.00482434*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.48782202*z1 - 0.63179763*z2; output = 1.00000000*x + 2.00000000*z1 + 1.00000000*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.04431445*z1 - 0.72062964*z2; output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.78062325*z1 - 0.87803603*z2; output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2; z2 = z1; z1 = x; } return output; }
bandstop.ino
float bandstop
(float input)
Remove the components of a signal which fall inside a specific frequency band. Eight float values of state are required.
This file was automatically generated using filter_gen.py and the SciPy toolkit. In general you will need to regenerate the filter for your particular application needs.
The plot shows the band-stop signal transfer ratio as a function of frequency. Please note this is plotted on a linear scale for clarity; on a logarithmic scale (dB) the rolloff slopes become straight.
// Band-Stop Butterworth IIR digital filter, generated using filter_gen.py. // Sampling rate: 10 Hz, frequency: [0.5, 1.5] Hz. // Filter is order 4, implemented as second-order sections (biquads). // Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.butter.html float bandstop(float input) { float output = input; { static float z1, z2; // filter section state float x = output - -1.10547167*z1 - 0.46872661*z2; output = 0.43284664*x + -0.73640270*z1 + 0.43284664*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.48782202*z1 - 0.63179763*z2; output = 1.00000000*x + -1.70130162*z1 + 1.00000000*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.04431445*z1 - 0.72062964*z2; output = 1.00000000*x + -1.70130162*z1 + 1.00000000*z2; z2 = z1; z1 = x; } { static float z1, z2; // filter section state float x = output - -1.78062325*z1 - 0.87803603*z2; output = 1.00000000*x + -1.70130162*z1 + 1.00000000*z2; z2 = z1; z1 = x; } return output; }
ring_buffer.ino
// ring_buffer.ino : fixed-length sample history buffer useful for finite filters const unsigned int RING_LENGTH = 10; float ring_buffer[RING_LENGTH]; // circular buffer of samples unsigned int ring_position = 0; // index of the oldest sample // Put a new value into a circular sample buffer, overwriting the oldest sample. void ring_buffer_put(float value) { if (ring_position < RING_LENGTH) ring_buffer[ring_position] = value; if (++ring_position >= RING_LENGTH) ring_position = 0; } // Calculate the first derivative as the finite difference between the newest // and oldest values. The delta_t parameter is the sampling interval in // seconds. float ring_buffer_deriv(float delta_t) { float oldest = ring_buffer[ring_position]; float newest = (ring_position < RING_LENGTH-1) ? ring_buffer[ring_position+1] : ring_buffer[0]; return (newest - oldest) / ((RING_LENGTH-1) * delta_t); }
ring_median.ino
// ring_median.ino : platform-independent median filter on ring buffer // No copyright, 2020, Garth Zeglin. This file is explicitly placed in the public domain. // Note: this assumes the data is held in a global ring buffer, see ring_buffer.ino. float median_buffer[RING_LENGTH]; // working buffer of samples copied from ring_buffer //================================================================ // Utility function for the sorting function. int float_compare(const void *e1, const void *e2) { float f1 = *((const float *) e1); float f2 = *((const float *) e2); if (f1 < f2) return -1; else if (f1 == f2) return 0; else return 1; } //================================================================ // Reduce signal outliers using a median filter applied over a ring buffer. // No additional state is required, but uses the global ring_buffer array. float ring_median_filter(void) { // copy and sort the ring buffer samples memcpy(median_buffer, ring_buffer, sizeof(median_buffer)); qsort(median_buffer, RING_LENGTH, sizeof(float), float_compare); // return the median element return median_buffer[RING_LENGTH/2]; } //================================================================
trajfit.ino
This file contains a causal finite filter to estimate trajectory parameters by fitting a quadratic curve to the samples in a ring buffer. The result is an estimate of position, velocity, and acceleration for the most recent sample. The filter coefficients were generated using trajfit_gen.py and the SciPy toolkit.
In general you will need to regenerate the filter for your particular application needs; the coefficients encode the sampling rate for correct scaling of the output, and the best window size depends upon your application.
// Trajectory estimation filter generated using trajfit_gen.py. // Based on Savitzky-Golay polynomial fitting filters. // Sampling rate: 10 Hz. // The output array will contain the trajectory parameters representing the signal // at the current time: [position, velocity, acceleration], with units of [1, 1/sec, 1/sec/sec]. // Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_coeffs.html void trajfit(float input, float output[3]) { const float coeff[3][5] = {{ 0.085714, -0.142857, -0.085714, 0.257143, 0.885714}, { 3.714286, -3.857143, -5.714286, -1.857143, 7.714286}, { 28.571429, -14.285714, -28.571429, -14.285714, 28.571429}}; static float ring[5]; // buffer for recent time history static int oldest = 0; // index of oldest sample // save the new sample by overwriting the oldest sample ring[oldest] = input; if (++oldest >= 5) oldest = 0; // iterate over the coefficient rows int index = oldest; for (int i = 0; i < 3; i++) { output[i] = 0.0; // clear accumulator // Iterate over the samples and the coefficient rows. The index cycles // around the circular buffer once per row. for (int j = 0; j < 5; j++) { output[i] += coeff[i][j] * ring[index]; if (++index >= 5) index = 0; } } }
Development Tools
The development of this sketch involves several other tools which are not documented:
- A Python script for generating digital filters using SciPy: filter_gen.py
- A Python script for generating the trajfit filters using SciPy: trajfit_gen.py
- An Arduino sketch to capture testing data: RecordSonar.ino
- A customizable Python script for capturing a serial data stream: record_Arduino_data.py
- A C++ program for offline testing of the filter code using the test data: test_filters.cpp
- A Python matplotlib script for generating figures from the test data: generate_plots.py
- A set of recorded and computed test data files: data/
- A set of plotted figures of the test data files: plots/
The Python scripts use several third-party libraries:
- pySerial: portable support for the serial port used for Arduino communication
- SciPy: comprehensive numerical analysis; linear algebra algorithms used during filter generation
- Matplotlib: plotting library for visualizing data
Source: Arduino Sketch FilterDemos