More About Sonar Sensors

Unfortunately, further tests with multiple sonars show that the sonars have a lot of errors, mostly when not pointed directly at a flat surface. That is, if the angle of the sensor to a wall is close to the normal, the sensors are quite accurate. As the angle from normal increases, the incidence of spurious large distances also increases.

As well, there appear to be false echoes off the ground – since my robot has little ground clearance – when the actual distances increase. You can see that from this graph, which shows sonar distances while the robot is moving. The robot path is shown as the blue plot. The sonar positions are left-side (about 80° to the left of straight ahead), left-front (about 10° to the left), right-front (10° to the right), and right-side (80° to the right).

A low-pass filter could be used, but it's important to realize that the errors are generally large errors, not small random errors. A different approach is to ignore outliers. I tried this approach, which uses a running average to give a little smoothing to eliminate some wiggles, while not delaying the filtered data very much.

if (newValue is within threshold of filteredValue) {
    filteredValue = 0.5*filteredValue + 0.5*newValue;
} else if (newValue is within threshold of lastValue
           && lastValue is within threshold of nextToLastValue) {
    filteredValue = 0.33*nextToLastValue + 0.33*lastValue + 0.33*newValue;
}

The threshold value depends on the maximum speed of the robot and the sample rate. Here I was sampling at 4Hz with a maximum robot speed of about 0.25m/s. In 250ms the robot can move about 6cm. Using a threshold of double that, or 12cm, filters out most of the noise. As you can see from the algorithm above, 3 measurements in a row that are close together is enough to allow the distance to jump to a new value, such as happens when encountering the edge of a wall.

The smoothed data is shown below. They seem good enough to make navigation decisions. There are residual changes in distance, but I believe they are legitimate, or at least they are large enough distances that they can be ignored. Short distances seem not to have spurious jumps.