Localization : Particle filter

앞서 histogram filterKalman filter에 대해 알아 보았다. 특히 Kalman filter의 경우 Gaussian distribution을 보이는 state distribution과 measurements distribution의 이론적 해를 사용하여 계산할 수 있었다. 이번에는 이론적 해를 구하기 까다로운 경우는 접근하는 방법을 소개하고자 한다. 제목과 같이 particle filter를 사용하는 방법이다. 이론적 해를 구하기 어려울 때 사용한다고 하더라도, 개념을 표현하는 수식은 있다. 다른 filter에도 동일하게 적용되는 일반적 식이지만 particle filter의 개념을 소개하기 위해 사용하기로 한다.(실제 필터의 사용은 이런 수식을 사용하여 계산해내야 하는것이 아니므로 사용하기 쉽다.)

기본 개념

일반적으로 어느 system의 state가 시간에 따라 변하고, 이 state에 대한 정보는 각 time step마다 측정하는 noisy measurements에 의해 얻어지는 state-space model을 예측하기 위해 아래와 같은 식을 생각해 볼 수 있다.

 x_k = f_k (x_{k-1}, v_{k-1})

여기서, x_k는 system의 state x에 대한 time step k에서의 state vector이다. v_{k-1}는 state noise vector를 의미하고, f_k는 Kalman filter에서 설명한 state function이다. 이 function은 시간에 따라 변할수 있으므로 일반화시켜 표현한 것이지만, Kalman filter의 경우 시간에 따라 변하지 않는 function으로 가정하였다. 일반적으로, x_k는 직접 측정가능할수 없다고 가정하므로, x_k에 대한 정보는 노이즈가 있는 measurements z_k로부터 다음식을 사용하여 얻어 질 수 있다.

 z_k = h_k (x_k, n_k)

여기서, h_k는 measurement function이다.

앞서 Kalman filter 포스팅에서 밝혔듯이, filtering은 과거 state 및 measurements정보로 부터 state를 예측하는 prediction step과 현재의 측정정보를 사용하여 예측된 state를 보정하는 correction step(또는 update step)으로 나눌 수 있다. 먼저 prediction step에서, p(x_k | z_{1:k})는 time step k-1에서 filtering distribution p(x_{k-1} | z_{1:k-1})로부터 다음과 같은 식을 통해 구해질 수 있다.

p(x_k |z_{1:k-1}) = \int p(x_k |x_{k-1})p(x_{k-1} | z_{1:k-1})dx_{k-1}

여기서, p(x_k | x_{k-1})는 첫 식을 이용하여 구해질수 있으며, 가장 최근의 measurement 값을 반영하지 않은 상태에서 구하는 x_k에 대한 prior라고 할 수 있다. update step에서는, 이 prior를 새로운 measurement, z_k를 사용하여 Bayes’ rule을 사용하여 update 하여 k step state에 대한 posterior를 구한다.

 p(x_k | z_{1:k})  \propto p(z_k | x_k )p(x_k | z_{1:k-1})

위 두식을 통해 개념적 이론은 정리가 되었지만 각 distribution을 이론적 식으로 표현할 수 있는지가 미지수이다. Gaussian distribution을 사용하고, 이론적 해석이 가능한 경우 Kalman filter를 사용한다. 그러나, 이론적으로 구하기 어려운 경우는 Mote Carlo method를 사용한다. Monte Carlo sampling 방법은 이론적으로 system해석이 어려운 경우, measurement의 오차를 이용하여 state function을 예측하되, 수많은 particle을 동원한 통계적 기법을 사용하는 것이다.

Particle Filter

여기서는 SIS(Sequential Importance Sampling) 방법을 사용하는 particle filter를 소개한다. 이 알고리즘에서는 time step k-1에서 앞서 말한 filtering distribution, p(x_{k-1} | z_{1:k-1})대신해서 full posterior distribution, p(x_{0:k-1} |z_{1:k-1})을 생각한다. SIS의 기본 아이디어는  particle이라 불리우는 가중치를 가진 sample,  \{x_{0:k-1}^i ; w_{k-1}^i\}_{i=1:N}을 사용하여 time step k-1에서posterior distribution, p(x_{0:k-1} | z_{1:k-1})의 근사치를 구하고, 이 particle들을 지속적으로 update(re-sampling)하여 다음 time step에서의 posterior distribution, p(x_{0:k} | z_{1:k})에 대한 근사치를 추정하게 된다.

SIS의 importance sampling에서는 target distribution, p(x)의 근사치는 proposal distribution, q(x)의 sampling을 통해 구하려고 한다. 그러므로, 변화하는 target distribution을 추적하려면 proposal distribution을 measurements를 이용하여 계속 update 해주어야 한다는 필요성이 나타나게 된다. 이를 위해서 해야 하는 것은 무엇일까? proposal distribution과 target distribution의 차이/오차를 반영하여 proposal distribution에서 추출한 sampling에 개별로 그 중요도(weights)를 부가하는 것이다. 그러면 중요도가 높은 것은 계속해서 sampling이 되고, 그렇지 않은 것들은 점차 proposal distribution에서 사라지게 된다. 여기서 도입한 각 sample의 weight는 w_i  \propto \frac{\pi(x^i )}{q(x^i )}의 관계를 가지고 있고, 여기서 \pi(x)는 target distribution과 비례적 상관관계가 있는 임의의 함수이다. SIS의 핵심 개념은 다음 step의 posterior distribution,  p(x_{0:k} |z_{1:k})의 근사치를 구할 수 있도록 particle x_{0:k-1}^i과 weights w_{k-1} ^i를 계속해서 update하는 방법을 구현하는 일이다. 먼저 particle update를 위해 time step k에서 proposal distribution은 아래와 같은 상관 관계가 있다고 가정한다.

q(x_{0:k} | z_{1:k}) = q(x_k | x_{0:k-1} , z_{1:k}) q(x_{0:k-1} | z_{1:k-1}) 

한편, weight로서 의미를 가지기 위해서는 기본적으로 다음과 같은 특성을 보여야 한다.

w_k ^i \propto \frac{p(x_{0:k}^i | z_{1:k})}{q(x_{0:k}^i | z_{1:k})}

위식을 통해 수학적으로 아래와 같은 recursive expression을 얻을 수 있다.

w_k ^i \propto w_{k-1}^i \frac{p(z_k | x_k ^i ) p(x_k ^i |x_{k-1} ^i)} {q(x_k ^i) | x_{0:k-1}^i , z_{1:k})}


필자도 깔끔하게 정리된 수식을 선호하지만, 수식을 이해하고 그 물리적 의미를 파악하는 일이 어려울 때가 있다. 그 이유는 개념이 어려운 경우도 있지만, 수학이라는것이 대부분 generalization(일반화)된 형태로 표현되기 때문이기도 하다. 위의 식들도 일반화되어 있는 표현이므로 즉각에서 그 의미를 파악하기 쉽지 않다. 위의 proposal distribution식의 의미는 update된 proposal distribution은 이전의 distribution에 새로운 measurement를 포함한 관측치 및 target distribution을 기반으로 예측되는 time step k의 proposal distribution의 확률분포를 곱한값이 예측되는 target distribution이라는 의미이다. weight는 새로운 measurement를 동원하여 time k에서의 target distribution을 예측할 수단으로 weight를 도입하고 새로운 weight는 기존 weight와 기존 proposal distribution을 근거로 나타난 target distribution의 비와 비례한다. 더 풀어서 기술한다면, 관측데이터를 이용해 error와 반비례관계를 가지도록 weight를 수정하고, 그 weight를 이용하여 새로운 proposal distribution을 가지는 particle을 생성시키는 것이다.  여기서 distribution은 통계적 표현이고, 통계적 기법을 의미하는 고상한(?) 용어는 Monte Carlo Method이다. 즉 컴퓨터는 수많은 random state를 발생시킬 수 있으므로 이 것을 사용하면, distribution 형태의 function을 구현할 수 있다. 이방법이 particle filter의 기본 개념이고 실제 사용은 앞서 적은 이론식에 비해 매우 간단한 개념이다. 필자가 이 블로그를 통해 수학을 강조하는 이유는 결국 대부분의 논문이나 기술은 수학적 표현을 사용하여 개념을 전달하고자 하며, 사실 가장 효과적인 수단이기 때문이다. 


Particle filter를 이용한 위치 예측 예

Particle filter를 사용예를 보여주기 위해 움직이는 로봇이 주변의 4곳의 landmarks와의 거리를 측정하는 측정값을 사용하여 위치 및 방향을 예측하는 예를 들어 본다. 즉, 거리 측정값은 로봇의 위치를 정확히 나타내는 값이다. 일단 particles의 움직임을 로봇과 동일하게 움직여서 motion update (prediction)단계를 거친다. 이후 실제 측정치와 각 particle의 거리 측정치와의 오차를 이용하여 그 오차를 확률로서 표현한다. 즉, 실제치와 비슷한 거리를 보이는 particle에 대해서는 높은 확률을, 큰 차이를 보이는 particle에 대해서는 낮은 확률값을 내도록 한다.(실제치의 Gaussian distribution에서 particle의 거리를 입력하여 Gaussian 확률값을 얻어 내는 방법이다.) 이 확률값은 일종의 importance weight이며 높은 확률값을 가진 particle은 실제 로봇의 위치와 같을 확률이 높은 것으로 해석할 수 있다. 이러한 각 particle의 가중치를 이용하여, resampling과정을 통해 가중치가 높은 particle이 더 많이 선택이 되도록 다시 particle을 sampling하는 과정을 거친다. 이와 같은 과정이 measurement update과정으로 해석될 수 있으며, 실제 measurement를 이용하여 예측치의 정확도를 높이는 과정으로 설명할 수 있다. 실제 예측치는 particle의 위치 평균값을 사용하여 예측한다.

아래는 반시계방향으로 0.1\pi의 방향으로 10 m를 이동하는 과정을 10번 수행하는 로봇/자동차의 주행상황에서 이론적으로 계산된 위치 및 방향(ground-truth state)과 대비해서Gaussian noise를 가지는 이동 및 측정상황에서 측정값이 아래 그림과 같이 나타날때 particle filter에 의해 예측된 값을 계산한 결과를 예들들어 제시하였다.

particle-filter-result

1000개의 particle을 사용한 예측값으로 noise로 인해 매번 결과 값이 약간씩 다르지만 비교적 정확하게 그 위치를 예측하는 것을 알 수 있다.

Advertisements

답글 남기기

아래 항목을 채우거나 오른쪽 아이콘 중 하나를 클릭하여 로그 인 하세요:

WordPress.com 로고

WordPress.com의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Google+ photo

Google+의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Twitter 사진

Twitter의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Facebook 사진

Facebook의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

%s에 연결하는 중