FilterDemos Arduino Sketch

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:

1. A Python script for generating digital filters using SciPy: filter_gen.py

2. A Python script for generating the trajfit filters using SciPy: trajfit_gen.py

3. An Arduino sketch to capture testing data: RecordSonar.ino

4. A customizable Python script for capturing a serial data stream: record_Arduino_data.py

5. A C++ program for offline testing of the filter code using the test data: test_filters.cpp

6. A Python matplotlib script for generating figures from the test data: generate_plots.py

7. A set of recorded and computed test data files: data/

8. A set of plotted figures of the test data files: plots/

The Python scripts use several third-party libraries:

1. pySerial: portable support for the serial port used for Arduino communication

2. SciPy: comprehensive numerical analysis; linear algebra algorithms used during filter generation

3. Matplotlib: plotting library for visualizing data