Kalman Filter 공부했다. - 2 차원 이동 실습

지난 시간에 1차원에 대한 칼만 필터를 설계해 보았었다. 그 코드를 잠시 보자면

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
 
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)

 위와 같았는데, 칼만 필터에 대한 설명은 지난 번에 하였으므로, 이번에는 각 Matrix들이 왜 저렇게 생겼는지 알아보고, 차원을 높여서 2차원에서 칼만 필터를 설계해 본다.

 주의 깊게 봐야 할 Matrix로 P, F, H 정도가 있다. P는 두 벡터의 불확실성에 대한 행렬로 칼만 필터를 처음 실행할 시점에선 불확실성을 높게 1000으로 잡아 주었고, 위치와 속도 둘 사이의 불확실성에는 서로 관계가 없으므로 대각 성분에만 값을 넣어준 것이 보인다. 

 H는 벡터에서 측정 값만을 뽑아내는 행렬인데, 우리는 위치 x에 대해서만 측정을 하므로 [1, 0]

이런 모양을 하고 있게 된다.

 F는 state를 나타내는 벡터로 사실 이것을 설명하기 위해서 이 글을 작성하는 것이기도 하다.

  이는 약간의 물리 지식이 필요하기도 하다. 속도를 가진 어떤 물체의 위치를 알고 싶다고 할 때

거리 = 시간 * 속도

라는 놀라운 사실을 알고 있어야 한다. 그래서 속도(X_dot)를 통해 0.1초 후의 어떠한 물체의 위치(X)를 알고 싶다고 한다면, X = X + 0.1*X_dot 이 되는 것이다. - 다만, 칼만 필터를 사용하면서 우리는 속도의 값을 직접적으로 받을 수 없었다는 것을 기억하자.

그래서 위 그림은 2차원 (x,y)인 상황에서 x와 x_dot(x 방향으로의 속도), y와 y_dot(y방향으로의 속도)를 하나의 벡터로 나타낼 때 다음 time_step(=0.1)에서의 상태를 나타내는 새로운 행렬 F 을 나타내 보라는 문제이다. 

 일단 수식들을 먼저 정리해 보자. time_step(=0.1)전의 상태들에 대해서는 각 변수들에 _1을 붙이도록 하겠다. x-1, y-1이렇게 말이다. 

x = x-1 + 0.1 * x_dot

x_dot = x_dot-1

y = y-1 + 0.1 * y_dot

y_dot = y_dot-1

 자, 이렇게 모든 변수들에 대해서 식이 완성되었으므로 F행렬의 빈칸을 채울 수 있게 되었다. 그럼 나머지 행렬들에 대해서는 앞서 말하였으니, 실질적인 코드를 채울 수 있게 되었다.

dt = 0.1
 
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
 
 
P =  matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 1000., 0.], [0., 0., 0., 1000.]])# initial uncertainty: 0 for positions x and y, 1000 for the two velocities
F =  matrix([[1., 0., 0.1, 0.], [0., 1., 0., 0.1], [0., 0., 1., 0.], [0., 0., 0., 1.]])# next state function: generalize the 2d version to 4d
H =  matrix([[1., 0., 0., 0.], [0., 1., 0., 0.]])# measurement function: reflect the fact that we observe x and y but not the two velocities
R =  matrix([[0.1, 0.], [0., 0.1]])# measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
I =  matrix([[1., 0., 0., 0.], [0., 1., 0., 0. ], [0., 0., 1., 0.], [0., 0., 0., 1.]])# 4d identity matrix

짠! x-y좌표에서의 칼만 필터가 구현이 되었다!!

# Fill in the matrices P, F, H, R and I at the bottom
#
# This question requires NO CODING, just fill in the 
# matrices where indicated. Please do not delete or modify
# any provided code OR comments. Good luck!
 
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:
            # subtract 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)
 
 
########################################
 
def filter(x, P):
    for n in range(len(measurements)):
        
        # prediction
        x = (F * x) + u
        P = F * P * F.transpose()
        
        # measurement update
        Z = matrix([measurements[n]])
        y = Z.transpose() - (H * x)
        S = H * P * H.transpose() + R
        K = P * H.transpose() * S.inverse()
        x = x + (K * y)
        P = (I - (K * H)) * P
    
    print('x= ')
    x.show()
    print('P= ')
    P.show()
 
########################################
 
print("### 4-dimensional example ###")
 
measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
initial_xy = [4., 12.]
 
# measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
# initial_xy = [-4., 8.]
 
# measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]
# initial_xy = [1., 19.]
 
dt = 0.1
 
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
 
#### DO NOT MODIFY ANYTHING ABOVE HERE ####
#### fill this in, remember to use the matrix() function!: ####
 
P =  matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 1000., 0.], [0., 0., 0., 1000.]])# initial uncertainty: 0 for positions x and y, 1000 for the two velocities
F =  matrix([[1., 0., 0.1, 0.], [0., 1., 0., 0.1], [0., 0., 1., 0.], [0., 0., 0., 1.]])# next state function: generalize the 2d version to 4d
H =  matrix([[1., 0., 0., 0.], [0., 1., 0., 0.]])# measurement function: reflect the fact that we observe x and y but not the two velocities
R =  matrix([[0.1, 0.], [0., 0.1]])# measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
I =  matrix([[1., 0., 0., 0.], [0., 1., 0., 0. ], [0., 0., 1., 0.], [0., 0., 0., 1.]])# 4d identity matrix
 
###### DO NOT MODIFY ANYTHING HERE #######
 
filter(x, P)
 

 짚고 넘어가야 할 부분이 조금 있는데

u = matrix([[0.], [0.], [0.], [0.]]) # external motion

 예제에서는 모두 그냥 0행렬이었던 이 U가 무슨의미일까?? 이는 외부의 힘, 그래서 물리적으로 는 곧 "가속도"를 의미한다. 지금은 그저 물체의 위치를 보면서 속도와 다음 위치를 예측하는 것일 뿐 외부의 가속도는 알 수가 없으므로 0으로 취급했던 것이다.

P =  matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 1000., 0.], [0., 0., 0., 1000.]])

불확실성을 나타내는 행렬 P, 여기에서 비대각성분(off_diagonal)은 전부 0으로 되어 있다 이것이 무슨 의미를 가지고 있는 것이고 왜 0으로 설정해 준 것일까?? 

 어떠한 두 변수가 서로 관계를 가진다(속도와 위치처럼)는 것을 숫자로 표현한 것을 Covariance라고 한다. 이것이 높을 수록 서로 연관이 크다는 것이다. 그런 의미에서 대각 성분은 자기 자신과의 관계를 뜻한다.

 위에서는 비대각 성분들이 모두 0이었다. 하지만, 우리는 이 변수들이 서로 영향이 있다는 것을 알고 있다. 그래서 칼만 필터가 진행이 될 수록 이 비대각 성분들은 값을 보이기 시작할 것이다. 그리고 속도 성분들에 대해서도 1000이라는 큰 불확실성이 점차 작아질 것이라는 것을 볼 수 있다.