Filtering out outliers in Arduino ultrasonic distance sensor data

A friend of mine asked me how he could remove invalid data points from the measurements made by the ultrasonic range finder in his Arduino project. A quick search got me to this page, but some of the links were dead and there was no complete example on the page anyway, so I decided to roll my own.

Here’s my take, written in Python since I didn’t have an Arduino handy for testing. This is based on dropping outliers from recent measurements to come up with a “fixed average”:

#!/usr/bin/env python

# Example on filtering out invalid data points in a stream, coming in from for
# example an ultrasonic distance sensor.

# This test data has two invalid data points; 310 and 210
data = [40, 41, 40, 39, 42, 45, 310, 43, 41, 44, 38, 120, 121, 117, 120, 120, 210, 110, 112, 114, 117, 113, 78, 80, 78, 74, 60, 70, 76]
raw_values = [];

while data :
  # Read a raw value
  raw_value = data.pop()

  # Keep last 5 values stored in an array
  raw_values.append(raw_value)
  if len(raw_values) > 5:
    raw_values.reverse()
    raw_values.pop()
    raw_values.reverse()
  
  # For debugging only; Print out current stored raw values
  for raw_value in raw_values:
    print raw_value,
  
  # Calculate the raw average of all the stored values; This value will be
  # thrown off by invalid values.
  avg = round(sum(raw_values) / len(raw_values))
  print ":",
  print avg,

  # Drop the highest and lowest stored value if we have enough values.
  sorted = list(raw_values);
  if len(raw_values) >= 3:
    sorted.sort()
    sorted.pop()
    sorted.reverse()
    sorted.pop()

  # And calculate an average based on the remaining three values. This is
  # closer to correct, but will be thrown off if there is more than one
  # invalid value currently stored.
  fixed_avg = round(sum(sorted) / len(sorted));

  print ":",
  print fixed_avg,
  
  # Print the difference between the raw average and the fixed average:
  print "(",
  print abs(avg - fixed_avg),
  print ")"