## Introduction

Mr. Stitson has an ultrasonic sonar mounted in the front. namely a HC-SR04; it is inexpensive, and ubiquitous, but unfortunately not quite reliable. I have a single reason for having a sonar installed, I want to keep away my boys playing the “bumping” game, what is, according to some scientists, has an unhealthy effect on robots.

To avoid hitting the wall, in the main loop there is some logic that, based on the current speed and the latest sonar readings, slows down the vehicle.

## Noisy sensor data

Not rocket science, but it works reasonably well. until the sonar returns valid data. Unfortunately, this is not the case. Standing still, these are common sonar readings:

``````30
30
30
31
5
29
31
31
5
33
4
30
31
31
31
31
31
30
30
30
30
``````

I do not care about the slight fluctuation around 30 cm, but the 4 and 5 cm readings (it is called impulse noise I believe) are showstoppers, they immediately let the robot stop. Most people would cry Kalman filter here, but that would be like using a sledgehammer to crack a nut; a simple median filter will do the job perfectly.

## Implementing the median filter

Such erroneous readings appear quite often, but I did not observe them more than twice in a row. A median filter takes the median value from the last N readings (a.k.a the window size), N = 5 is enough to filter out two extremely bad readings (like we have here) in a row.

All we have to do is to sort the last five readings and pick up the third one. First, store the last readings in a cyclic buffer:

``````// The cyclic buffer
// MAX_DISTANCE will be the the result until the filter warms up
for(int i=0; i<5; i++) lastReadings[i] = MAX_DISTANCE;
// Number of readings since the beginning

// Add an element to the cyclic buffer
``````

Next, we need a function to calculate the median value based on this buffer (do not sort the buffer itself, then you loose cyclicity). That can be done very nicely and efficiently as there exist optimal sorting network for N = 5 which uses only nine comparisons (and one can be trivially ignored as we need the third element only). There is also an algorithm for computing the median value of five elements with six comparisons, but that is anything, but nice and I want to avoid premature optimization anyway.

``````#define swap(a,b) a ^= b; b ^= a; a ^= b;
#define sort(a,b) if(a>b){ swap(a,b); }

int median(int a, int b, int c, int d, int e)
{
sort(a,b);
sort(d,e);
sort(a,c);
sort(b,c);
sort(a,d);
sort(c,d);
sort(b,e);
sort(b,c);
// this last one is obviously unnecessary for the median
//sort(d,e);

return c;
}
``````

## Remarks

The actual code used in the robot can be found here: https://github.com/domoszlai/robotcar/blob/master/sonarnp.cpp

Some remarks about the code. It is based on the NewPing library. The class continuously reads the sonar (every 33 milliseconds only to avoid interference with the previous measurement) in an asynchronous manner. The measure() method always returns immediately with the latest (filtered) measurement. Although the results come asynchronously using a timer based mechanism, the measurements must be initialized from the main “thread” in the loop() method. This loop() method is part of the green thread library I use (notice the Thread base class). Read the comments in the code.

Finally, some remarks to median filter.

Median filters introduce a delay proportional to N. For N = 5, the filter is on average two values behind the actual measurements, that is ~60 milliseconds. I have to be only faster than my kids, so it is acceptable for me. If you think you have real-time requirements, then buy a proper sonar and use Kalman filter. If you want to smooth the sensor data fluctuations, use a Kalman filter. Median filters are very efficient, much faster than Kalman filters; if you do not have any of the previous requirements, stick with the median one.