공부

Particle Filter 공부했다.

Swimming_Kim 2018. 12. 31. 21:03

 계속해서 배우고 있는 필터 시리즈(?)의 대망의 마지막 Particle Filter이다. 위는 지금까지 배운 각 필터들에 대해서 정리를 함과 동시에 지금부터 배울 Particle Filter에 대한 간략한 설명을 하고 있다. 각각의 필터가 이산적인지(Discrete - 모눈종이처럼 나뉘어져 있는지), 연속적인지(Continuous - 일반적인 좌표 처럼인지) 체크가 되어 있고, 가장 확률이 높은 지점 하나만 알려주는지, 아니면 여러 후보군들을 알려주는지도 체크가 되어 있다. 


 제일 오른쪽에서 보이듯이, 이 필터들은 모두 Motion과 Measurement의 불확실성을 보완해주기 위해서 생긴 것이다. 애초에 완벽하게 움직이고 완벽한 센서가 있었으면 사실 그게 제일 좋은 것이다. 이렇게 불확실성에 기반하기 때문에 이 필터들의 결과값도 그저 추정이라는 의미로 설명을 하고 있다.


 위 그림에 ?가 어떤 의미인지 의아해 할 수 있는데, Particle Filter는 그 연산의 복잡도를 내가 설정해 줄 수가 있다. Histogram Filter는 고려해야 할 변수가 많을 경우에 연산량이 지수적(exponential)으로 증가하고, 무적의 Kalman Filter도 제곱으로 복잡도가 증가한다. 그런데 이제 배울 Particle Filter는 프로그래밍 하기도 무지무지 쉽고, 복잡도도 우리가 설정해 줄 수가 있다고 한다. 





우선 작동하는 과정을 사진으로 살펴보자. Particle이라는 이름에서 알 수 있듯이 작은 입자들을 흝뿌린다. 그리고 로봇이 이동할수록, 입자들이 모인다! 그러다가 보면~ 한 지점으로 모이게 된다. 위에서 마지막 그림에 실제로 로봇이 위치한 지점과 대각선 방향으로 또 커다란 뭉치가 있는 것이 보이는데, 이는 이 복도가 중심점을 기준으로 대칭으로 있어서 그런다. 내가 써 놓고도 이게 무슨 말인지 모르겠는데, 졸려서 그런 것도 있지만, 이제부터 Particle Filter의 원리를 배우고 다시 보면 이해가 갈 것이다. 


 왜인지는 모르겠지만, 이번 강좌에서는 코드부터 던져주고 시작한다. 프로그래밍 하기 쉽다고 해서 빨리 보여주고 싶었나...


1
2
3
4
5
6
7
8
myrobot = robot()
# enter code here
myrobot.set_noise(5., 0.1, 5.)
myrobot.set(30.0, 50.0, pi/2)
myrobot = myrobot.move(-pi/2, 15.0)
print myrobot.sense()
myrobot = myrobot.move(-pi/2, 10.0)
print myrobot.sense()
cs


 앞으로는 거의 모든 과제들에서 클래스를 제공해 준다. 나도 잘 모르지만, 간략하게 설명하면, 우리가 실습을 하기 위해서는 measurement도 하고 특정한 각도와 방향으로 motion도 취하고 이러한 움직임들에 대해서 노이즈도 생각해주어야 하는데, 이러한 기능들이 모두 담겨 있는 코드 상의 '로봇'이라는 함수 덩어리를 제공하겠다는 것이다. 

 

 그래서 위 Main문을 보면, myrobot = robot()이렇게 해주는 것만으로 move/sense/set_noise등의 기능들을 수행할 수 있는 myrobot이라는 이름의 로봇이 하나 만들어졌다. 각 문장들을 차례로 설명하자면, set_noise안의 숫자들은 차례로, 움직임, 회전, Sensing에 대한 노이즈를 설정해 주는 것이고, move를 통해 먼저 특정 각도로 돌고, 지정된 숫자만큼 앞으로 움직이게 된다.




 이 좌표계에서 모서리 부분마다 landmark가 있어서 로봇과 이 landmark들 사이의 거리를 알 수가 있는데 sense는 이 값을 반환하는 부분이다. 그래서 print(sense)이렇게 하면 4개의 landmark들과 로봇 사이의 거리를 차례로 보여준다.


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
# Make a robot called myrobot that starts at
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2, move
# 15 m, and sense. Then have it turn clockwise
# by pi/2 again, move 10 m, and sense again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.
 
from math import *
import random
 
 
 
landmarks  = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
 
 
class robot:
    def __init__(self):
        self.x = random.random() * world_size
        self.y = random.random() * world_size
        self.orientation = random.random() * 2.0 * pi
        self.forward_noise = 0.0;
        self.turn_noise    = 0.0;
        self.sense_noise   = 0.0;
    
    def set(self, new_x, new_y, new_orientation):
        if new_x < 0 or new_x >= world_size:
            raise ValueError, 'X coordinate out of bound'
        if new_y < 0 or new_y >= world_size:
            raise ValueError, 'Y coordinate out of bound'
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError, 'Orientation must be in [0..2pi]'
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
    
    
    def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.forward_noise = float(new_f_noise);
        self.turn_noise    = float(new_t_noise);
        self.sense_noise   = float(new_s_noise);
    
    
    def sense(self):
        Z = []
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            dist += random.gauss(0.0, self.sense_noise)
            Z.append(dist)
        return Z
    
    
    def move(self, turn, forward):
        if forward < 0:
            raise ValueError, 'Robot cant move backwards'         
        
        # turn, and add randomness to the turning command
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # move, and add randomness to the motion command
        dist = float(forward) + random.gauss(0.0, self.forward_noise)
        x = self.x + (cos(orientation) * dist)
        y = self.y + (sin(orientation) * dist)
        x %= world_size    # cyclic truncate
        y %= world_size
        
        # set particle
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res
    
    def Gaussian(self, mu, sigma, x):
        
        # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
    
    
    def measurement_prob(self, measurement):
        
        # calculates how likely a measurement should be
        
        prob = 1.0;
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
        return prob
    
    
    
    def __repr__(self):
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
 
 
 
def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))
 
 
 
####   DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
 
myrobot = robot()
myrobot.set(30, 50, pi/2)
#print(myrobot.sense())
myrobot = myrobot.move(-pi/2, 15)
print(myrobot.sense())
myrobot = myrobot.move(-pi/2, 10)
print(myrobot.sense())
cs



 다음으로 해볼 일은 Particle들을 만드는 것이다. 총 1000개를 만들 것인데, 물론 우리가 모두 위치를 지정해 줄 수는 없고, 랜덤하게 x, y, orientation들을 지정해 줄 것이다. 그리고 이 Particle들은 단순한 점이 아니라 로봇이 있을 수 있는 위치라는 것을 잊지 말아야 한다.  그래서 로봇이 특정한 Motion을 취하면 위치가 바뀌듯이 이 Particle들도 위치를 바꾸어야 한다. 이러한 1000개의 Particle들이 담긴 리스트 p를 만들어 보자.


1
2
3
4
N = 1000
p = []
for i in range(N):
    p.append(random.random())
cs


 짠! 아주아주 쉬웠다. 


 

 이 Particle들은 그냥 입자가 아니다!! 로봇의 위치라고 생각해야 한다!!! 그래서 특정한 모션도 취할 수 있어야 한다. 회전도 할 수 있고 앞으로 갈 수도 있어야 한다.


1
2
3
4
5
6
7
N = 1000
p = []
for i in range(N):
    x = robot()
    x = x.move(0.1, 5)
    p.append(x)
 
cs


 요로케 말이다.



 이제 위에 Landmark들이 왜 있었는지에 대해서 생각을 해 보아야 한다. Landmark들과의 거리를 고려하면, 좌표계 위의 정확히 어디 있는지 모르는 점의 위치를 구할 수 있게 된다. 반대로, 각 Particle들이 실제 로봇의 위치와 얼마나 가까운지 알 수 가 있는 것이다.  Particle들과 Landmark들 사이의 Measurement를 통해 만들어진 Vector/ 실제 로봇의 Measurement Vector의 차이가 작으면 작을수록 그 Particle이 실제 로봇의 위치와 가깝다는 뜻이 된다.


 글로 표현하기 너무 어렵다 ㅠㅠ 위 그림에서 초록 화살표들과 검정 화살표가 비슷할수록 좋은 Particle이란 뜻이 된다!!!! 그래서 두 화살표의 차이에 따라 weight를 다르게 줄 것이다. weight가 갑자기 무슨 의미이냐면



 실제 로봇 위치와 가까울수록 weight는 클 것이다. 그래서 파란 동그라미에 로봇이 있을 때 particle들의 weight를 그려 보면 위와 같을 것이다. 앞으로의 단계들에서, 미미한 weight를 가진 particle들은 살려둘 필요가 없다. 그래서 솎아내기 작업을 해야 하는데, 이를 Resampling이라고 한다.


 


이 Resampling하는 과정을 앞으로 코딩할 건데 그 단계적인 절차를 살펴보자. 각 Particle들을 정했고, 그에 해당하는 weight들도 구해서 가지고 있다. 하지만 이 값을 뒤죽박죽이어서 일단 Normalizing을 거쳐주어야 한다. 그런 다음 원래 있던 N개의 Particle 집합에서 다시 N개를 뽑아 새로운 Particle 집합을 만들어주어야 한다. 무엇에 따라??

weight에 따라!! 천천히 접근해 보자.



 N=5라고 하고, weight들이 다음과 같이 나와서 Normalize해준 뒤 a라고 해 주었다. 이런 상황에서 1번 Particle이 한 번도 뽑히지 않을 수 있을까? Yes!! 1번은 weight가 제일 작다. 그래서 다시 뽑힐 확률은 상당히 작다고 할 수 있다.



 그러면 3번이 뽑히지 않을 수도 있을까?? 이것도 Yes... 가장 weight가 높은 3번도 뽑히지 않을 수가 있다.(이것이 particle filter의 단점이라면 단점이다.)



 5번 뽑을 때, 3번이 한 번도 뽑히지 않을 확률은 0.6 * 0.6 * 0.6 * 0.6 * 0.6으로 0.777정도가 된다. 실제 로봇의 위치일지도 모르는 중용한 Particle인데 약 3%의 확률로 뽑히지 않을 수도 있다는 것이다. 



 그럼 확률이 제일 작은 1번이 뽑히지 않을 확률은?? 0.9을 5번 곱한 0.59정도이다. 중요하지 않은 Particle이었는데 뽑힐 확률이 40%정도 있다는 것이다. 그럼 이제 Resampling을 직접 코드로 구현해 보자!!



 그런데 어떻게 weigth에 따라서 다시 Particle들을 뽑을 것인가?? 이를 위해서 약간의 프로그래밍적인 trick이 필요하다. 룰렛 게임을 해 본적이 있는지 모르겠다. 원판에 구역을 나누고 이들 돌려서 멈추는 부분에 해당하는 무엇을 하는 게임인데, Resampling과정을 이를 이용해 만들 수가 있다!!


 위 사진을 보면 알 수 있듯이, weight에 따라서 각 구역이 차지하는 넓이가 달라진다. 그리고 룰렛을 마구 돌리는 것이 아니라, 가장 큰 weight의 2배 만큼만 돌려 준다. 그렇게 해서 가리키고 있는 위치를 선태하는 것이다!!



 이렇게 룰렛을 돌리게 되면, weight가 작은 Particle은 뽑힐 확률이 적어 지고, weight가 큰 Particle들은 뽑히기 쉬울 것이다!! 그리고 기존의 룰렛과 다른 점이 있다면 이는 판이 돌아가는 것이 아니고 화살표(index)가 직접 돌아가는 방식이다. 이동한 위치가 위와 같을 때에는, index는 1번을 가리키게 될 것이다. 2번으로 가기에는 움직인 거리가 모자라기 때문이다. 이를 프로그래밍으로 표현해 보자면 오른쪽과 같다. 


1
2
3
4
5
6
7
8
9
10
p3 = []
max_w = max(w)
beta = 0.0
index = int(random.random() * N)
for i in range(N):
    beta += random.random() * 2.0 * max_w
    while w[index] < beta:
        beta = beta - w[index]
        index = (index + 1) % N
    p3.append(p[index])
cs


 화살표를 최대 weight의 2배 만큼 돌려주고, 이 움직인 거리를 beta라고 해준다. 그리고 화살표가 가리키는 곳 바로 전의 index를 뽑아주게 된다. 


beta = beta - w[index]


그렇게 index가 바뀌었으므로 이전 index가 가졌던 부분 만큼 움직인 거리 Beta에서 빼 주고,


        index = (index + 1) % N

 

 index가 한 바퀴를 돌았다면 다시 리셋을 해 준다. 사실 별로 코드는 길지 않다만, 생각도 해야 하고 해서 상당히 시간을 썼었다. 


 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
# Please only modify the indicated area below!
 
from math import *
import random
 
landmarks  = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
 
class robot:
    def __init__(self):
        self.x = random.random() * world_size
        self.y = random.random() * world_size
        self.orientation = random.random() * 2.0 * pi
        self.forward_noise = 0.0;
        self.turn_noise    = 0.0;
        self.sense_noise   = 0.0;
    
    def set(self, new_x, new_y, new_orientation):
        if new_x < 0 or new_x >= world_size:
            raise ValueError, 'X coordinate out of bound'
        if new_y < 0 or new_y >= world_size:
            raise ValueError, 'Y coordinate out of bound'
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError, 'Orientation must be in [0..2pi]'
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
    
    
    def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.forward_noise = float(new_f_noise);
        self.turn_noise    = float(new_t_noise);
        self.sense_noise   = float(new_s_noise);
    
    
    def sense(self):
        Z = []
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            dist += random.gauss(0.0, self.sense_noise)
            Z.append(dist)
        return Z
    
    
    def move(self, turn, forward):
        if forward < 0:
            raise ValueError, 'Robot cant move backwards'         
        
        # turn, and add randomness to the turning command
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # move, and add randomness to the motion command
        dist = float(forward) + random.gauss(0.0, self.forward_noise)
        x = self.x + (cos(orientation) * dist)
        y = self.y + (sin(orientation) * dist)
        x %= world_size    # cyclic truncate
        y %= world_size
        
        # set particle
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res
    
    def Gaussian(self, mu, sigma, x):
        
        # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
    
    
    def measurement_prob(self, measurement):
        
        # calculates how likely a measurement should be
        
        prob = 1.0;
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
        return prob
      
    def __repr__(self):
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
 
 
 
def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))
 
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
 
####   DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
T = 10 #Leave this as 10 for grading purposes.
 
p = []
for i in range(N):
    r = robot()
    r.set_noise(0.05, 0.05, 5.0)
    p.append(r)
 
for t in range(T):
    myrobot = myrobot.move(0.1, 5.0)
    Z = myrobot.sense()
 
    p2 = []
    for i in range(N):
        p2.append(p[i].move(0.1, 5.0))
    p = p2
 
    w = []
    for i in range(N):
        w.append(p[i].measurement_prob(Z))
 
    p3 = []
    index = int(random.random() * N)
    beta = 0.0
    mw = max(w)
    for i in range(N):
        beta += random.random() * 2.0 * mw
        while beta > w[index]:
            beta -= w[index]
            index = (index + 1) % N
        p3.append(p[index])
    p = p3
    print(eval(myrobot, p))
#enter code here, make sure that you output 10 print statements
cs


 이렇게 전체 코드를 작성하였다. T만큼 move(0.1, 0.5)라는 움직임을 취해 주면서 Particle Filter가 잘 작동하는지 보기 위해 만든 버전이다. 새롭게 보이는 함수가 있는데, eval이라는 함수이다. 이는 오차를 계산하는 함수인데,


1
2
3
4
5
6
7
8
def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))
cs


 world_size의 배수로 나눠주고 빼주고 하는 과정은 이 world가 cyclic(0-100 까지 있다면 100다음 다시 1로 돌아가는 세계)이기 때문에 해준 과정이다. 어쨌든, 이렇게 에러까지 구해 보았는데, 강좌에서는 여러 번 돌려 보기를 권한다. 왜냐하면, Particle Filter는 완벽하지 않다. 적은 확률이지만, weight가 높은 Particle들이 Resampling과정에서 뽑히지 않을 수도 있다. 이렇게 된다면 에러의 값이 무척이나 클 것이다. 



 이렇게 개론적인 부분은 끝이 났다. 사실 이론보다는 코드 실습이 많았는데, 이 속에 수학적인 이론이 모두 숨어 있던 것이라고 한다. 

 우선 measurement update는 Bayes Rule과 관련이 깊다. 그리고 이는 우리가 weight에 따라서 Particle들을 Resampling해 주었던 과정과 완전히 일치한다. 

 그리고 Motion update과정은 우리가 전체 Particle들에서 새로이 N개를 뽑은 그 과정이 이에 해당된다고 한다.


결국 하고 싶은 말은 프로그래밍 과정 속에서 수학적인 이해도 같이 겸한 것이라고 말하고 싶은 것 같다. 다음으로는 실습을 더 해본다.