So I started with a library program that read data from my sharp IR distance sensor. I modified the program to print the data to teraterm, with some advice from Erik O. Then I modified it to turn a servo based on the sensor data. Then I modified it with a servo calibration routine. So far so good. I noticed a lot of chatter in the readings and the servo movement, so I figured I should filter the sensor data. I am shooting from the hip on this one. First I added what I call a spike filter which detects and averages out sensor data spikes. That filter data goes into an averaging filter which averages the last 5 values. PWM is then calculated with that despiked averaged reading. It all seems to work, though I do still get some chatter and spikes. I could smooth it out by averaging more values, and changing the threshold of my spike filter, increasing overall latency. I am wondering if somebody could have a quick look at my code and see if I am doing a good job so far :) I am curious if there is a more standard way to do this. After I created my method I decided to search the library for a filter routine. I did find an analog filter of some sort, but don't really understand it at this point. So far I am proud of my little achievements here! I will post my code below. Thanks!

/* Sharp Sensor Test
 * Connections:
 * PIN15          (White) Signal
 * 5.0V USB Out   Sensor Power (+)
 * GND            Sensor Gnd   (-)
 * Place a 10uf cap close the the sensor between VCC nad GND
 */
#include "mbed.h"

/* Standard LED's on the mbed */
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

Serial pc(USBTX, USBRX); // tx, rx /

/* Our signal line is PIN15 */
AnalogIn   Sensor(p15);
PwmOut servo(p21);

int main() {
    
    servo.period(0.020);          // servo requires a 20ms period 
    
    /**********Begin Servo Range Calibration******************/
    float UPL = 0.0021; /*Initialize PWM Upper Limit*/
    float LPL = 0.0009; /*Initialize PWM Lower Limit*/
    char c = 'z'; /*Initialize char for calibtation inputs*/
 
    /*Calibration Loop for Upper PWM Limit*/
    servo.pulsewidth(UPL);
    pc.printf("\n***Calibration of Upper PWM Limit***");
    pc.printf("\nPress 'u' to increase, 'd' to decrease,  'q' to calibrate Lower PWM Limit\n");
    while( c != 'q') 
    {
        c = pc.getc();
        if(c == 'u') UPL += 0.0001;
        if(c == 'd') UPL -= 0.0001;
        servo.pulsewidth(UPL);
    }
    /*Calibration Loop for Lower PWM Limit*/
    servo.pulsewidth(LPL);
    pc.printf("***Calibration of Lower PWM Limit***");
    pc.printf("Press 'u' to increase, 'd' to decrease,  'q' to quit\n");
    c = 'z';
    while( c != 'q') 
    {
        c = pc.getc();
        if(c == 'u') LPL += 0.0001;
        if(c == 'd') LPL -= 0.0001;
        servo.pulsewidth(LPL);
    }
    pc.printf("\nUpper PWM Limit: %f   Lower PWM Limit: %f",UPL,LPL);
    
    /*converting Analog pin reading (0-1) to servo PWM range as calibrated*/
    /*sensor producing pin range from .06 to .85*/
    float x; /*extrapolated lower limit of PWM range*/
    float y; /*extrapolated upper limit of PWM range*/
    float UAL = 0.850; /*observed upper limit of pin reading from sensor*/
    float LAL = 0.060; /*observed lower limit of pin reading from sensor*/
    x = UPL-(((UAL)*(UPL-LPL))/(UAL-LAL)); /*extrapolation for x*/
    y = LPL+(((1-LAL)*(UPL-LPL))/(UAL-LAL)); /*extparolation for y*/
    /*checking if the extrapolation is correct*/
    pc.printf("\nFor UAL=.850, UPL: %f=%f",UPL,(0.850*(y-x)+x));
    pc.printf("\nFor LAL=.060, LPL: %f=%f",LPL,(0.060*(y-x)+x)); 
    c = 'z';
    while (c != 'y')
    {
        pc.printf("\nenter 'y' to continue\n");
        c = pc.getc();
    }
    /****************End Servo Range Calibration********************/
    
    /****************Begin filtering sensor readings****************/
    int count = 4; /*count for spike filter*/
    float ss = 0.60; /*spike error threshold percentage*/
    float sp[5]={}; /*initial array for sensor values for spike filter*/
    float cum = 0; /*initial cumulative value for moving average filter*/
    float old = 0; /*initial value for placeholder used in moving average filter*/
    int i = 0; /*initial value for counter in moving average filter*/
    int as = 5; /*size of array for moving average filter*/
    float sensvals[5]={};
    float sensvalavg;
    float avg0134;
    float sensval;
    
    while(count>0) /*loop to fill sp[0-3] with sensor values, done to initialize the Spike filter*/
        {
        sensval=Sensor.read();
        sp[count-1]=sensval;
        count-=1;
        }

    float PWMVal;  
          
    while (1) {
        /*****filter continues here inside main loop**********/
        sensval=Sensor.read();
        sp[4]=sp[3]; /*moving values down the array and filling sp[0] with the newest data point*/
        sp[3]=sp[2];
        sp[2]=sp[1];
        sp[1]=sp[0];
        sp[0]=sensval;
        avg0134 = (sp[0]+sp[1]+sp[3]+sp[4])/4; /*taking avg of the array values except position 2*/
        if (sp[2] > (1+ss)*avg0134 || sp[2] < (1-ss)*avg0134) /*comparing position 2 with the avg to see*/
            {                                     /*if it is > or < the ss percentage, if so, use avg0134*/
            sp[2]=avg0134; 
            pc.printf("\n***********SPIKE************SPIKE************");
            }
        old = sensvals[i];
 /* debug      pc.printf("\ncum1= %f   old1=%f",cum,old);  */
        cum = cum+sp[2]-old;  /*moving average filter*/
        sensvals[i]=sp[2];    /*moving average filter*/
        i+=1;                 /*moving average filter*/
        if (i>as-1) i=0;      /*moving average filter*/
            
 /* debug       pc.printf("\n%f %f %f %f %f",sp[0],sp[1],sp[2],sp[3],sp[4]);
                pc.printf("\n%f %f %f %f %f %f %f %f %f %f",sensvals[0],sensvals[1],sensvals[2],sensvals[3],sensvals[4]
                                                           ,sensvals[5],sensvals[6],sensvals[7],sensvals[8],sensvals[9]);
                pc.printf("\ncum= %f  old=%f  i=%f  sensvals[i]=%f  sp2= %f",cum,old,i,sensvals[i],sp[2]);  */ 
             
        sensvalavg = cum/as; /*moving average filter*/
    /****************End filtering sensor readings******************/
    
        PWMVal=(sensvalavg*(y-x)+x); /*conversion to PWM*/
        if (PWMVal >=UPL) PWMVal=UPL;  /*limits servo movement per calibration routine*/
        if (PWMVal <=LPL) PWMVal=LPL;  /*limits servo movement per calibration routine*/   
        servo.pulsewidth(PWMVal); /*sending out the final PWM value*/
 /* debug      pc.printf("\n SenseValAvg: %f    PWMVal: %f\n",sensvalavg,PWMVal);  */       
            
        /*
         * Efficient way as the object gets closer to the
         * sensor the LED's will turn on, as the object
         * moves away they count back down. Original by Dan Ros
        */
        led1 = (Sensor > 0.2) ? 1 : 0;
        led2 = (Sensor > 0.4) ? 1 : 0;
        led3 = (Sensor > 0.6) ? 1 : 0;
        led4 = (Sensor > 0.8) ? 1 : 0;
        /*
         * A cool effect when the full range of the sensor
         * has been reached, all LED's flash
        */
        if (Sensor  >= 0.9) {
            led1 = led2 = led3 = led4 = 1;
            wait(0.02);
            led1 = led2 = led3 = led4 = 0;
            wait(0.02);
        }

        wait(0.002);

    }
}

So if I am following this right then you have a moving average filter in which you hold the last 5 samples in. You then check if the middle value is within some limits and if not replace it with the average of the other 4. This is the value then used. This seems OK.

In the past I have used a median filter that reads in a number of values in a short space of time and then sorts then in value and returns the middle value (the median average). This seems to work quite well, maybe it is something you could have a look at?

There was an forum topic here on median filtering.

Martin

Hi Martin, Thank you for looking at my messy code, I did my best to make it look nice. Yes, first I read in 5 points, then compare the middle point to the surrounding 4. if the middle point is larger or smaller by 60% then I replace it with the average of the other 4, so that is my spike filter. then that middle value gets passed to the next filter where it and the next 4 values are avaraged.

Your comment about the median filter makes me think. You say you read them in a short period of time, and here I realize that I am reading data inside the main while loop with the wait command in it. It doesn't seem like the best thing now that I think about it. The wait was helping me control the rate of data I see on the terminal screen, but it also affects the sampling rate. I like your filter idea and will try it, though it seems about the same as the averaging portion of my filter, except you use an actual data point where mine is calculated. Again you said "reads values in a short space of time". is their a trick to doing that at the higest rate, while still dumping data to the terminal screen at a slower rate? Would I just need a loop with no wait command, but controll my print sampling with a counter that I can tune? Thanks agian, dave

Dave,

'Sensor' is an alias for 'Sensor.read()'. Every time your code gets the value 'Sensor', it is reading the mbed A/D input (as rapidly as possible). So, when the code sets the LED's to indicate the relative distance or flash, you have just read the sensor 5 times in a row (whether you meant to or not :-) ). Note that if spikes really do occur in just 1 reading out of 5 (or more), your de-spiking scheme only catches 20% of them, but mixes the spike into an average of 5 readings the other 80% of the time. That might account for why you still see much of their effects. In contrast, the median-of-3 filter removes every isolated spike. In your code you would have to use 'senseval' with the LED's (instead of 'Sensor') to avoid extra readings. Your code's sampling rate is set the sum of the 'wait(0.002)' delay and the Sensor.read() library internal wait for the A/D to settle. To get a fixed sample rate you need to attach a routine to a timer which reads the A/D, sets a (volatile) flag and variable, etc. Then the main loop will be free to check the flag and value, printf() messages, run the LED's, etc.

Dave,

I am controlling servos using some fairly noisy potentiometers, reading analog values similar to what you are doing. I've found the best filtering technique for me was "exponential smoothing" http://en.wikipedia.org/wiki/Exponential_smoothing. This effectively averages in part of a new reading with all previous readings and smooths things quite well.

I also found by averaging 3 reads and then adding them to the smoothing function I get an even better result, although you can skip this step if you like. The 4.0 divisor may or may not work for you. The lower this is, the less lag, but results get noisier.

Lag wasn't a major issue for me as I am targeting around 20Hz sample rate for my program.

float InputPin::readAnalog()
{
    // small average filter
    float val = mAnalogPin->read();
    val+= mAnalogPin->read();
    val+= mAnalogPin->read();
    val/=3.0f;
    
    mExpoSmooth += ((val - mExpoSmooth)/4.0f);
    
    return mExpoSmooth; //mExpoSmooth accumulates all previous readings and combines with 25% of the latest reading
}

Hi Fred, Thanks for your reply. So my spike filter actually walks the data down the array so every sensor read goes through it. So first reading fills position 0 (P0) in the array, P0 gets written to P1 (P0->P1) then the next reading fills P0, then P1->P2 P0->P1 and new P0, and so on. I'm not sure of the technical way to say it but the readings stream through the array. This allows the rising sensor data to influence the amount of spike filtration (that's the theory anyway, maybe I implemented it wrong?) With the median of 3, do I take 3 readings then average them, take 3 more and average them, and so on? so essentially my servo will get pwm pulse at 1/3 the sensor data rate? I will have to experiment.

Dave thanks as well. looking into you expo smoothing suggestion. Also it looks like the beginning of the code you posted is a simple median-of-3 as Fred suggests.

I will do some edits with this. at the same time I am expanding the code to have a feedback loop as now I have attached the sensor to the servo. This has been a fun exercise. will let you know how it goes!

1. dave Your Genuine Man, Awesome Work :) I am Impressed.

Gzunda Bed Mover

Thanks Celina, that was kind :)

So the struggle continues. I tried a couple filtering routines. I created an looping iteration to find the "stable band" of sensor data. Both gave mixed results. I decided it would be best to visualize the data so I made a little loop that printed the sensor readings to the terminal as fast as they came in. I plotted the data in excel. Below is a sample that spanned over 100 readings. You can see the crazy data spikes that have been bothering me.

What I noticed is the spikes seem very periodic. In fact each spike is either 9 or 11 sensor readings apart. That seems strange to me that they be in a fixed period. Does anybody have a comment on this? Is it some sort of aliasing? Power for sensor is taken from Vu pin on the board. Could it be dirty input voltage to the sensor? I do have a 10uf cap on the sensor power input.

Here is a plot of some raw sensor data, with the data average of 5 readings, average of 3 readings, and finally average of 3 with the spikes filtered.

The spike filtration shown here is simply replacing the spike values with a max of .23. The software implementation would detect a spike by comparing the newest reading to the previous average of 3...then if the newest value is greater than a threshold (like 1.5 times the previous average) the reading will be replaced with the average. This will remove the spikes and attenuate the average. Problem is this will cause a lag in the rise time of sensor readings depending on the threshold value. I think it could be tuned to have almost no rise time lag. Since I am using it for feedback positioning, this could be an issue, but I suppose the control loop could fix those issues. Thanks for your help! Dave

Martin, I finally tried your Median filter. I did a median of 3 but with my own code:

    int count2 = 0;
    float med[3] = {};
    float sensend = 0;
    while (1)
    {
        count2 += 1;
        med[2] = med[1];
        med[1] = med[0];
        med[0] = Sensor;
        pc.printf("\n%i   %f",count2,med[0]);   
        if (med[0]<med[1])
            {
            if (med[0]>=med[2]) sensend=med[0];
            else if (med[1]<med[2]) sensend=med[1];
            else sensend=med[2];
            }
        else if (med[0]!=med[1])
            {
            if (med[0]<=med[2]) sensend=med[0];
            else if (med[1]>med[2]) sensend=med[1];
            else sensend=med[2];
            }
        else sensend=med[0];
        pc.printf("   %f",sensend);
    }

Here is a sample of the results. It does a nice job but still is susceptible to closely space spikes (look around data point 470). I think I will add my spike threshold filter that will ignore new sensor readings that are +/- .004 from previous median and see what happens.

Hi David,

Have you seen this investigation into mBed ADC noise?

http://mbed.org/users/chris/notebook/Getting-best-ADC-performance/

Best regards,

Mike

No but will go check it out, thanks!

Next update, I tried all sorts of spike filtering but they just cause a rise time lag that will not work for me. So I decided to go with a Median of 5 filter using qsort. It works very well and I am happy with it. He is the code and a plot of the data:

#include "mbed.h"
#include <stdlib.h>
#include <stdio.h>

Serial pc(USBTX, USBRX); // tx, rxSerial pc(USBTX, USBRX); // tx, rx
AnalogIn input(p15);
 
 int compare (const void * a, const void * b)
{
  return ( *(int*)a - *(int*)b );
}
 
int main() {

    float samples[5]={};
    float med[5]={};
    int count = 0;
    float sensval;
    while(1)
    {
        count += 1;
        sensval = input;
        med[4] = med[3];
        med[3] = med[2];
        med[2] = med[1];
        med[1] = med[0];
        med[0] = sensval;
        
        samples[4] = med[4];
        samples[3] = med[3];
        samples[2] = med[2];
        samples[1] = med[1];
        samples[0] = med[0];
        
        qsort (samples, 5, sizeof(int), compare);
    
        // show the averages
        pc.printf("\n%i   %f   %f",count,sensval,samples[2]);
    }
}

See http://mbed.org/users/networker/code/medianfilter/ for an efficient median filter with programmable window size (computes new median on every input sample.

I looked at this but have a question about grounding the unused analog pins. In his images he does not show jumpers going from the pins to ground, so how do you do it?