In my last post I did some experimenting with a low pass filter in Excel. But then there is reality. The US sensor is a slow sensor, I can’t use the sample rate I experimented with. I found out that RobotC, my programming environment, reads the US sensor about every 70 ms. This is a lot slower than the 4 ms I used in my experiment. As a result the filter responds far too slow as you can see in the first image. FreeRover would hit a wall ansd still be thinking it was over 20 cm away. If I increase Alpha the filter reacts faster but it will not be as effective as a filter. I needed something else.
The filter described in my last post and above is a first order filter, meaning it uses the current reading and the previous result. Second order filters also use the two previous readings. I started experimenting with a second order filter. Here is an image of the results.
Again, the red line represents raw readings, the green line is the result of a first order filter, the blue line is the result of a second order filter. Thetime axis has changed scale as I use a sample rate of once every 0,070 ms. There are five scenario’s. Scenario 1 and 3 are faulty readings of zero. Scenari 4 is a faulty reading of 255. I won’t go into much detail of these scenario’s, but notice that the second order filter is about as responsive as the first order filter but is more effective in filtering out spike errors.
Scenario 5 is the most interesting one. Here, the robot drives into a wall at a speed of 100 cm/second. This is quite fast for a robot, almost the speed of a walking man. Both filters respond with some delay, this means that the real distance to the wall is less then one might think based on filtered values. Upon hitting the wall the first order filter gives a distance of 5,5 cm, the second order gives 6,3 cm. The size of the lag can be altered by altering alpha and beta but will also depend on the speed of the robot.
I conclude that it is important to know the sensor, the kind of errors it gives and the sampling rate it can handle. It also is important to know the robot, how fast it will go. And it is important to tune the filter. This is best done in a spreadsheet at first.
Here is my code for a second order low pass filter for a US sensor.
float lowPassUS;
task SonarLowPass()
{
// sensor reading speed
int deltaT=70;
float alpha=0.2, beta=0.18;
float US0=0,US1=0, US2=0;
// Start main loop of the task;
while(true)
{
wait1Msec(deltaT);
// low pass value of US sensor
US0=(float)SensorValue[sonar];
lowPassUS=US1+alpha*(US0-US1)+beta*(US0-US2);
US2=US1;
US1=lowPassUS;
}
}
My steering geometry was square. The steering wheels were always parallel to each other. However, if a car is cornering the outside wheel is making a large circle, the inside wheel is making a small circle. So, the steering angle of the outside wheel should be less than that of the inside wheel. This can be achieved with a steering geometry called 
