median filtering

Projects General Guidance
1

Hi guys,
I have this code,
I know it isnt very streamlined, but it what I have for now,
The plan is to filter out the noise on an accelerometer, I cant find a kalman filter that lets me easily use my acc module , its an analog module not iic. So i decided to have a go at making a basic filter,
The problem is that the servo goes all stuttery, and im not sure why.
The plan of the filter is to take ten readings and get their sum, then divide by 10 to get an average reading.
the I compare each seperate reading to the median, to determine ten "differences"
i then deal with any neg difference by multiplying the result by -1 if the difference is less than 0.
I then check if the difference is less than a threshold, if it is then the original reading is added to the sum of the "filtered median" and the count of accepted readings is increased,
from there i repeat everything and at the end i have a sum and a number to divide it by, in order to get a median that doesnt include ay outrageous readings,

this is then mapped and written to a servo.
it seems like it should work , but its sooooo jerky it isnt feasable to use,

does anyone have any advice on why it may be so jerky, or what i can do to maybe make it mroe streamlined and run quicker?
thanks, in advance,

#include <AcceleroMMA7361.h>
AcceleroMMA7361 accelero;
#include <Servo.h>
Servo servo;
int ser= 0;


//////////////variables for differences
int d1=0;
int d2=0;
int d3=0;
int d4=0;
int d5=0;
int d6=0;
int d7=0;
int d8=0;
int d9=0;
int d10=0;
///////////////////////////////readings on y axis
unsigned int y1;
unsigned int y2;
unsigned int y3;
unsigned int y4;
unsigned int y5;
unsigned int y6;
unsigned int y7;
unsigned int y8;
unsigned int y9;
unsigned int y10;
//////////////variables to calc medians
unsigned int median;
unsigned int filteredmedian =0;
int filtercount =0;
int result =0;
/////////////threshold for median and difference
int thresh =2;

void setup()
{
  Serial.begin(9600);
  servo.attach(9);
  //////////////put servo to neutral
  servo.writeMicroseconds(1500);
  delay(500);
  accelero.begin(13, 12, 11, 10, A0, A1, A2);
  accelero.setARefVoltage(5);                   //sets the AREF voltage to 3.3V
  accelero.setSensitivity(LOW); 
    //sets the sensitivity to +/-6G
  accelero.calibrate();
 } 
  void loop()
{
  ///////////////reset filter variables
  filteredmedian=0;
  filtercount=0;
  
  ///////////make ten seperate readings
  y1 = accelero.getYVolt();
  delay(5);
  y2 = accelero.getYVolt();
  delay(5);
  y3 = accelero.getYVolt();
  delay(5);
  y4 = accelero.getYVolt();
  delay(5);
  y5 = accelero.getYVolt();
  delay(5);
  y6 = accelero.getYVolt();
  delay(5);
  y7 = accelero.getYVolt();
  delay(5);
  y8 = accelero.getYVolt();
  delay(5);
  y9 = accelero.getYVolt();
  delay(5);
  y10 = accelero.getYVolt();
  delay(5);
  /////////////take sum of readings, divide for median
  median=((y1+y2+y3+y4+y5+y6+y7+y8+y9+y10)/10);
  
  ////////////calc difference
  d1 =(median - y1);
  ////////////take care of negative differences
  if (d1<0){
    d1=-d1;
   } 
   ///////////if below threshold, add reading to filtered sum
  if((d1)<thresh){
    filteredmedian = (filteredmedian+y1);
    filtercount ++;
  }
  //////////repeat process 9 times more
  d2 =(median - y2);
  if (d2<0){
    d2=-d2;
   } 
  if((d2)<thresh){
    filteredmedian = (filteredmedian+y2);
    filtercount ++;
  }
  
  d3 =(median - y3);
  if (d3<0){
    d3=-d3;
   } 
  if((d3)<thresh){
    filteredmedian = (filteredmedian+y3);
    filtercount ++;
  }
  d4 =(median - y4);
  if (d4<0){
    d4=-d4;
   } 
  if((d4)<thresh){
    filteredmedian = (filteredmedian+y4);
    filtercount ++;
  }
  d5 =(median - y5);
  if (d5<0){
    d5=-d5;
   } 
  if((d5)<thresh){
    filteredmedian = (filteredmedian+y5);
    filtercount ++;
  }
  d6 =(median - y6);
  if (d6<0){
    d6=-d6;
   } 
  if((d6)<thresh){
    filteredmedian = (filteredmedian+y6);
    filtercount ++;
  }
  d7 =(median - y7);
  if (d7<0){
    d7=-d7;
   } 
  if((d7)<thresh){
    filteredmedian = (filteredmedian+y7);
    filtercount ++;
  }
  d8 =(median - y8);
  if (d8<0){
    d8=-d8;
   } 
  if((d8)<thresh){
    filteredmedian = (filteredmedian+y8);
    filtercount ++;
  }
  d9 =(median - y9);
  if (d9<0){
    d9=-d9;
   } 
  if((d9)<thresh){
    filteredmedian = (filteredmedian+y9);
    filtercount ++;
  }
  d10 =(median - y10);
  if (d10<0){
    d10=-d10;
   } 
  if((d10)<thresh){
    filteredmedian = (filteredmedian+y10);
    filtercount ++;
  }
  ///////////get final filtered reading
  result = filteredmedian/filtercount;
  ////////////map result limits to servo range
  ser = map(result,1440,1850,2000,1000);
  //////////////write to servo
  servo.writeMicroseconds(ser);
  ///////////////serial prints for debugging
  //Serial.println(y1);
  //Serial.println(y2);
  //Serial.println(y3);
  //Serial.println(y4);
  //Serial.println(y5);
  //Serial.println(y6);
  //Serial.println(y7);
  //Serial.println(y8);
  //Serial.println(y9);
  //Serial.println(y10);
  //Serial.println(median);
  //Serial.println(filteredmedian);
  Serial.println(filtercount);
  Serial.println(result);
 }
2

Why do you take the absolute value of the error? If one value reads above and one value reads below, then your average of the 2 should be relatively accurate. It sounds somewhat like you want to apply a PI loop to your problem. I would look into it. I believe your method has mathematical some ramifications that may end up in some instability depending on the input.

Additionally, you may want to try adding a few hundred millisecond delay to the end of your loop. Electromechanical things do not like being updated rapidly. Neither really does a roughly tuned control loop.

3

Topic subject line is misleading, your approach has nothing to do with median filtering:

4

You,re right, sorry about that

5 
//////////repeat process 9 times more

Or, for those of us who don't like typing, use arrays and for loops.

6

Ive tried To Use Arrays, And my Jind Boggles.
Although A For Loop Is Something I Can Do, Aswell As Typing.

Does The Compiler optimise The Code ? Or Would Using Smrter Code Actually Make It Smaller?

7

Ive tried To Use Arrays, And my Jind Boggles.

My sincere condolences to your Jind.

Although A For Loop Is Something I Can Do, Aswell As Typing.

The jury is still out on that one, I fear.

The compiler won't optimise-in a for loop when you don't write one.

8

Hahaha i always love the sarcastic comments that float around here ,

I,ll see if I Can Fix My Jind And Learn To Type