칼만, 베이즈 필터 - 파티클 필터 개요와 몬테카를로 방법

파이썬 로보틱스에서는 파이썬 언어로 로봇 공학 알고리즘 구현 위주다보니

 

대부분 알고리즘들에 대한 설명이 많이 부족하다

 

어짜피 다른 곳에도 자료는 많으니까

 

내가 주로 본 자료로는

 

probabilistic robotics 나

 

cyrill stachniss의 slam 강의를 여러번 봤었는데

https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_

 

이해하기는 많이 힘들더라

 

그나마 쉬운 자료는

 

한빛 미디어인가? 거기서 나온

 

'칼만 필터는 어렵지않아' 교재가 그나마 이해할만 하더라

 

일단 파이썬 로보틱스의 파티클 필터 예제를 올리기전에 파티클 필터에 대해서 다시 복습해보고자 한다.

 

이번에는 

 

kalman and bayesian filter in python의 파티클 필터를 살펴보면

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/12-Particle-Filters.ipynb

 

개요

 우리가 움직이는 물체를 추적할때 그 물체가 전투기, 미사일이거나 크리켓을 하는 사람들일 수도 있다. 우리가 배운 필터중에 어떤 필터를 사용해야 할까? 일단 이 문제애 대한 특성을 생각해보자

 

- 멀티모달 multimodal : 0, 1, 또는 그이상의 물체를 동시에 추적해야한다

- 합침 occlusions : 한 물체가 다른 물체에 숨겨져서 여러개의 물체가 하나로 관측될 수 있다.

- 비선형 운동 : 비행체는 바람에 영향을 받고, 사람들을 서로서로 부딪칠수도 있습니다.

- 비선형 관측 : 라이다는 물체까지의 거리를 주지만 x, y, z 좌표계로 변환하는 가정에 제곱근이 필요하며 비선형이 됩니다.

- 비가우시안 노이즈 non-Gaussian noise : 컴퓨터 비전에서 배경 주위를 이동하는 물체가 배경의 일부분으로 인식 될수도 있습니다.

- 연속 contiunuous : 물체의 위치와 속도는 시간에 따라 연속적으로 됩니다.

- 다변수 : 위치, 속도, 회전속도 등 다양한 속성들을 추적해야합니다.

- 알수없는 동작 모델 unknown process model : 이 시스템의 동작 모델을 모르는 경우도 있습니다.

 

우리가 그동안 배운 필터들은 이러한 제약들을 가지고 있습니다.

- 이산 베이즈 필터 discrete bayes filter : 멀티모달로 비선형적인 측정값과 운동을 다룰수 있지만 이산적이고 단변수입니다.

- 칼만 필터 : 칼만 필터는 가우시안 노이즈를 가진 유니모달 선형 시스템의 추정에 사용되나 이 문제에 적합하지 않습니다

- 무향 칼만 핕터 : 무향 칼만 필터는 비선형, 연속, 다변수 문제를 다루지만 멀티 모달이 아니고, 합침 문제를 다룰수가 없습니다. UKF는 가우시안이아닌 노이즈도 다룰수는 있지만 매우 비선형적이거나 심한 비가우시안에 적합하지 않습니다.

- 확장 칼만 필터 : 확장 칼만필터는 UKF와 동일한 이점과 제한을 가지고 있으나 비선형과 비가우시안 노이즈에 강하게 대응할수 있습니다.

 

몬테카를로 샘플링 Monte Carlo Sampling

 UKF 챕터에서 아래의 비선시스템의 영향을 가우시안으로 그려보았습니다.

 

 

 좌측 그림은 3000개의 점이 가우시안을 따르며

 오른쪽 그림은 다음의 식에따라 이 점들

 

 결과를 구하기 위해 유한한 임의의 수의 샘플 포인트를 사용하는 방법을 몬테카를로 방법이라 합니다. 이 방법은 생각보다 간단한데, 이 문제를 나타내는 샘플들을 충분히 생성시키고, 이 점들이 시스템에 따라 이동시킵니다. 그리고 점들의 변형을 결과로 계산하면 됩니다.

 

 간단히 말하면 이게 파티클 필터의 동작이 됩니다. 베이지안 필터 알고리즘을 각각이 시스템의 가능한 상태를 나타내는 파티클 수 천개에 적용되고, 파티클의 통계적인 가중치를 이용하여 수천개의 파티클로부터 추정 상태를 구하게 됩니다.

 

일반적인 파티클 필터 알고리즘 Generic Particle Filter Algorithm

1. 임의의 파티클 집합을 생성

 파티클은 위치와 방위 그리고 필요한 추가적인 상태들을 가지고 있습니다. 각각의 파티클들은 가중치(확률)도 가지고 있는 이는 실제 시스템의 상태와 얼마나 일치하는지를 의미합니다. 처음에는 모든 파티클들을 같은 가중치로 초기화 시킵니다.

 

2. 파티클의 다음 상태를 예측

 실제 시스템의 운동처럼 파티클을 이동시킵니다.

 

3. 갱신

 관측 정보를 기반으로 파티클들의 가중치를 갱신시킵니다. 실제 관측치와 가까운 파티클들은 실제 관측지와 가깝지 않은 파티클들에 비해 더 높은 가중치를 가지게 됩니다.

 

4. 리샘플링

 신뢰할수 없는 파티클들을 버리고 가중치가 큰 파티클들로 대체시킵니다.

 

5. 추정치 계산

 상태 추정치를 구하기 위해 파티클 집합의 평균과 공분산을 계산 합니다.

 

 이 알고리즘은 실제 사용하는데 극복해야할 문제들을 가지고 있지만 이게 기본 개념이 됩니다. 예를 보면 이전에 UKF, EKF로 로봇의 위치 추정 문제를 다뤘던것 처럼 파티클 필터를 사용하면, 로봇은 선회와 속도 제어 입력을 가지고 각각의 랜드마크로 부터 거리를 측정하는 센서도 가지고 있습니다.. 이 센서와 입력 또한 노이즈를 가지고 있지만 우리는 이들을 이용해서 로봇의 위치를 추정해야 합니다.

 

 

몬테카를로를 이용한 확률 분포

 우리가 구간이 [0, $\pi$]에서 곡선 y = $e^{sin(x)}$의 넓이를 구해야 한다고 가정해 봅시다. 넓이는 $\int_{pi}^{0}$ $e^{sin(x)} dx$를 계산하면 되므로 살펴봅시다.

 

 만약 당신이 현명하다면 이 문제를 도전하지는 않을겁니다. y = $e^{sin(x)}$는 해석적으로 적분될수 없기 때문인데, 우리가 살고 있는 세상은 지금처럼 적분할수 없는 방정식들이 가득 합니다. 예를들면 한 물체의 밝기를 계산을 하는 경우 그 물체가 일정량의 빛을 반사할것이지만 그 반사되는 빛의 일부는 다른 물체에 부딪혀 원래 물체에 다시 부딪히게 될것입니다. 이 결과 기존의 물체의 밝기는 더 밝아지게 되는데 이는 재귀 적분 recursive integral이 됩니다.

 

 하지만 몬테카를로 기술을 이용하면 이 적분은 다루기 쉬워집니다. 곡선의 넓이를 구하기 위해 곡선의 구간을 감싸는 박스를 만들고, 이 박스 안에 임의의 위치를 가진 점들을 생성시킨 뒤 전체 점들의 갯수와 곡선 안에 존재하는 점들의 비율을 계산하면 됩니다.

 

 예를 들어 박스의 넓이가 1인데 점의 40%가 곡선 안에 존재한다고 하면 그 곡선의 넓이는 약 0.4가 될것입니다. 여기서 점들의 갯수가 많아질수록 더 정확해지나  수천개의 점들만 사용해도 충분히 정확한 결과를 구할 수 있습니다.

 

 수치적으로 적분하기 힘든 함수를 다룰때 이 기술을 사용할수 있으며 적분 불가능하고 비연속적인 함수인 경우에도 사용 가능합니다. 

 

 원의 넓이를 구하여 원주율 $\pi$를 계산하여 봅시다. 우선 반지름이 1인 원을 준비하고, 그 원을 사각형 안에다가 놓읍시다. 이 사각형 변의 길이는 2로 넓이는 4가 됩니다. 이제 이 상자 안에 균일 분포를 따르는 임의의 점들을 생성하고, 원안에 점들이 얼마나 많이 존재하는지 세어봅시다. 원의 넓이는 원안의 점들의 개수/전체 점들의 개수*상자의 넓이로 구할 수 있습니다. 우리는 A = $\pi$ $r^2$임을 알고 있으니, $\pi$ = A/$r^2$인지 계산해봅시다.

 

우선 점들을 만들어 봅시다.

N = 20000
pts = uniform(-1, 1, (N, 2))

 만약 원의 중심으로부터 거리가 반지름보다 같거나 작으면 점들은 원안에 있다고 볼수 있는데, 이 거리를 벡터의 크기를 계산하는  numpy.linalg.norm 함수로 거리를 구하겠습니다. 벡터들은 (0,0)에서 시작하므로 원점을 기준으로 점들의 거리를 계산하겠습니다.

dist = np.linalg.norm(pts, axis=1)

다음으로 이 기준에 맞는 점들을 구하기 위해 다음 조건을 주어 구해봅시다.

in_circle = dist <= 1

이를 이용해서 다음과 같이 pi를 계산할 수 있습니다.

import matplotlib.pyplot as plt
import numpy as np
from numpy.random import uniform 

N = 20000  # number of points
radius = 1.
area = (2*radius)**2

pts = uniform(-1, 1, (N, 2))

# distance from (0,0) 
dist = np.linalg.norm(pts, axis=1)
in_circle = dist <= 1

pts_in_circle = np.count_nonzero(in_circle)
pi = 4 * (pts_in_circle / N)

# plot results
plt.scatter(pts[in_circle,0], pts[in_circle,1], 
            marker=',', edgecolor='k', s=1)
plt.scatter(pts[~in_circle,0], pts[~in_circle,1], 
            marker=',', edgecolor='r', s=1)
plt.axis('equal')

print('mean pi(N={})= {:.4f}'.format(N, pi))
print('err  pi(N={})= {:.4f}'.format(N, np.pi-pi))

이는 몬테카를로 방법을 이용해서 확률 밀도를 계산한 것이 됩니다.