쉽게 설명한 파티클 필터(particle filter) 동작 원리와 예제

파티클 필터(particle filter)는 칼만 필터(kalman filter)와 마찬가지로 노이즈가 있는 환경에서 측정된 데이터를 필터를 사용해 실제 위치를 추정하는 도구다. 파티클 필터(particle filter)는 보통 가우시안 분포가 아닌 측정 데이터를 다루기 위해 사용된다고 한다. 물론 가우시안 분포의 데이터에서 사용하지 말라는 건 아니다. 

본 포스트에서는 파티클 필터(particle filter)의 어려운 수학적인 내용은 제하고, 쉽게 예를 들며 필터의 동작원리에 대해 알아보았다.

파티클 필터(particle filter)에 대해 검색해 보면 아래와 같은 그림을 많이 보게 된다. 아래 그림은 파티클 필터의 estimation cycle을 도식화한 것이다.

<’Real-Time Tracking of Multiple Moving Objects Using Particle Filters and Probabilistic Data Association’ original scientific paper 中>

위 그림의 검은색 원들은 particle을 의미한다. 이 particle에는 보통 위치 데이터와 weight가 포함된다. 검은색 원이 큰 것이 있고 작은 것이 있는 이유는 weight가 크고 작음을 의마한다.

typedef struct _particle_t

{

   int x;

   int y;

   float weight;

}particle_t;

노이즈가 있는 환경에서 레이저 센서나 레이더등을 이용하여 물체의 위치를 측정할 때를 가정해 순서대로 이 필터가 동작하는 과정에 대해 알아 보겠다. (순서는 위 그림과 조금 다르다)

1. 초기 상태

측정 범위안에 랜덤 혹은 일정한 간경으로 particle을 뿌려 놓는다. 이 떄 particle의 weight는 모두 0으로 한다.  

for(int i = 0; i<particle_count; i++)

{

   particle[i].x = random(0, measure_range);

   particle[i].y = random(0, measure_range);

   particle[i].weight = 0;

}

[소스 코드는 이해에 도움을 주기위한 것이다.]

2. prediction / motion model

타겟이 어떻게 움직일지 예측하는 단계로, 각각의 particle들이 기존 위치에서 (process motion variance * 가우시안 랜덤[-1,1])에 해당하는 거리를 움직인다고 추정한다. 그리고, 이때 particle의 weight는 0으로 리셋한다. 

for(int i = 0; i < particle_count ; i++)

{

   particle[i].x = particle[i].x + process_variance * get_gaussian_random();

   particle[i].y = particle[i].y + process_variance * get_gaussian_random();

   particle[i].weight = 0;

}

3. measure & update / observation model

2단계에서 예측된 particle의 위치와 센서로 측정된 위치 데이터를 이용해 particle들의 weight를 업데이트 하는 과정이다. particle의 weight는 측정 위치와 particle의 위치 차이와 정규 분포 함수를 이용해 구할 수 있다. 

 

x는 particle의 x 좌표, mx는 측정 데이터의 x 좌표, sigma는 measure variance를 의미한다.

그리고, 모든 측정 데이터는 모든 particle에 적용해 weight를 업데이트 해야 한다.

for(int m = 0; m < measure_count; m++)

{

   for(int i = 0; i < particle_count; i++)

   {

      float px = get_normalpdf(particle[i].x, measure_data[m].x);

      float py = get_normalpdf(particle[i[.y. measure_data[m].y);

      particle[i].weight += px*py;

   }

}

4. resampling

resampling 과정은 쉽게 설명하면, weight가 작은 particle은 지워 버리고, weight가 큰 particle은 복사해 여러 개로 만드는 과정이다. resampling 전후의 particle개수는 동일해야 한다. 

J.D. Hole의 'Resampling in particle filters'의 논문을 보면, resampling 방법에는 simple random resampling, stratified resampling, systematic resampling, residual resampling 등 여러가지가 있다. 그 중에 systematic resampling은 방법은 아래 코드와 같다.

[systematic resampling 예제 코드]

float u[N]; // N은 particle의 개수

float cw[N]; // cumulative weights

for(int i = 0; i< N; i++)

{

   u[i] = max_particle_weight*random(0,1)/N;

   if (i == 0) cw[i] = particle[i].weight;

   else cw[i] = cw[i-1] + particle[i].weight;

}

int k = 0;

for(i = 0; i < N; i++)

{

   whie(cw[k] < u[i])

   {

      k = (k+1)%N;

   }

   new_particle[i] = particle[k];

}

5. 위치 추정

노이즈가 있는 환경에서도 센서를 통해 측정된 좌표들을, 위 2~4번 과정을 반복하면서 타겟의 위치를 추정할 수있다. 타겟 위치의 추정은 particle중 weight가 가장 큰 위치를 타겟으로 선정하거나, 가중 평균등을 사용하여 위치를 선정할 수 있다. 아래 영상은 파트클 필터(particle filter)를 사용하여 타겟의 위치를 추정하는 예제다. 영상의 하단의 파란색그래프는 해당 위치 particle의 weight 크기를 나타낸 것이며, 회색 바는 파티클 필터의 결과를 바탕으로 타겟의 위치를 tracking한 것이다.

 

[관련 포스트]

- 간단한 칼만 필터(Kalman Filter) 소스 코드와 사용 예제