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

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

아두이노를 이용해서 가장 손쉽게 사용해볼 수 있는 센서는 초음파센서(HC-SR04)라고 생각합니다.

이 센서는 초음파를 보내고 받을 때 까지의 시간을 측정합니다.

따라서 이 센서의 기본적인 반환값은 측정물체에 의해 반사된 초음파가 다시 센서까지 도달하는데 걸리는 시간이라고 할 수 있습니다.

​그러므로 초음파 센서를 사용하여 물체까지 거리를 구하기 위해서는 반환값을 적절히 변형해야 합니다.

데이터 시트에서 제시한 초음파의 속도는 340m/s이므로 1cm이동하는데 대략 29.4microsec가 걸린다는 것을 알 수 있습니다.

%% spec

clear

clc

L1 = 1; L2 = 1; r=0.5; % 링크 1 , 2 의 길이 및 경로 원의 반지름

phi= 0:1:360; % 0~ 360 까지 1 씩 값을 올림

xx = 1 + r*cosd(phi); % (1.1)에 r = 0.5 인 원을 그리기 위한 x 값

X = [xx xx xx xx xx]; %matrix를 만듬 phi가 0~360까지

yy = 1 + r*sind(phi); % (1.1)에 r = 0.5 인 원을 그리기 위한 y 값

Y = [yy yy yy yy yy]; %matrix를 만듬 phi가 0~360까지

%% plot

for i = 1:length(X) % X 행렬까지 반복 1부터 360까지의 X좌표 Y좌표를 하나하나 inverse kinematics 를 하기위함

x = X(i);

y = Y(i);

theta2 = acosd((x^2 + y^2 - L1^2 - L2^2)/(2*L1*L2)); % 세타2 값 정의

theta1 = asind((L2*sind(theta2))/(sqrt(x^2 + y^2))) + atand(y/x); % 세타1 값 정의

x1 = L1*cosd(theta1); % link 1 의 경로 x좌표

y1 = L1*sind(theta1); % 경로 y좌표

figure(1)

plot(X,Y,'k') % 원을 그림 'k'는 검은색을 나타냄

grid on %

hold on % 고정

H=plot([0 x1],[0 y1],'r','Linewidth',5) % 링크 1을 그린다?

hold on

H1=plot([x1 x],[y1 y],'b','Linewidth',5) % 링크 2을 그린다?

axis([-2 2 -2 2])

pause(0.01) % 빨리 돌게 하고싶으면 이거 시간 작게하면됩니당

delete(H) % delete가 plot을 iter마다 지워줌 근데 원은지우면안되니까 링크만지웁니다.

delete(H1)

end