My team has been learning more-sophisticated programming of late, and I thought I’d share one topic we recently dove into: filtering noise out of sensor data. It started when we were trying to use an ultrasonic sensor to drive the robot straight down the field, keeping a constant distance from the field perimeter (ultrasonic sensor attached to the side of the robot). We were making use of a PID algorithm, increasing or decreasing power to one side of the drive train depending on whether the robot was further away from or closer to the perimeter than our chosen target.

We were using the RobotC datalog (as I recommend everyone use when writing a PID loop) and looking at the error value each time through the loop. It looked something like this:

Iteration Error
1 0
2 0
3 251
4 251
5 1
6 1

Uhhhhh… problem? And that’s when I learned that certain VEX sensors—including the ultrasonic, gyro, accelerometer, and potentiometer—have a tendency to return “noisy” data. This fact makes the sensor distinctly less useful.

## What to Do

Based on what we saw in the table above, without the ability to filter data, the concept of using the ultrasonic sensor to drive straight would be pretty much a non-starter. I mean, how can you adjust left- and right-side power to maintain a fixed distance from the wall, if you randomly get complete junk? You just can’t.

That’s when I started researching techniques for filtering sensor data. I’ll cut to the chase here, since you’re probably reading this article in order to save yourself some time and background reading: For simple filtering of data like this instance, a median filter is just fine. A search of the VEX Forum will return many threads mentioning things like Kalman filters, and (a) from what I’ve read and (b) my experience using it with the ultrasonic sensor, I’ll confirm that a median filter worked perfectly. (For background, a Kalman filter is useful when you’ve got a complex, dynamical system with multiple sensors providing input.)

So what is a median filter? It’s simply using the median of the previous x-number-of-values as a replacement for junky data. From other research I did on the VEX Forum, people have indicated that these noisy sensors rarely produce more than 2 junky pieces of data in a row. Since these PID iterations are taking place over a VERY small period of time, using replacement values for 1 or 2 numbers every now and then does not cause undue problems (or any, in our usage of it thus far).

## Filtering Steps

The programming steps for filtering the data are like so:

1. Keep a running array of the most recent 5 sensor values.
2. When the sensor value of the current iteration is sufficiently far off from your chosen target (or the previous value), you say, “Hey, that’s junk.”
3. Calculate the median of the 5 stored values.
4. Use that median in the current PID loop.
5. Store that median in the array instead of the junky data actually returned from the sensor.

### 1. An Array of Rolling Values

Step 1 is where the hard part started. There’s nothing built into easyC or RobotC to do this, so you have to write this yourself. Start by defining an array to hold 5 values:

`int ultrasonicArray;`

In picture format, we’ve just created the following (recalling that when programming, the first item in the array has an index of 0): With data in it, we’ve got something like this: Each time we go through the loop, we need to shift each value one box to the right so that our new piece of data can slot into position 0: If I start by putting data item “f” into the first array “box”, then f will overwrite e before I have a chance to move e over into box 1. So, this moving-over-of-the-data must start by moving the box 3 value (b) into box 4, and then working my way backward until I can put f (the current sensor value) into box 0, like so:

```void rollingValues() {
for (int i = 4; i > 0; i--) {
ultrasonicArray[i] = ultrasonicArray[i-1];
}
ultrasonicArray = currentValue;
}```

OK, Step 1 complete.

### 2. Evaluating the Current Sensor Reading

This part was relatively easy for us because we wanted to drive our robot to keep a constant distance between it and the wall. We used the following statement, saying (arbitrarily, really), if the current sensor value differs from the previous value by ±5 centimeters, it’s junk. Moving 5 centimeters away from the wall in the span of 20-50 milliseconds is a good indication that something is wrong.

```currentValue = SensorValue[SonicSensor];
if (currentValue > ultrasonicArray + 5) ||
(currentValue < ultrasonicArray - 5) {
// replace this value with a better value (filter it out)
}```

#### Other Sensor Uses

If you are filtering other types of sensor data, which are not expected to remain constant over time (such as using a gyro to turn, using a potentiometer to lift an arm to a desired height, or using an ultrasonic to stop your robot a certain distance from a target), you’ll have to modify this statement to set your own terms for what qualifies as “junk data”. Do some testing of your own. What’s the average change from iteration to iteration of your sensor value? What qualifies as “too far off”? You’ll have to determine this value based on your own robot and your own usage, and modify the if-statement above accordingly.

### 3. Calculate the Median of the Rolling Array

This part was hard too. RobotC (and, I think easyC as well) has no built-in median function, and Lo! no built-in sorting function either. You have to write your own. Here again I’ll cut to the chase. After doing a lot of reading, I found in several different places people saying that if you have 5 values or less to sort, the following algorithm is it. Look no further, this is the preferred method.

The coding here makes use of RobotC macros; you could just as easily write these as separate functions, and call the functions, but I took the easy route and copied & pasted various other people’s code (hey, steal from the best).

These macros/functions are as follows:

```int temp;
#define swap(w, z) temp = w; w = z; z = temp;
#define sort(x, y) if(x < y) { swap(x, y); }```

The first macro here swaps 2 values it’s given: put the first value into a temporary variable, assign the 2nd value to the first variable, then put the temporary value into the 2nd variable.

The second macro above sorts 2 values, saying: if the second one is greater than the first one, call the swap macro and switch them.

OK, but how does that help me sort an array of 5 values? The beauty is of it is in the nifty pair-wise steps below.

Note: I want to sort my array so that I can choose the middle/median value. HOWEVER, I don’t want to overwrite my running-values array. If I did, then it wouldn’t be a record of my past sensor values; it would be jumbled up. So this code creates temporary variables (a through e) and sorts *them*, and leaves the values in the array untouched.

```int ultrasonicMedian(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 unnecessary for the median
//sort(d,e);

return c;
}```

This code goes piece-wise through pairs of values of the array, and swaps them as needed to arrive at the sorted answer (descending sort order). Since a picture is worth 1,000 words, here’s a whole bunch of thousands-of-words. Let’s say we have the following data: The first comparison in the code above looks at a and b; b is greater than a, so they are swapped. Now we have: Next, we compare d and e: e > d, so they are swapped, resulting in: Next, we compare a and c: c > a, so they are swapped: Moving on, compare b and c next: c > b, so they are also swapped, leaving us with: Next compare a and d; a is greater than d, so no change. After that, we compare c and d; since d > c, they are swapped, giving: The next comparison is b versus e. Since b > e, then no change. Finally we compare b and c, and again no change.

You’ll notice in the code above that the last comparison, between d and e is not performed. Why is that? Well, all we care about in this process is box c above, which is the median. Comparing and possibly swapping boxes d and e at this point will never change what’s in box c, so we get to skip this last step. Yay!

In the code above, the very last step is to return the value of c, the median.

### 4. Use the Median Value in the PID Loop

Returning to the snippet of code shown in Step 2 and including a call to the functions in Step 3 we get (split into several lines for display purposes here):

```currentValue = SensorValue[SonicSensor];
if (currentValue > ultrasonicArray + 5) ||
(currentValue < ultrasonicArray - 5) {
currentValue = ultrasonicMedian(
ultrasonicArray, ultrasonicArray,
ultrasonicArray, ultrasonicArray,
ultrasonicArray
);
}```

#### Other Sensor Uses

Again, if you’re using sensors for a purpose different from that described here, using the median may not be *quite* what you’re looking for. If you’re using a gyro to turn, then using one of your previous sensor values will essentially tell your program that you’ve gone backward.

Again again: do your own testing. What’s your average rate of turning? You may want to take the median value and multiply it by a constant to generate a better replacement for your junky data value. Or you could calculate the average slope of the previous x-number-of-changes, and apply that slope to the previous sensor reading to estimate the current sensor value. You get the idea; you need to think this through based on your specific use of the sensor. (And in the end, you may conclude that you need a more sophisticated type of filter instead of the median filter described here.)

### 5. Store the Median in the Array of Running Values

In the step above, you’ll see that if the current value is outside our acceptable range, then the sensor value is overwritten with the median in the currentValue variable, and it will get slotted into the running-values array when the new data is added and the others are shifted down (Step 1). In order to incorporate the latest value, we call the following, at the very end of the loop:

`rollingValues();`

## Putting It All Together

What I’ve shown above includes a handful of things without the surrounding context of what order things are actually used in your program. So here they are all together in one batch. I’ve inserted some comments in lieu of code for parts of the PID calculation not related to filtering.

```int ultrasonicArray;
int currentValue;

// Swap two variables
int temp;
#define swap(w, z) temp = w; w = z; z = temp;
#define sort(x, y) if(x > y) { swap(x, y); }

// Median calculation
int ultrasonicMedian(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 unnecessary for the median
// sort(d,e);

return c;
}

// keep track of last 5 values;
// gets called each time through the PID loop
void rollingValues() {
for (int i = 4; i > 0; i--) {
ultrasonicArray[i] = ultrasonicArray[i-1];
}
// put the current value in the first position
ultrasonicArray = currentValue;
}

// set PID constants Kp, Ki, and Kd as usual here

//PID Driving function
void UltraSonicDriving(int leftPower, int targetClicks,
int targetDistance) {

// reset all important values here, before you start

// drive straight until encoder clicks reach target
while(abs(SensorValue[backLeftENC]) < targetClicks) {

currentValue = SensorValue[SonicSensor];

// if this sensor value is different from the last value
// by more than 5, replace with median
if (currentValue > ultrasonicArray + 5) ||
(currentValue < ultrasonicArray - 5) {
currentValue = ultrasonicMedian(
ultrasonicArray,
ultrasonicArray,
ultrasonicArray,
ultrasonicArray,
ultrasonicArray
);
}

// calculate error, integral, derivative as usual;
// here we would compare currentValue to the
// targetDistance to calculate error

// put them all together, as usual
rightPower = leftPower +
error * Kp + integral * Ki + derivative * Kd;

// our chassis drive function
driveFunc(leftPower, rightPower);

//don't hog CPU
wait1Msec(50);

// update array of sensor values
rollingValues();
}

// stop once targetClicks achieved
driveFunc(0,0);
}