Ardunio 红外测距模块的剔除奇异值滤波

int IRpin = 1;                                    // analog pin for reading the IR sensor 
int i = 0;
float data[100];
float sum=0,avg =0,s2 =0,Sdeviation =0;
void setup() {
  Serial.begin(9600);                             // start the serial port
} 
void loop() {    
  for(i=0;i<100;i++){
   float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
   float distance = 65*pow(volts, -1.10);// worked out from graph 65 = theretical distance / (1/Volts)  
   data[i] = distance;
   sum +=distance; //和
   delay(100);     // arbitary wait time.
  }
  avg = sum/100; //均值
  for(int i =0;i<100;i++){
   s2 += pow(data[i] - avg,2);
  }
  Sdeviation = sqrt(s2/100);
  if (abs(data[0]-avg) >= 3*Sdeviation){
    data[0] = avg;
  }
  sum =0;
  sum += data[0];
  for (i=1;i<100;i++){
    if (abs(data[i]-avg) >= 3*Sdeviation){data[i] = avg;}
  sum += data[i];
  }
  Serial.println(sum/100);
  i = 0;sum = 0;
}