칼만필터 - (2)

'칼만필터의 이해'라는 책에서 제시하고 있는 '이동평균 필터'의 개념과 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으로 채워놓고 시작하게 되면 초기에 오차가 크게 나타나는 현상을 피할 수 있기 때문입니다.

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

이러한 이동평균 필터의 한계를 극복하기 위해서 '칼만필터의 이해' 라는 책에서는

저주파 통과 필터를 제시합니다.