### Introduction

Mr. Stitson - Front view with the sonar |

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

30 30 30 31 5 29 31 31 5 33 4 30 31 31 31 31 31 30 30 30 30I 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 int lastReadings[5]; // 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 int nrReadings = 0; // Add an element to the cyclic buffer lastReadings[nrReadings++ % 5] = sonar->read();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.

This article has some nice figures related to the topic.

### Conclusion and future work

I'm quite satisfied with the current implementation, it solves my main problem in a very efficient way with minimal effort. It may make sense, though, to smooth the sensor data as well. Furthermore, I definitely want to implement prediction of future data. These can be done by Kalman filters. However, although, Kalman filter is an incredible tool for sensor fusion, when one can provide a proper physical model, I feel it too much otherwise. I would rather try something new, like alpha-trimmed mean filters. Or just a simple linear prediction on top of the median filtered data.

As for the sensor itself, I think ultrasonic sensors are not the best tool to avoid obstacles, they just do not provide enough information for a proper plan. I will rather try using two cameras for creating stereoscopic 3D images. Of course, than I need a proper processor, but this is my plan anyway. Lidars seem also very great, but they are a way too expensive just for having fun.