저주파 통과 필터를 적용하게 되면 이전의 데이터 평균 값에 비해

최근의 데이터에 더 높은 가중치를 둘 수 있습니다.

const int ECHO=9;
const int TRIG=10;

#define SIZE 50

int buffer[SIZE];

float duration, Cm;
float sum;
float distance;
float preDistance;
float Cm1, duration1;
float alpha=0.7;

void setup() {
  Serial.begin(9600);
  Serial.flush();
  pinMode(TRIG,OUTPUT);
  pinMode(ECHO,INPUT);
  digitalWrite(TRIG,LOW);

  Cm=duration/(29.1*2);
  sum=0;

  for(int i=0; i<=SIZE-1; i++)
  {
    digitalWrite(TRIG,HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG,LOW);
    duration=pulseIn(ECHO,HIGH);
    buffer[i]=Cm;
    if(Cm>400) Cm=400;
    else if(Cm<2) Cm=2;
    sum+=buffer[i];
  }
  preDistance=sum/SIZE;
}

void loop() {
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  duration1=pulseIn(ECHO,HIGH);
  Cm1=duration1/58.8;

  distance=alpha*preDistance+(1-alpha)*Cm1;

  if(distance>400) distance=400;
  else if(distance<2) distance=2;

  Serial.println((int)distance);
  delay(5);

  preDistance=distance;
}

업로드 한 후 결과를 살펴보면 중간에 생기던 잡음이 제거되며 또한 물체와 센서 사이의 거리가 갑자기 바뀌더라도 정확하게 측정될 것입니다.

'칼만필터의 이해'라는 책에서 제시하고 있는 '이동평균 필터'의 개념과 Matlab 코드를 응용하여 아두이노 방식으로 적용해보았습니다.

이동평균 필터는 일정한 갯수의 데이터를 모아서 평균을 낸 값을 출력하는 방식입니다.

const int ECHO=9;
const int TRIG=10;

#define SIZE 50

int buffer[SIZE];
int Cm1, duration1;

long duration, Cm;

float sum;
float distance;

void setup() {
  Serial.begin(9600);
  Serial.flush();
  pinMode(TRIG,OUTPUT);
  pinMode(ECHO,INPUT);
  digitalWrite(TRIG,LOW);

  Cm=duration/(29.1*2);
  sum=0;

  for(int i=0; i<=SIZE-1; i++)
  {
    digitalWrite(TRIG,HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG,LOW);
    duration=pulseIn(ECHO,HIGH);
    buffer[i]=Cm;
    if(Cm>400) Cm=400;
    else if(Cm<2) Cm=2;
    sum+=buffer[i];
  }
}

void loop() {
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  duration1=pulseIn(ECHO,HIGH);
  Cm1=duration1/58.8;
  sum-=buffer[0];

  for(int i=0; i<SIZE-1; i++)
  {
    buffer[i]=buffer[i+1];
  }

  buffer[SIZE-1]=Cm1;
  sum+=buffer[SIZE-1];
  distance=sum/SIZE;

  if(distance>400) distance=400;
  else if(distance<2) distance=2;

  Serial.println((int)distance);
  delay(10);
}

위 코드의 setup 부분에서 배열을 채워놓고 loop로 들어가는 이유는 배열을 모두 0으로 채워놓고 시작하게 되면 초기에 오차가 크게 나타나는 현상을 피할 수 있기 때문입니다.

이동평균 필터는 배열에 저장되는 모든 데이터를 동일한 가중치를 놓고 평균을 구하기 때문에 최근 값이랑 이전 값이 차이가 나더라도 출력되는 값에서 이를 민첩하게 반영하지 못합니다. 따라서 이동평균필터는 데이터 간의 변화가 클 때에는 사용하기에 적합하지 않습니다.