공부

Kalman Filter 공부했다.

Swimming_Kim 2018. 12. 23. 01:01

 Monte Carlo Localization에 이어서 이번에는 System을 Estimate하는 또다른 방법에 대해서 배워본다. 바로 Kalman Filter이다. 제어나 SLAM에서 EKF라는 말을 자주 듣곤 하는데, 이는 Extended Kalman Filter의 약자로, 로봇 분야에서는 거의 대부분을 맡고 있는 중요한 개념이다. 



  Tracking에 주로 쓰이는 방법들은 위와 같다고 하는데, 이 세가지 방법들의 차이를 알기 쉽게 그려 놓은 것이 있어서 첨부를 해 본다. 지난번에 배운 Monte Carlo Localization은 discrete한 환경을 가정하고, Multi-Modal이라는 것은 마치 낙타의 혹이 여러개 있는 것처럼 로봇이 존재할 수 있는 후보군들이 여러 곳으로 나올 수 있다는 것이다.


 그에 비해서 Kalman Filter는 Continuous한 환경에서 사용할 수가 있고, (우리가 살고 있는 세상의 좌표개념은 Continuous이다. 그래서 자주 쓰인다.) 뒤에서 배우겠지만 Gaussian 함수를 기본으로 해서 Uni-Modal의 확률을 보여준다. 이것이 중요한데, 하나의 제일 정확한 지점을 알 수 있다는 것이다.


 칼만 필터가 어떠한 일을 하는지 알기 쉽게 살펴보자.



 time step이 1초라고 하였을 때, 다음 위치로 어디가 적합할 것 같은가??



 외부에서 커다란 힘이 작용하지 않는 이상 위와 같은 위치로 움직일 것이다. 이것이 칼만 필터가 내놓는 결과이다. 여기에서 살짝 더 말하자면, 1초 간격으로 얼마만큼 움직였다는 것을 알기에 공짜로 속도까지 알게 될 수가 있다. 오~~ 놀랍다!!! 정보 하나로 두개의 정보를 알 수 있다니!!!


 칼만 필터(한국말로 하겠다 이젠)에서 가장 중요한 것이 바로 가우시안을 사용한다는 것이다. 그래서 가우시안이 어떤 것이고, 무슨 특이한 점이 있어서 이를 많이 사용하는지 알아본다. 



 일단 이렇게 생겼다. 종 모양처럼 생겼다. 아마 고등학교 수학 교육을 받았다면 확률과 통계시간에 유일하게 배우는 확률 함수가 있는데 그것이 바로 가우시안이다. 자연계에서 가장 많이 보이는 확률 분포이다. 가우시안은 확실하다!! - 하나의 최고점을 가지고 이를 중심으로 좌우로 퍼지는 형태를 가지고 있다. 양쪽은 0으로 수렴한다. 



 식으로 표현하자면 다음과 같지만 외울 필요는 전혀 없다!! 대학 시험에서도 식을 주거나 식이 필요 없게 문제를 만든다.(적어도 내 수업들에서는 그러했다.) 중요한 것은


 Mean(평균 주축)

 Variance(분산 얼마나 퍼져 있는지 정도)


 이 두가지만 제대로 알고 있으면 된다. Variance가 클수록 더 퍼져있는 모양을 가지게 된다. 




 아래로 내려갈수록 더 퍼져 있는 가우시안이 보인다. 그러면 아래로 내려갈수록 분산의 크기도 더 커지고 있을 것임을 알 수 있다. 



 칼만 필터가 작동하는 원리에 대해서 아주 간단하게 말하고 있다. 지난 시간, Measurement는 Bayes Rule을 사용하고, Product연산으로 구하며, Prediction은 Probability의 총 합이 1이 되어야 함을 인지해서 Normalization을 해야 하고, Convolution연산으로 구할 수 있다고 하였다. 


 그렇게 해서 구한 이 Measurement와 Prediction의 함수들을 Gaussian을 이용해서 합치는 것이다!! 그래서 Gaussian이 매우 매우 중요하다고 위에서 강조하고 있는 것이다.



 검은색과 파란색의 두 확률 분포를 가우시안을 통해 합치게 된다면, 평균은 둘의 중간 위치를 가지게 되고, 분산은 무조건 작아지게 된다. 그래서 높이(이해를 위해 이렇게 표현하겠다.)는 무조건 높아지고, 이는 확실해진다는 것이다. (놀랍게도... 무조건!!!)



 새로운 평균과 분산을 구하는 식은 다음과 같은데, 외우면 좋지만 왜 이런 결과가 나오는지를 알려주기 위해서 강의에서는 제시하고 있다.



그래서 검은색의 두 확률분포처럼 전혀 상관이 없어 보이는 경우에도 우리의 갓 갓 가우시안은 분산을 줄여 주신다. 수식을 보더라도 분산이 반으로 줄어든 것을 볼 수 있다.



 어떠한 Motion을 취핬을 경우, Motion에 대한 불확실성 때문에 화살표 방향으로 확률 분포가 이동하면서 옆으로 더 퍼져있는 가우시안을 보이는 것을 알 수가 있다. 이 과정에서 새로운 가우시안에 대한 mean과 variance는 사진에서 파란 동그라미 안에 있는 바와 같다. 


 


 다차원에서의 가우시안이 어떠한 모양을 보이는지에 대한 그림이다. 위 그림을 보면 x의 값을 아는 상황에 y가 어떠한 값을 가질지 확률을 통해서 알 수 있음을 알 수가 있다. 만약 y에 대한 불확실성이 너무나 크다면 제일 오른쪽 그림과 같인 가우시안이 보일 것이고, x와 y의 블확실성이 모두 작다면 오른쪽에서 두 번째 그림과 같은 가우시안을 볼 수 있을 것이다. 이렇게 x와 y가 서로 상관이 있는 관계에 있다는 것을 영어로 Correlate하다고 한다. 



앞에서도 보았던 예제이다. 1초가 지나면서 x의 좌표 값이 1,2,3,4이렇게 늘어난다고 하는 경우이다. x와 x_dot(x를 시간에 대해 미분한 것 즉, 속도)의 관계를 그래프로 나타내고자 한다고 하자. x의 값에 대해서는 measurement를 한 값이 있기에 적은 불확실성을 보인다. 하지만, 속도 x_dot에대해서는 측정된 값이 없기 때문에 아주 큰 불확실성을 가진다. 다만! 


속도가 1일 경우 다음 x의 위치는 2이겠구나

속도가 2일 경우 다음 x의 위치는 3이겠구나

속도가 3일 경우 다음 x의 위치는 4이겠구나

 .......


이런 식으로 예측(=Prediction)을 할 수는 있고 이를 그래프 상에 가우시안으로 나타낼 수가 있겠다. 그렇게 그림을 그려본 것이 아래이다.



 지금까지의 상황을 정리하자면 처음으로 Measurement를 수행하고 Predict를 세웠다. 그럼 다시 로봇이 Motion을 취할 것이고 이는 자신의 위치 x에 대한 불확실성을 다시 높인다. 그럼 다시 로봇은 measurement를 수행할 것인데~



 짠!!! 1초 뒤에 위치가 2가 되었다. 이번에도 속도에 대한 불확실성은 엄청나게 높기 때문에 홀쭉한 가우시안의 모습을 보일 것이다. 그. 런. 데 이전에 세워 두었던 Predict와 겹치는 부분이 생긴 것이다. 그래서 이 두 가우시안을 곱해 보았더니, 속도에 대한 새로운 가우시안을 얻을 수 있게 된 것이다!! 앞서 말하였지만, 두 가우시안을 곱하는 과정에서 무조건 분산을 줄어들게 된다. 이는 곧, Measurement를 하면 할수록 더 정확한 속도를 구할 수가 있게 되었다는 뜻이다!! 사실 이 과정을 내가 실컷 써 놓았음에도 너무 장황한 것 같아서 정리를 해 보면...


      ● Measurement를 통해 현재 위치에 대한 가우시안을 얻었다.

      ● 속도에 대한 Predict를 세워보았다.

      ● 다시 Motion을 취해서 위치가 옮겨졌다.

      ● 옮겨진 위치에서 Measurement를 수행해 보니 이전에 Predict와 겹치는 부분이 나타났다.

      ● 나는 위치에 대한 Measurement만 수행했는데 속도에 대한 값도 얻게 되었다!



 States(x 위치)에 대한 정보를 얻었는데(observables), 알지 못하였던 속도(Hidden)에 대한 정보를 얻었다. 이는 Hidden되어 있는 값이 시간과 x 위치에 대해서 상관관계를 지니고 있었기 때문이다.   칼만 필터는 이렇게 주어진 정보들을 통해서 이들과 관계가 있는 숨겨진 정보를 얻게 해주는 필터이다.



 지금부터 2차원 칼만 필터를 설계할 것이다. 이를 위해서는 State와 Velocity에 대한 해석이 필요한데, 공대생이 미분 방정식을 세우는 것이 바로 이러한 것을 하는 것이란다. '계'를 해석한다고 한단다. 


 위치와 속도에 대한 벡터를 알아내는 것이 우리의 목표이다. 이 벡터가 다음 time step에서 어떠한 값을 갖게 되는지를 알려주는 transition-Matrix(F)를 세워 보자, 다음 위치는 현재 위치 + 속도 * time_step인데 time_step이 1이라고 하였다. 그래서 [[1, 1], [0, 1]]이러한 값을 가지는 F를 세웠다.


그리고, 순수하게 Measurement의 값만을 추출하는 Matrix H를 설계한다. 이제, 칼만 필터를 순서대로 수행할 것인데 그 수식을 다음과 같다. 



 음... 보기만 해도 복잡하다. 왜 이렇게 나오는지 궁금하기도 해서 이것저것 자료를 찾아 보았는데, 직관적으로 이해를 시켜준 자료가 있어서 링크를 남긴다. - 칼만 필터의 이해 -


 이 문서를 아~~주 간략하게 이야기해 본다. 앞서, Prediction과 Measurement 이 두 가우시안의 합성으로 더 정확한 벡터를 찾아낸다고 하였는데, 다음의 공식을 사용해야 한다고도 했다.


출처 : http://nerve.tistory.com

 식을 조금 변형해 보았는데, 실제로 Measurement와 Prediction에서의 노이즈를 아직 고려하지 않은 상황이다. 이 두 노이즈를 도메인까지 맞추어서 상수 c라고 정의를 하고 적용해 준다.


출처 : http://nerve.tistory.com

 그럼, 이제 기존의 평균, 분산을 구하는 식에 이를 최종적으로 적용을 하면 아래와 같다.

 

 출처 : http://nerve.tistory.com

변수들을 다시 정의 해준다.

출처 : http://nerve.tistory.com


음... 일단 나도 완벽하게 이해를 하지 못해서 정리하는 것도 간략하게만 해 두었는데, 말하고 싶은 것은 이거다. 칼만 필터의 수식은 결국 가우시안의 평균과 분산 공식에서 유도된 것이다!!


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
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
# Write a function 'kalman_filter' that implements a multi-
# dimensional Kalman Filter for the example given
 
from math import *
 
 
class matrix:
    
    # implements basic operations of a matrix class
    
    def __init__(self, value):
        self.value = value
        self.dimx = len(value)
        self.dimy = len(value[0])
        if value == [[]]:
            self.dimx = 0
    
    def zero(self, dimx, dimy):
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError
        else:
            self.dimx = dimx
            self.dimy = dimy
            self.value = [[0 for row in range(dimy)] for col in range(dimx)]
    
    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError
        else:
            self.dimx = dim
            self.dimy = dim
            self.value = [[0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1
    
    def show(self):
        for i in range(self.dimx):
            print(self.value[i])
        print(' ')
    
    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError
        else:
            # add if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res
    
    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError
        else:
            # subtract if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res
    
    def __mul__(self, other):
        # check if correct dimensions
        if self.dimy != other.dimx:
            raise ValueError
        else:
            # multiply if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
            return res
    
    def transpose(self):
        # compute transpose
        res = matrix([[]])
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res
    
    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
    
    def Cholesky(self, ztol=1.0e-5):
        # Computes the upper triangular Cholesky factorization of
        # a positive definite matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        
        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else:
                if d < 0.0:
                    raise ValueError
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                if abs(S) < ztol:
                    S = 0.0
                try:
                   res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
                except:
                   raise ValueError
        return res
    
    def CholeskyInverse(self):
        # Computes inverse of matrix given its Cholesky upper Triangular
        # decomposition of matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        
        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/tjj**2 - S/tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
        return res
    
    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res
    
    def __repr__(self):
        return repr(self.value)
 
 
########################################
 
# Implement the filter function below
 
def kalman_filter(x, P):
    for n in range(len(measurements)):
        # measurement update
        z = matrix([[measurements[n]]])
        y = z - ( H * x )
        S = ( H * P * H.transpose() ) + R
        K = P * H.transpose() * S.inverse()
 
        # prediction
        x = x + (K * y)
        P = (I - K * H) * P
 
        x = ( F * x ) + u
        P = F * P * F.transpose()
 
        print('x', x, 'P', P)
    return (x,P)
 
############################################
### use the code below to test your filter!
############################################
 
measurements = [1., 2., 3.]
 
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
 
print(kalman_filter(x, P))
# output should be:
# x: [[3.9996664447958645], [0.9999998335552873]]
# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]
 
cs

 

갑자기 코드가 툭 튀어나왔다. 이는 강좌에서 실습 예제로 짜라고 준 코드인데, 대부분 행열 연산과 형식에 대한 것이고 중요한 부분은 아래이다.


1
2
3
4
5
6
7
8
9
measurements = [1., 2., 3.]
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
print(kalman_filter(x, P))
cs


 1, 2, 3이렇게 이동하면서 Measurement를 하였고, 초기 위치는 0이었다고 가정한 1-D 칼만 필터의 설계이다. 사실 필요한 모든 벡터들과 행렬들을 주어진 상황에서 칼만 필터 수식을 재현만

(kalman_filter라는 함수로 해 보라는 미션이었다. 


1
2
3
4
5
6
7
8
9
10
11
12
13
14
def kalman_filter(x, P):
    for n in range(len(measurements)):
        # measurement update
        z = matrix([[measurements[n]]])
        y = z - ( H * x )
        S = ( H * P * H.transpose() ) + R
        K = P * H.transpose() * S.inverse()
        # prediction
        x = x + (K * y)
        P = (I - K * H) * P
        x = ( F * x ) + u
        P = F * P * F.transpose()
        print('x', x, 'P', P)
    return (x,P)
cs


 사실 수식을 그대로 옮긴 것이기에 큰 의미는 없을 수도 있다고 할 수 있지만, 각각의 요소들이 어떠한 의미를 가지고 있고, 행렬의 각 원소가 어떠한 의미를 지니고 있는지 다시 한 번 짚고 넘어가는 차원에서 올려 본다.