RoyalAzalea's Personal Blog

Tensorflow Study _ LAB 02 : Linear Regression

Mon, 28 Aug 2017 00:00:00 +0000

Tensorflow를 사용하여 Linear Regression을 구현해보자. 구현 과정은 다음과 같다.

Graph build
Session run
Update

1. Graph build

Hypothesis
- tf.Variable을 이용하여 Weight과 Bias를 랜덤으로 생성
- random_normal을 사용할 때 shape을 설정해야 함(ex. [1], [1,2])
- Tensorflow 에서 Variable은 기존 프로그래밍 언어의 변수들과는 조금 다른 개념이다. Ternsorflow가 사용하는 변수로 자체적으로 변경한다. trainable variable로 봐도 된다.
Cost function
- tf.square를 통해 제곱을 표현
- reduce_mean이라는 평균을 내주는 함수를 사용
- train이라는 이름의 노드로 표현
Minimize
- cost를 최소화 하기 위해 GradientDescent를 사용(여러 방법중 하나로, 현재는 마법처럼 생각하기!)
- learning_rate는 최소값을 찾기위해 이동하는 단계를 의미

2. Session run

세션을 만든 이후 실행하기 전에 Tensorflow의 글로벌 변수 초기화를 시켜줘야 함!
- tf.global_variables_initailizer() 사용
2000번의 step 중 20번에 한번 씩 출력하도록 세팅
train을 실행시키면 cost를 따라 cost에 붙은 hypothesis등 여러 노드들이 실행됨

3. update

내부적으로 W와 b에 대한 값이 업데이트 됨

lab_02_01.py

# Lab 2 Linear Regression
import time
import tensorflow as tf
tf.set_random_seed(777) # for reproducibility

# Setting X and Y data
x_train = [1,2,3]
y_train = [1,2,3]

'''
W와 b 값을 찾기위해 y_data = x_data * W + b 사용
이미 W = 1, b = 0 임을 알고 있지만 TensorFlow가 잘 찾는지 확인해보자
'''
W = tf.Variable(tf.random_normal([1]), name = 'weight')
b = tf.Variable(tf.random_normal([1]), name = 'bias')

# Hypothesis 구현
hypothesis = x_train * W + b

# Cost/loss function 구현
cost = tf.reduce_mean(tf.square(hypothesis - y_train))

# Minimize
optimizer = tf.train.GradientDescentOptimizer(learning_rate=0.01)
train = optimizer.minimize(cost)

# Graph의 session을 만들고 시작(graph의 global 변수들 반드시 초기화 해야함!)
sess = tf.Session()
sess.run(tf.global_variables_initializer())

# Fit in line
start_time = time.time()

for step in range(2001):
    sess.run(train)
    if step % 20 == 0:
        print("step: %s, cost: %s, W: %s, b: %s " % (step, sess.run(cost), sess.run(W), sess.run(b)))

end_time = time.time()

print("total time : " , round((end_time - start_time)*1000), " ms")

result

step: 0, cost: 2.82329, W: [ 2.12867713], b: [-0.85235667]
step: 20, cost: 0.190351, W: [ 1.53392804], b: [-1.05059612]
step: 40, cost: 0.151357, W: [ 1.45725465], b: [-1.02391243]
 ~
step: 1920, cost: 1.77484e-05, W: [ 1.00489295], b: [-0.01112291]
step: 1940, cost: 1.61197e-05, W: [ 1.00466311], b: [-0.01060018]
step: 1960, cost: 1.46397e-05, W: [ 1.004444], b: [-0.01010205]
step: 1980, cost: 1.32962e-05, W: [ 1.00423515], b: [-0.00962736]
step: 2000, cost: 1.20761e-05, W: [ 1.00403607], b: [-0.00917497]
total time :  361  ms

위 결과를 보면, W가 1로, b는 0으로 다가가고 있음을 볼 수 있다. 아래는 Placeholder를 사용하여 Linear Regression을 구현한 코드이다.

세션을 실행할 때 list를 사용하여 한꺼번에 실행시켜줄 수 있음 (ex. [cost, W, b, train])
cost_val은 cost, W_val은 W, b_val은 b의 return을 받음, 그리고 _는 train의 return을 받으나 굳이 필요하지 않아서 _로 씀
Placeholder를 이용하는 큰 이유중 하나는 만든 모델에 직접 값들을 줄 수 있기 때문이다.
Placeholder를 만들 때, shape = [None]을 쓰면 1차원 값을 몇개든 받을 수 있다!
학습 후 hypothesis에 직접 X 값을 주면, 학습모델을 통한 예측값 Y를 얻을 수 있음

lab_02_02.py

# Lab 2 Linear Regression
import time
import tensorflow as tf
tf.set_random_seed(777) # for reproducibility

'''
W와 b 값을 찾기위해 y_data = x_data * W + b 사용
이미 W = 1, b = 0 임을 알고 있지만 TensorFlow가 잘 찾는지 확인해보자
'''
W = tf.Variable(tf.random_normal([1]), name = 'weight')
b = tf.Variable(tf.random_normal([1]), name = 'bias')

'''
X_data와 Y_data대신 Placeholder를 사용하여 만든 모델에
값을 줘보자
'''
X = tf.placeholder(tf.float32, shape=[None])
Y = tf.placeholder(tf.float32, shape=[None])

# Hypothesis 구현
hypothesis = X * W + b

# Cost/loss function 구현
cost = tf.reduce_mean(tf.square(hypothesis - Y))

# Minimize
optimizer = tf.train.GradientDescentOptimizer(learning_rate=0.01)
train = optimizer.minimize(cost)

# Graph의 session을 만들고 시작(graph의 global 변수들 반드시 초기화 해야함!)
sess = tf.Session()
sess.run(tf.global_variables_initializer())

# Fit in line
print("step -- cost -- weight -- bias --")

start_time = time.time()

for step in range(2001):
    cost_val, W_val, b_val, _ = \
    sess.run([cost,W,b,train],
             feed_dict={X: [1, 2, 3, 4, 5], Y:[2.1, 3.1, 4.1, 5.1, 6.1]})

    if step % 20 == 0:
        print("step: %s, cost: %s, W: %s, b: %s " % (step, cost_val, W_val, b_val))

end_time = time.time()

print("total time : ", round((end_time - start_time) * 1000), " ms")

print("-----------testing our model-----------")

print("X : [1.5, 3.5]")
print("Y : %s " % sess.run(hypothesis, feed_dict={X: [1.5, 3.5]}))

result

step -- cost -- weight -- bias --
step: 0, cost: 5.8308, W: [ 2.05798721], b: [-0.85453004]
step: 20, cost: 0.697443, W: [ 1.54248559], b: [-0.85012037]
step: 40, cost: 0.608993, W: [ 1.50494218], b: [-0.72296369]
step: 60, cost: 0.531838, W: [ 1.47186399], b: [-0.60357893]
step: 80, cost: 0.464458, W: [ 1.44096124], b: [-0.49201021]
 ~
step: 1900, cost: 2.05671e-06, W: [ 1.00092793], b: [ 1.09664977]
step: 1920, cost: 1.79626e-06, W: [ 1.00086725], b: [ 1.09686911]
step: 1940, cost: 1.56876e-06, W: [ 1.0008105], b: [ 1.09707403]
step: 1960, cost: 1.37009e-06, W: [ 1.00075734], b: [ 1.0972656]
step: 1980, cost: 1.19689e-06, W: [ 1.00070786], b: [ 1.09744453]
step: 2000, cost: 1.04504e-06, W: [ 1.00066149], b: [ 1.09761178]
total time :  783  ms
-----------testing our model-----------
X : [1.5, 3.5]
Y : [ 2.5986042   4.59992695]

Tensorflow Study _ LEC 02 : Linear Regression

Mon, 28 Aug 2017 00:00:00 +0000

Linear Regression은 Supervised Learning의 한 종류로, 학습을 통해 Traning Data로 부터 선형모델을 만든다.

“예를 들어, 학생들의 ‘공부시간 - 시험점수’ 관한 Data가 있다. 이때, 공부시간이 많은 학생일수록 시험점수가 좋다고 가정해보자. 이 Data를 학습시킨 후, 시험보기 전 내가 공부한 시간을 모델에 입력하면 대략 몇 점이 나올 것이라고 답이 나온다. 그리고 그 답으로 나온 점수는 공부한 시간이 많다면 높게, 공부한 시간이 적다면 낮게 나온다. 왜냐하면, 공부시간이 많을 수록 시험점수가 좋은 Data로 학습을 시켰기 때문이다.”

이처럼, Linear Regression을 통해 가설(Hypothesis)을 세울 수 있다. 이 가설은 우리가 Traning Data로 부터 학습한 모델이 선형(Linear)일 것이라고 예측한다. 즉, 어떤 Data들을 학습해서 여기에 잘 맞는 선형모델 or 선 을 찾는 것 이다. 참고로 학습을 시킬 때, $x$는 예측을 하기위한 기본적인 자료로 Feature라고 불리며, $y$는 해당 $x$의 결과이다.

이제 Linear Regression을 통해 선을 찾는것을 알게됐다. 근데, 우리가 찾은 선들 중 어떤 선이 좋은 선일까? 우리가 세운 가설과 실제 값이 얼마나 다른지 측정하면 된다. 즉, 우리가 측정한 가설과 실제 점들 사이의 거리가 가까우면 좋은 선이고, 멀면 좋지 않은 선이다. 그리고 우리가 세운 가설과 실제 값이 얼마나 다른지 확인하는 함수를 비용함수(cost function or loss function)라고 한다.

그림

이제 cost function을 이용해서 최적의 선을 찾는 방법을 보겠다. 우선, Linear Regression에서 Hypothesis를 1차 방정식이라고 생각해보자. 그럼 다음과 같은 식이 나온다.

$H(x) = Wx + b \label{1}\tag{1}$

그리고 실제 값과 예측 값의 차이는 선과 점들 사이의 거리를 계산해서 구하면 된다. 예측 값과 실제 값과의 차이는 $H(x) - y$로 구할 수 있다. 하지만, 이 식은 음수가 나오므로 좋지않다. 앞의 식 대신 $(H(x) - y)^2$을 사용한다. 이 식은 제곱을 하므로 나오는 값이 모두 양수가 되고, 예측 값과 실제 값의 차이가 크면 cost가 커지므로 패널티도 줄 수 있다.

이제 위의 식을 기반으로 선과 모든 점들의 거리를 구하여 제곱하고 더하여 평균을 낸다. 그러면 다음의 일반화 된 cost function들을 구할 수 있다.

$cost_1 = {1 \over m}\sum_{i=i}^m (H(x^{(i)}) - y^{(i)})^2 \label{2}\tag{2}$ $cost_2 = {1 \over 2m}\sum_{i=i}^m (H(x^{(i)}) - y^{(i)})^2 \label{3}\tag{3}$

여기서 $H(x^{(i)})$는 가정으로 얻은 예측값이고, $y^{(i)}$는 실제 값이다. 그리고 식 $\ref{1}$에서 $H(x)$는 $W$와 $b$의 함수이므로, 식 $\ref{2}$와 $\ref{3}$의 $cost_1$과 $cost_2$도 $W$와 $b$의 함수가 된다.

cost가 작을수록 예측 값과 실제 값의 차이가 적으므로, 학습 시 cost 값을 최소로 만드는 $W$와 $b$를 구하는게 학습의 목표이다!!

Tensorflow Study _ LAB 01 : Tensorflow Basics

Tue, 01 Aug 2017 00:00:00 +0000

Tensorflow Basics

1. Introduction

Tensorflow는 Machine Learning을 하기위한 open-source 라이브러리로 현재 많은 ML 라이브러리 중 가장 많은 사람들이 사용하고 있다. Tensorflow의 특징은 다음과 같다.

data flow graphs를 통한 numerical computation, 풍부한 표현력, 직관적인 data 흐름 인식
코드 수정 없이 CPU/GPU mode 동작
계산 구조와 목표 함수 정의를 통한 자동 미분 계산 처리
python/C++ 및 다양한 언어 지원

2. Terminology

Ternsorflow 공부에 앞서, Tensorflow에서 사용되는 기본 용어들을 살펴보겠다.

Operation(op)

graph 상의 node는 operation(op)라고 불린다. operation은 하나 이상의 tensor를 받을 수 있다. 그리고 operation은 계산을 수행하고, 결과를 하나 이상의 tensor로 반환할 수 있다.
Tensor

내부적으로 모든 데이터는 tensor로 표현된다. tensor는 일종의 다차원 배열로, graph 내의 operation 간에는 tensor만이 전달된다.
Session

graph를 실행하기 위해서는 session 객체가 필요하다. session은 operation의 실행 환경을 캡슐화 한것이다.
Variables

variables는 graph 실행시, parameter를 저장하고 갱신하는데 사용된다. 메모리 상에서 tensor를 저장하는 버퍼 역할을 한다.

3. data flow graphs

data flow graphs는 node와 edge를 사용한 방향 그래프(Directed Graph)이다. node는 수학적 계산, 데이터 입/출력, 그리고 데이터의 읽기/저장 등의 작업을 수행한다. edge는 node들 간 데이터의 입출력 관계를 나타낸다.

간략히 data flow graphs에서 node들은 하나의 operation을 뜻하고, edge는 data arrays(tensor)를 뜻한다고 생각할 수 있다. 그리고 tensor들이 edge를 통해 돌아다니면서 설정한 작업들이 진행된다.

4. Tensorflow Mechanism

Tensorflow Mechanism을 이해하기 위해 아래 그림을 보자.

그림의 순서와 같이 처음 operation을 이용하여 graph를 build 한다. 그 다음 session을 통해 graph를 실행하고 graph를 통해 variables return/update 한다.

5. Examples

간단하게 Tensorflow를 통해 Hello World를 출력하는 예제를 살펴보겠다. hello라는 constant node를 만들고, sess라는 session을 만들어 run 하여 실행한다.

code

import tensorflow as tf
# hello test
print("------------------------------------------------------------")
print("@ Hello Test")

hello = tf.constant('Hello, TensorFlow!')
sess = tf.Session()
print(sess.run(hello))

result

------------------------------------------------------------
@ Hello Test
b'Hello, TensorFlow!'

출력 결과를 보면 b라는 문자가 나오는데 이건 Bytes Literals라고 알려주는 것 뿐이다.

다음 예제는 Tensorflow version을 확인하고, session 사용/미사용 시 어떠한 출력을 해주는지 보는 예제이다.

code

import tensorflow as tf
# Check TF version
print("------------------------------------------------------------")
print("@ Tensorflow version check")
print(tf.__version__)

# Computational Graph
print("------------------------------------------------------------")
print("@ Not using session")
node1 = tf.constant(3.0, tf.float32)
node2 = tf.constant(4.0) # also tf.float32 implicitly
node3 = tf.add(node1, node2)

print("node1: ", node1, "node2: ", node2)
print("node3: ", node3)

print("@ Using session")

sess = tf.Session()
print("sess.run(node1, node2): ", sess.run([node1, node2]))
print("sess.run(node3): ", sess.run(node3))

result

------------------------------------------------------------
@ Tensorflow version check
1.0.1
------------------------------------------------------------
@ Not using session
node1:  Tensor("Const_1:0", shape=(), dtype=float32) node2:  Tensor("Const_2:0", shape=(), dtype=float32)
node3:  Tensor("Add:0", shape=(), dtype=float32)
@ Using session
sess.run(node1, node2):  [3.0, 4.0]
sess.run(node3):  7.0

결과를 보면 session을 사용하지 않은 경우 출력으로 하나의 tensor라고 알려만 준다. 그러므로 어떠한 연산을 진행하고 결과값을 보려면 session을 만들어 node를 실행해야 한다.

다음 예제는 Placeholder를 사용하는 예제이다. Placeholder는 미리 그래프를 만들어 놓고 실행시키는 단계에서 값들을 주고 싶을 때 사용한다. node를 만드는 단계에서 Placeholder를 사용하여 만들며, 값을 줄 때는 feed_dict를 통해 각 node에 값을 전해준다.(참고로 배열도 가능!)

code

import tensorflow as tf
# Placeholder
print("------------------------------------------------------------")
print("@ Placeholder & feed_dict test")

a = tf.placeholder(tf.float32)
b = tf.placeholder(tf.float32)
adder_node = a + b # + provides a shortcut for tf.add(a, b)

print(sess.run(adder_node, feed_dict={a: 3, b: 4.5}))
print(sess.run(adder_node, feed_dict={a: [1,3], b: [2, 4]}))

print("@ add_and_triple test")
add_and_triple = adder_node * 3.
print(sess.run(add_and_triple, feed_dict = {a: 3, b: 4.5}))

result

------------------------------------------------------------
@ Placeholder & feed_dict test
7.5
[ 3.  7.]
@ add_and_triple test
22.5

Tensorflow Study _ LEC 01 : Machine Learning Basics

Tue, 01 Aug 2017 00:00:00 +0000

Machine Learning Basics

일반적으로 개발자들이 만든 프로그램은 개발자가 만든 대로 동작하는 explicit 프로그램이다. 좋은 프로그램들이 많지만, rule이 많은 상황에서는 이러한 explicit 프로그램을 만들기 어렵다. 1959년에 우리가 일일히 프로그래밍 하지 말고, 프로그램 자체가 어떤 현상이나 자료를 학습을 해서 스스로 무언갈 배워보자는 재밌는 생각이 나왔다. 그리고 이 재밌는 생각이 바로 Machine Learning의 시작이다.

참고로 Machine Learning이란 단어는 처음 Arthur Samuel 교수가 1959년에 발표한 논문 “Some Studies in Machine Learning Using the Game of Checkers” 에서 “Machine Learning: Field of study that gives computers the ability to learn without being explicitly programmed” 라고 정의했다.

Machine Learning을 하기 위해 프로그램은 학습을 해야한다. 학습의 방법에 따라 Supervised Learning과 Unsupervised Learning으로 나뉜다. 아래 Machine Learning 에 대해 간단하게 분류를 해보았다. 아래 분류는 간단한 분류로 다음에 여러 분류에 대해 언급하겠다.

Supervised Learning
- Regression
- Binary Classfication
- Multi-label Classfication
Unsupervised Learning

Supervised Learning의 경우 labeled data/information을 가지고 학습한다. 우리가 일일히 data/information의 label을 나누고 학습을 하도록 한다.

Unsupervised Learning의 경우 un-labeled data/information을 가지고 학습한다. 우리가 일일히 label을 주지 않고, 프로그램이 data/information을 보고 스스로 학습하여 유사한 것들을 모으거나 필요한 작업을 한다.

일반적으로 Machine Learning에서 Supervised Learning이 많이 쓰인다. Supervised Learning에는 크게 세 종류가 있다. 점수처럼 측정이 되는 Regression, pass/non-pass처럼 결정이 되는 Binary Classification, 그리고 학점처럼 등급이 나오는 Multi-label Classfication이 있다.

참고로 Machine Learning을 하기 위해 프로그램이 학습 할 data가 필요하다. 보통 이 data를 Traning Data Set이라고 하며, Machine Learning에서 Traning Data Set을 사용하여 학습을 통해 어떤 모델을 만들게 된다. 그리고 이 모델을 통해 어떤 input에 대한 output을 얻을 수 있다.

Tensorflow Study _ 환경 구성

Fri, 07 Apr 2017 00:00:00 +0000

설치환경 Window 10

Tensorflow를 공부하기 위한 환경 구성 (Python, PyCharm, Anaconda)

공부자료

모두를 위한 머신러닝/딥러닝 강의 : https://hunkim.github.io/ml/
Tensorflow Docs 번역본 : https://tensorflowkorea.gitbooks.io/tensorflow-kr/content/

1. 설치

Python : https://www.python.org/downloads/release/python-352/
(현재 tensorflow에서 python version 3.6.X 를 지원해주지 않는다. 그러므로 python version 3.5.X를 설치해야 한다. 참고로 3.5.2 설치함)

PyCharm : https://www.jetbrains.com/pycharm/download/#section=windows

Anaconda : https://www.continuum.io/downloads (설치할 Anaconda는 python version과 사용할 bit(64 or 32)를 맞춰야한다. tensorflow는 64 bit에서 지원하기 때문에 64 bit로 설치해야 한다. 참고로 python 3.5 version 3.5.X와 64 bit에 맞는 Anaconda3-4.2.X를 설치해야 한다.)

2. Anaconda에 Tensorflow 설치

tensorflow라는 이름으로 콘다환경을 구성한다.(python version 3.5.X 사용)

C:> conda create -n tensorflow python=3.5

이제 tensorflow라는 이름의 콘다환경을 활성화 시켜준다.

C:> activate tensorflow
 (tensorflow)C:>  # Your prompt should change

tensorflow라는 이름의 콘다환경에 Tensorflow 를 설치한다. (CPU-only version과 GPU version 중 우선 CPU-only version만 설치)

CPU-only version

(tensorflow)C:> pip install --ignore-installed --upgrade https://storage.googleapis.com/tensorflow/windows/cpu/tensorflow-1.0.1-cp35-cp35m-win_amd64.whl

GPU version

(tensorflow)C:> pip install --ignore-installed --upgrade https://storage.googleapis.com/tensorflow/windows/gpu/tensorflow_gpu-1.0.1-cp35-cp35m-win_amd64.whl

3. PyCharm에서 구성한 콘다환경 설정

우선 PyCharm에서 python Project를 만든다. 이후 Project Interpreter를 설정할 때 위에서 구성한 tensorflow라는 이름의 콘다환경에 들어있는 python.exe를 선택한다.

이제 File -> Setting -> Project:XXX -> Project Interpreter 를 보면 Package에 tensorflow가 있음을 확인할 수 있다. (tensorflow라는 콘다환경에 TensorFlow 가 설치됨)

아래는 예제와 테스트 결과이다.

test.py

import tensorflow as tf


hello = tf.constant('Hello, TensorFlow!')
sess = tf.Session()
print(sess.run(hello))

result

C:\DevProgram\Anaconda3\envs\tensorflow\python.exe C:/Users/RoyalAzalea/Documents/Pycharm/lab_01/test.py
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "BestSplits" device_type: "CPU"') for unknown op: BestSplits
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "CountExtremelyRandomStats" device_type: "CPU"') for unknown op: CountExtremelyRandomStats
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "FinishedNodes" device_type: "CPU"') for unknown op: FinishedNodes
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "GrowTree" device_type: "CPU"') for unknown op: GrowTree
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "ReinterpretStringToFloat" device_type: "CPU"') for unknown op: ReinterpretStringToFloat
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "SampleInputs" device_type: "CPU"') for unknown op: SampleInputs
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "ScatterAddNdim" device_type: "CPU"') for unknown op: ScatterAddNdim
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "TopNInsert" device_type: "CPU"') for unknown op: TopNInsert
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "TopNRemove" device_type: "CPU"') for unknown op: TopNRemove
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "TreePredictions" device_type: "CPU"') for unknown op: TreePredictions
E c:\tf_jenkins\home\workspace\release-win\device\cpu\os\windows\tensorflow\core\framework\op_kernel.cc:943] OpKernel ('op: "UpdateFertileSlots" device_type: "CPU"') for unknown op: UpdateFertileSlots
b'Hello, TensorFlow!'

Process finished with exit code 0

Kalman Filter Study _ 13장. 수평 자세 측정_2부

Wed, 29 Mar 2017 00:00:00 +0000

13.3 센서 융합을 통한 자세 결정

지금까지 자이로와 가속도계로 자세를 결정하는 방법과 결과를 분석해 봤다. 그 결과 두 센서 모두 단독으로 사용하기에는 약점이 있었다. 물론 고가의 센서를 쓰면 되겠지만 우리의 관심사항은 아니다.

그런데 분석 결과를 찬찬히 검토해 보면 두 센서가 상호 보완 관계인 것을 알 수 있다. 가속도계로 구한 자세는 시간이 지나도 오차가 커지지 않고 일정 범위로 제한되는 장점을 가졌다. 반면 자이로는 자세 변화는 잘 감지하지만 시간이 지나면 오차가 누적되어 발산하는 문제가 있었다. 즉 단기적으로는 자이로 자세가 더 낫지만, 중장기적으로는 가속도 자세가 더 좋다. 그렇다면 자이로의 누적 오차 문제를 가속도계로 보정하면 좋지 않을까? 빙고! 이게 바로 센서 융합이다.

여러 센서의 출력을 모아서 더 좋은 성능을 끌어내는 기법을 ‘센서융합(Sensor Fusion)’ 이라고 한다. ‘센서 결합’ 이라는 용어도 많이 쓴다. 그런데 센서를 융합한다고 해서 아무 센서나 조합하면 안된다. 센서 융합의 목적은 개별 센서로는 불가능한 성능을 내겠다는 것인데, 센서 구성이 나쁘면 시너지 효과를 기대하기 어렵기 때문이다.

융합용 센서를 구성할 때는 특성을 잘 분석해서 상호 보완이 되는 센서끼리 묶는 것이 중요하다. 이 센서의 단점을 다른 센서의 장점으로 대체할 수 있도록 센서를 결합해야 한다는 뜻이다. 이렇게 해서 최종 출력에는 각 센서의 장점만 담겠다는 것이 센서 융합의 기본 전략이다. 융합이 성공하면 개별 센서를 뛰어넘는 성능이 나오는 이유가 바로 여기에 있다.

아래 그림은 자이로와 가속도계의 센서 융합 원리를 개념적으로 표현한 것이다. 가속도계로 결정한 자세가 칼만 필터의 측정값이 된다. 즉, 자이로의 자세 오차를 가속도계로 보정하도록 했다. 왜냐하면 각속도 자세는 발산해서 보정값으로 사용하기에 적합하지 않기 때문이다.

13.3.1 시스템 모델

앞 장의 칼만 필터 예제를 보면 문제가 달라도 알고리즘 자체는 바뀌지 않았다. 모델만 달라졌다. 마찬가지로 센서 융합 문제에서도 바뀌는 것은 모델이고, 칼만 필터 알고리즘은 똑같다. 그러므로 어떻게 시스템 모델을 선정하는지에 더 많은 관심을 가지고 내용을 읽어가는 것이 바람직하다. 그래야 나중에 여러분의 문제에 칼만 필터를 응용할 능력이 생긴다. 칼만 필터를 적용할 때 가장 중요한 것은 적합하고 정확한 시스템 모델을 찾아내는 일이다.

그럼 자이로와 가속도계의 센서 융합에 필요한 시스템 모델을 유도해 보겠다. 제대로 하려면 동역학에 대한 약간의 사전 지식이 필요하지만, ‘숲을 보지 못하는’ 잘못을 범하지 않도록 결과 위주로 설명하겠다.

우리가 관심 있는 물리량은 자세이다. 따라서 자세를 상태변수로 잡는 것은 지극히 자연스러운 선택이다.

$x = \begin{Bmatrix} \phi \\\\ \theta \\\\ \varphi \end{Bmatrix}$

예제에서는 수평 자세($\phi, \theta$)만 관심 있다고 했다. 따라서 요각($\varphi$)은 상태변수로 잡을 필요가 없지만, 수식을 설명하는데 필요해 그냥 나뒀다. 구현할 때는 요각에 아무 값이나 지정해도 상관없다.

이제 시스템 모델을 유도해보겠다. 앞에서 이미 자이로 각속도와 오일러각 사이의 관계식을 소개한 적이 있다.

$\begin{bmatrix} \dot{\phi} \\\\ \dot{\theta} \\\\ \dot{\varphi} \end{bmatrix} = \begin{bmatrix} 1 & \sin{\phi}\tan{\theta} & \cos{\phi}\tan{\theta} \\\\ 0 & cos{\phi} & -\sin{\phi} \\\\ 0 & \sin{\phi}/\cos{\theta} & \cos{\phi}/\cos{\theta} \end{bmatrix} \begin{bmatrix} p \\\\ q \\\\ r \end{bmatrix} \label{13.1}\tag{13.1}$

그런데 이 식을 시스템 모델로 삼기에는 심각한 문제가 있다. 시스템 모델이 되려면 다음과 같은 형태를 갖춰야 하는데, 위의 식은 이렇게 표현할 수가 없다. 행렬 안에 있는 오일러각을 밖으로 빼낼 방법이 없기 때문이다.

$x_{k+1} = Ax_k + w_k \Leftrightarrow \begin{Bmatrix} \dot{\phi} \\\\ \dot{\theta} \\\\ \dot{\varphi} \end{Bmatrix} = \begin{bmatrix} & & \\\\ & \bullet & \\\\ & & \end{bmatrix} \begin{Bmatrix} \phi \\\\ \theta \\\\ \varphi \end{Bmatrix} + w$

이럴 때는 어떻게 해야 할까? 고민할 것 없다. 칼만 필터를 쓰지 못한다. 무슨 수를 쓰더라도 모양을 맞춰야 한다. 이때 쓸 수 있는 방법이 상태변수를 다르게 잡아보는 것이다.

오일러각 대신 쿼터니언(Quaternion)을 상태변수로 잡아보겠다. 우리의 관심은 쿼터니언이 아니므로, 이에 대한 설명은 생략한다. 그냥 자세를 표현하는 방법 중의 하나라고만 언급해 두겠다.

$x = \begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix} \label{13.5}\tag{13.5}$

그런데 쿼터니언과 각속도의 관계는 이미 잘 알려져 있다.

$\begin{Bmatrix} \dot{q_1} \\\\ \dot{q_2} \\\\ \dot{q_3} \\\\ \dot{q_4} \end{Bmatrix} = {1 \over 2}\begin{bmatrix} 0 & -p & -q & -r \\\\ p & 0 & r & -q \\\\ q & -r & 0 & p \\\\ r & q & -p & 0 \end{bmatrix}\begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix} \label{13.6}\tag{13.6}$

상태변수만 바꿨을 뿐인데, 칼만 필터의 시스템 모델이 갖춰야 할 요구조건을 만족한다. 자세를 쿼터니언으로 표현하든 오일러각으로 표현하든 물리적인 자세는 똑같다. 하지만 상태변수를 어떻게 잡느냐에 따라 칼만 필터의 적용 여부는 이렇게 달라지기도 한다.

식 $\ref{13.6}$을 이산(Discrete) 시스템으로 바꾸면, 다음과 같은 시스템 모델을 얻을 수 있다.

${\begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix}}_{k+1} = \left( I + \triangle t \cdot {1 \over 2} \begin{bmatrix} 0 & -p & -q & -r \\\\ p & 0 & r & -q \\\\ q & -r & 0 & p \\\\ r & q & -p & 0 \end{bmatrix} \right) {\begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix}}_k \label{13.7}\tag{13.7}$

위의 식에서 시스템 모델의 행렬 $A$는 다음과 같이 정의된다.

$A = I + \triangle \cdot {1 \over 2} \begin{bmatrix} 0 & -p & -q & -r \\\\ p & 0 & r & -q \\\\ q & -r & 0 & p \\\\ r & q & -p & 0 \end{bmatrix} \label{13.8}\tag{13.8}$

그런데 지금까지와는 달리 행렬 $A$의 원소가 상수가 아니고, 각속도가 포함되어 있다. 각속도에 따라 행렬 $A$도 바뀌는 것이다.

이제 측정 관계식을 유도해 보겠다. 가속도로 계산한 오일러각을 측정값으로 사용한다는 사실은 이미 이야기했다. 그런데 시스템 모델의 상태변수는 쿼터니언이다. 따라서 측정값도 쿼터니언으로 바꿔서 사용해야 한다. 오일러각을 쿼터니언으로 바꾸는 공식은 다음과 같다.

$\begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix} = \begin{Bmatrix} \cos{\phi \over 2}\cos{\theta \over 2}\cos{\varphi \over 2} + \sin{\phi \over 2}\sin{\theta \over 2}\sin{\varphi \over 2} \\\\ \sin{\phi \over 2}\cos{\theta \over 2}\cos{\varphi \over 2} - \cos{\phi \over 2}\sin{\theta \over 2}\sin{\varphi \over 2} \\\\ \cos{\phi \over 2}\sin{\theta \over 2}\cos{\varphi \over 2} + \sin{\phi \over 2}\cos{\theta \over 2}\sin{\varphi \over 2} \\\\ \cos{\phi \over 2}\cos{\theta \over 2}\sin{\varphi \over 2} - \sin{\phi \over 2}\sin{\theta \over 2}\cos{\varphi \over 2} \end{Bmatrix} \label{13.9}\tag{13.9}$

가속도계로 결정한 자세를 위의 식에 대입하면 쿼터니언으로 표현된 측정값을 얻게 된다. 이 쿼터니언이 칼만 필터의 측정값($z_k$)이 된다. 이 말은 결국 모든 상태변수(즉 쿼터니언)가 측정된다는 뜻이다. 따라서 행렬 $H$는 단위행렬이 된다.

$H = \begin{bmatrix} 1 & 0 & 0 & 0 \\\\ 0 & 1 & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix} \label{13.10}\tag{13.10}$

여기까지의 과정이 너무 복잡해서 혼란스럽다면 찬찬히 다시 읽어 보기 바란다. 핵심을 요약하면 아래와 같다.

“자이로와 가속도계의 상호 보완적인 특성을 이용해, 가속도계로 자이로를 보상하는 센서 융합을 하려도 한다. 그런데 시스템 모델이 칼만 필터가 적용되지 않는 형태였다. 그래서 상태변수를 오일러각에서 쿼터니언으로 바꾸고, 측정값도 오일러각에서 쿼터니언으로 바꿨다. 그랬더니 시스템 모델이 칼만 필터를 쓸 수 있는 형태가 되었다.”

13.3.2 센서 융합 칼만 필터

시스템 모델을 모두 구했으니 이제 칼만 필터를 설계해보자. 이제는 말하기도 지겹지만, 시스템 모델이 있으면 칼만 필터의 구현은 기계적으로 따라 하면 된다.

EulerKalman.m은 칼만 필터로 구현한 센서 융합 프로그램이다. 이 함수는 시스템 행렬 $A$ 와 측정값을 인자로 받아 오일러각을 반환한다. 함수의 마지막 부분에 칼만 필터의 쿼터니언 추정값을 오일러각으로 변환하는 코드가 있다. 오일러각과 달리 쿼터니언은 물리적인 의미가 없기 때문에, 추정 결과는 오일러각으로 바꿔서 출력하는 게 더 낫기 때문이다.

잡음의 공분산 행렬 $Q$와 $R$은 시스템의 신호 특성과 관련 있는 값이라, 이론적으로 구하기는 어렵고 실제 데이터를 분석해봐야 한다. 보통은 이 행렬을 설계인자로 보고, 이런 저런 값을 넣어 성능 변화의 추이를 보면서 결정한다. 여기서는 아래와 같이 선정했다.

$Q = \begin{bmatrix} 0.0001 & 0 & 0 & 0 \\\\ 0 & 0.0001 & 0 & 0 \\\\ 0 & 0 & 0.0001 & 0 \\\\ 0 & 0 & 0 & 0.0001 \end{bmatrix}, R = \begin{bmatrix} 10 & 0 & 0 & 0 \\\\ 0 & 10 & 0 & 0 \\\\ 0 & 0 & 10 & 0 \\\\ 0 & 0 & 0 & 10 \end{bmatrix}$

상태변수와 오차 공분산 행렬의 초기값은 다음과 같이 선정했다. 이 상태변수(쿼터니언) 초기값의 물리적인 의미는 오일러각이 모두 $0$이라는 뜻이다.

$\hat{x}_0^{-} = \begin{Bmatrix} q_1 \\\\ q_2 \\\\ q_3 \\\\ q_4 \end{Bmatrix} = \begin{Bmatrix} 1 \\\\ 0 \\\\ 0 \\\\ 0 \end{Bmatrix}, P_0^{-} = \begin{bmatrix} 1 & 0 & 0 & 0 \\\\ 0 & 1 & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix}$

EulerKalman.m

function [phi theta psi] = EulerKalman(A, z)
% 오일러각을 반환하는 칼만 필터 함수
% 시스템 행렬 A와 측정값을 인자로 받아 오일러각을 반환
persistent H Q R
persistent x P
persistent firstRun


if isempty(firstRun)
  H = eye(4);

  Q = 0.0001*eye(4);
  R = 10*eye(4);

  x = [1 0 0 0]';  
  P = 1*eye(4);

  firstRun = 1;  
end


xp = A*x;
Pp = A*P*A' + Q;

K = Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);     % x = [ q1 q2 q3 q4 ]
P = Pp - K*H*Pp;

% 쿼터니언 추정값을 오일러각으로 변환!
phi   =  atan2( 2*(x(3)*x(4) + x(1)*x(2)), 1 - 2*(x(2)^2 + x(3)^2) );
theta = -asin(  2*(x(2)*x(4) - x(1)*x(3)) );
psi   =  atan2( 2*(x(2)*x(3) + x(1)*x(4)), 1 - 2*(x(3)^2 + x(4)^2) );

이제 칼만 필터의 센서 융합 성능을 알아보겠다. 아래 TestEulerKalman.m은 TestEulerKalman 함수를 테스트하기 위한 프로그램이다. 자이로와 가속도계의 측정값은 앞 절과 동일한 시험 데이터를 사용했다.

구현 코드를 보면 가장 먼저 식 $\ref{13.8}$에 따라 자이로의 측정 각속도를 가지고 시스템 행렬 $A$를 구성한다. 그 다음에 가속도계로 오일러각을 계산한다. 이 값은 EulerToQuaternion 함수를 통해 쿼터니언으로 변환되어, 칼만 필터의 측정값으로 입력된다. 참고로 EulerToQuaternion 함수를 호출할 때 요각($\varphi$)을 $0$으로 지정했는데, 어떤 값이든 상관없다.

TestEulerKalman.m

clear all

Nsamples = 41500;
EulerSaved = zeros(Nsamples, 3);

dt = 0.01;

for k=1:Nsamples
  % 각속도 읽어옴
  [p q r] = GetGyro();  
  % 측정된 각속도를 이용하여 시스템 행렬 A 구성
  A = eye(4) + dt*1/2*[ 0  -p  -q  -r;
                        p   0   r  -q;
                        q  -r   0   p;
                        r   q  -p   0
                      ];

  % 가속도 읽어옴                
  [ax ay] = GetAccel();
  % 측정된 가속도를 이용하여 오일러각 계산
  [phi theta] = EulerAccel(ax, ay);
  % 오일러각을 이용하여 쿼터니언각 계산
  z = EulerToQuaternion(phi, theta, 0);

  [phi theta psi] = EulerKalman(A, z);

  EulerSaved(k, :) = [ phi theta psi ];
end

% radian -> degree
PhiSaved   = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved   = EulerSaved(:, 3) * 180/pi;

t = 0:dt:Nsamples*dt-dt;

% 칼만 필터를 이용하여 구한 값 확인
figure
plot(t, PhiSaved)
xlabel('Time[sec]')
ylabel('Roll angle[deg]')

figure
plot(t, ThetaSaved)
xlabel('Time[sec]')
ylabel('Pitch angle[deg]')

figure
plot(t, PsiSaved)
xlabel('Time[sec]')
ylabel('Yaw angle[deg]')

EulerToQuaternion.m

function z = EulerToQuaternion(phi, theta, psi)
% 오일러각을 쿼터니언각으로 변환하는 함수
%
sinPhi   = sin(phi/2);    cosPhi   = cos(phi/2);
sinTheta = sin(theta/2);  cosTheta = cos(theta/2);
sinPsi   = sin(psi/2);    cosPsi   = cos(psi/2);

z = [ cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi;
      sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi;
      cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi;
      cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi;
    ];

센서 융합 결과를 보겠다. 다음 그래프에 롤각과 피치각의 궤적을 그렸다. $\pm30^\circ$ 의 움직임이 정확하게 측정되었다. 자이로와 가속도계의 결과와 비교해보면 놀라운 결과가 아닐 수 없다. 애초의 설계 목표대로 자이로의 누적 오차가 없어졌고, 가속도계의 부정확성도 제거되었다. 두 센서의 장점이 잘 융합된 덕분이다.

이상에서 자이로와 가속도계의 상호 보완적인 특징을 이용하여 두 센서를 융합하면 훨씬 더 좋은 성과를 거둘 수 있음을 확인했다. 특히 칼만 필터에 적합한 시스템 모델만 구하면 센서 융합 문제도 동일한 칼만 필터 알고리즘으로 해결 가능했다.

하지만 일반적인 센서 융합 문제에서 시스템 모델이 칼만 필터가 요구하는 선형 시스템 모델로 표현되는 경우는 드물다. 이 장에서 다룬 자이로와 가속도계의 융합이 오히려 특별한 경우이다. 따라서 칼만 필터를 보다 다양한 센서 융합 문제에 적용하려면 더 일반적인 방법이 필요하다. 이 문제는 뒤에서 다시 다뤄보겠다.

Kalman Filter Study _ 13장. 수평 자세 측정_1부

Tue, 28 Mar 2017 00:00:00 +0000

지금까지 살펴본 칼만 필터의 용도는 잡음 제거와 측정하지 않은 상태변수 추정 두 가지였다. 이 장에서는 저렴한 센서를 모아서 ‘청출어람’ 센서로 탈바꿈시키는 칼만 필터의 센서 융합 능력을 소개한다.

칼만 필터가 최초로 적용된 곳은 항공우주 분야였다. 발표되자마자 미국의 아폴로 달 탐사 프로그램에 적용되면서 일약 스타로 떠올랐다. 가장 많은 응용사례가 있는 곳도 항공우주 분야이다. 특히 항공기나 인공위성의 위치와 자세를 측정하는 항법(Navigation) 분야에서는 그 영향력이 절대적이라고 할 만큼 많이 사용된다. 칼만 필터 없는 항법 시스템은 상상하기 어려울 정도이다. 이 장에서 다룰 예제도 가속도계(Accelerometer)와 자이로스코프(Gyroscope)로 수평 자세를 찾아내는 간단한 항법 문제이다.

참고로 가속도계나 자이로는 관성(Inertial) 좌표계에 대한 측정값을 출력한다고 해서 관성항법 센서라고 부른다. 그리고 관성항법 센서로 위치와 자세 등을 측정하는 항법을 관성항법이라고 한다. 보통 관성항법은 자세뿐만 아니라 위치와 속도까지 다루는데, 여기서는 간단하게 자세 추정만 고려한다. 아래는 예제이다.

“헬기의 자세 측정 알고리즘을 개발해야 한다. 가속도계와 자이로를 이용해 헬기의 수평 자세를 알아내야 한다. 다시 말해 가속도와 각속도를 가지고, 수평면에 대해 헬기가 앞뒤와 좌우로 기울어진 각도를 찾아내야 한다.”

비록 헬기를 예로 들었지만, 여기서 설명하는 내용은 보행 로봇 등 자이로와 가속도계로 수평 자세를 측정하는 모든 시스템에 적용 가능하다. 문제를 더 자세히 분석해보겠다. 일반적으로 어떤 물체의 자세는 아래 그림처럼 세 개의 각도만 알면 정확히 표현할 수 있다. 참고로 이 세 개의 각을 오일러 각(Euler angle)이라고 함!

따라서 헬기의 자세를 알아낸다는 말은 이 세 각도를 찾는다는 말과 같은 뜻이다. 이 예제에서는 수평 자세에만 관심이 있으므로 세 개의 각도 중에서 롤각($\phi$)과 피치각($\theta$)만 찾으면 된다. 요각($\varphi$)은 수평 자세에서는 전혀 영향을 주지 않으므로 고려할 필요가 없다.

예제 시험 절차는 다음과 같다. 주파수 $0.2 Hz$, 최대 진폭 $\pm{30^\circ}$인 정현파로 항법센서를 흔드는 시험을 실시했다.

시험에서 저장한 데이터는 세 축 방향 가속도와 각속도이고, 저장 간격은 $0.01$ 초였다. 다음 그림은 항법센서에 가한 롤-피치-요축 정현파 진동의 궤적이다.

이 장에서 개발하는 알고리즘의 검증과 분석에는 위의 시험에서 얻은 가속도와 각속도를 공통으로 사용한다. 알고리즘의 성능이 좋은지 나쁜지는 추정 결과가 앞의 진동 궤적과 얼마나 비슷한 지로 판단하면 된다.

이어지는 절에서는 자이로와 가속도계로 수평 자세를 계산하는 방법을 소개한다. 그리고 측정 결과를 분석해서 두 센서가 왜 상호 보완이 되는지를 설명하겠다. 마지막으로 칼만 필터로 두 센서를 융합해서 수평 자세를 찾아내는 기법을 살펴보고, 융합이 성공적으로 되었는지 검토해 보겠다.

13.1 자이로를 이용한 자세 결정

자이로로 측정한 각속도를 적분해서는 오일러각을 구할 수 없다. 자이로는 오일러각의 변화율($\dot{\phi},\dot{\theta},\dot{\varphi}$)이 아니라, 헬기의 각속도($p,q,r$)를 측정한 값이기 때문이다. 따라서 자이로의 측정값을 오일러각의 변화율로 바꿔서 적분해야 한다. 동역학 분야에서 오일러각과 각속도의 관계는 이미 잘 알려져 있다. 참고로 오일러각의 변화율을 적분하여 오일러각을 구하고, 이전 오일러각과 적분한 값을 더하여 현재의 오일러각을 구할 수 있다.

이 식에 자이로의 측정 각속도($p,q,r$)를 대입해서 적분하면 현재 자세를 구할 수 있다. 물론 초기값은 알고 있어야 한다. 그런데 자이로의 측정값에는 오차가 섞여 있기 때문에 적분하면 자세에는 오차가 누적될 수 밖에 없다. 따라서 이 방법은 센서가 아주 정밀하거나 운용 시간이 짧지 않으면 실제로 사용하기는 어렵다.

아래 EulerGyro.m은 식 $\ref{13.1}$을 구현한 함수의 코드이다. 자이로에서 측정한 각속도와 샘플링 시간을 인자로 받아 오일러각을 반환한다. 적분은 가장 단순한 알고리즘을 사용했다. 실전에서는 문제의 특성에 따라 적절한 적분 방법으로 대체하면 된다.

EulerGyro.m

function [phi theta psi] = EulerGyro(p, q, r, dt)
% 자이로에서 측정한 각속도를 오일러각의 변화율로 바꾸고 적분하는 함수
% 자이로에서 측정한 각속도(p,q,r)와 샘플링 시간(dt)를 인자로 받음

persistent prevPhi prevTheta prevPsi

% 초기 오일러각은 모두 0으로 가정
if isempty(prevPhi)
    prevPhi = 0;
    prevTheta = 0;
    prevPsi = 0;
end

% 사용할 값으로 변환
sinPhi = sin(prevPhi);
cosPhi = cos(prevPhi);
cosTheta = cos(prevTheta);
tanTheta = tan(prevTheta);

% 적분(문제의 특성에 따라 적절히 변경)
phi   = prevPhi     + dt*(p + q*sinPhi*tanTheta + r*cosPhi*tanTheta);
theta = prevTheta   + dt*(    q*cosPhi          - r*sinPhi);
psi   = prevPsi     + dt*(    q*sinPhi/cosTheta + r*cosPhi/cosTheta);

prevPhi = phi;
prevTheta = theta;
prevPsi = psi;

end

아래 TestEulerGyro.m은 EulerGyro 함수를 테스트하기 위한 프로그램이다. 초기 오일러각은 모두 $0$이라고 가정했다.

TestEulerGyro.m

clear all

Nsamples = 41500;
EulerSaved = zeros(Nsamples, 3);
GetGyroSaved = zeros(Nsamples, 3);

dt = 0.01;

for k = 1:Nsamples
  [p q r] = GetGyro();   
  [phi theta psi] = EulerGyro(p, q, r, dt);

  GetGyroSaved(k, :) = [ p q r ];
  EulerSaved(k, :) = [ phi theta psi ];
end

% radian -> degree
PhiSaved   = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved   = EulerSaved(:, 3) * 180/pi;

t = 0:dt:Nsamples*dt-dt;

% 자이로센서가 측정한 각속도 확인 - 교재 그림과 맞춰주려고 40을 곱함
figure
subplot(3,1,1);
plot(t, GetGyroSaved(:,1)*40);
xlabel('Time[sec]')
ylabel('Roll angle[deg]')
subplot(3,1,2);
plot(t, GetGyroSaved(:,2)*40);
xlabel('Time[sec]')
ylabel('Pitch angle[deg]')
subplot(3,1,3);
plot(t, GetGyroSaved(:,3)*40);
xlabel('Time[sec]')
ylabel('Yaw angle[deg]')


% 오일러 각으로 변환한(각속도를 변화율로 변화 + 적분) 값 확인
figure
plot(t, PhiSaved)
xlabel('Time[sec]')
ylabel('Roll angle[deg]')

figure
plot(t, ThetaSaved)
xlabel('Time[sec]')
ylabel('Pitch angle[deg]')

% Yaw각은 수평자세에는 전혀 영향을 주지 않으므로 고려할 필요가 없다.(지금 예제)
figure
plot(t, PsiSaved)
xlabel('Time[sec]')
ylabel('Yaw angle[deg]')

아래 GetGyro 함수는 자이로에서 측정값을 읽어온다. 여기서는 미리 ArsGyro.mat 파일에 저장된 값을 반환하도록 했다.

GetGyro.m

function [p q r] = GetGyro()
% 자이로에서 측정값을 읽어오는 함수
% 미리 저장된 ArsGyro.mat에서 값 읽어옴
persistent wx wy wz
persistent k firstRun


if isempty(firstRun)
  load ArsGyro
  k = 1;

  firstRun = 1;
end

p = wx(k);
q = wy(k);
r = wz(k);

k = k + 1;

다음 그래프는 앞 절의 시험에서 자이로가 실제로 측정한 각속도이다. 실제 기동의 경향이 잘 나타나 있다. 실험에 쓰인 항법 센서는 잡음 제거 필터가 구현된 제품이라, 저가의 자이로보다 출력이 더 깨끗한 편이다.

그럼 TestEulerGyro 프로그램의 수행 결과를 살펴보겠다. 먼저 롤각($\phi$)을 보겠다. $50 \sim 130$ 초 사이에 $\pm{30\circ}$ 의 진폭으로 진동하는 움직임이 보인다. $180 \sim 250$ 초 사이에도 비슷한 움직임이 다시 나타난다. 외부에서 가한 진동이 잘 반영된 결과이다. 그런데 시간이 지나면서 오차가 조금씩 생기는게 보인다. 기동의 경향은 어느 정도 맞지만, 자세각은 많이 편향되어 있다. 오차가 누적된 탓이다.

이제 피치각($\theta$)의 변화를 살펴보겠다. $0 \sin 180$ 초 사이에는 피치축 기동이 없는데도 측정값이 점점 커져간다. 롤축 기동의 영향을 받아 오차가 계속 쌓여가기 때문이다. 이렇게 누적된 측정 오차가 상당하다. 그렇다고 적분한 자세각이 아무런 쓸모도 없는 것은 아니다. 오차 누적으로 값은 편향되어 가지만, 기동 패턴은 잘 감지하고 있기 때문이다.

요약하면 각속도를 적분하여 얻은 자세는 동적 움직임은 잘 포착해내지만, 점차 오차가 누적되어 실제값에서 멀어지는 특성이 있다. 따라서 자이로는 자세각 측정보다는 자세각의 동태를 측정하는데 사용하는게 더 낫다. 이러한 특징을 시간의 관점에서 보면, 자이로는 단기간의 측정은 비교적 정확하지만 장시간의 변화에는 부정확한 센서라고 볼 수 있다.

13.2 가속도를 이용한 자세 결정

가속도계로 측정한 가속도($f_x,f_y,f_z$)에는 중력 가속도와 속도의 크기나 방향이 바뀔 때 생기는 가속도 등 다양한 종류의 가속도가 포함되어 있다. 이런 특성을 수식으로 표현하면 다음과 같다.

$\begin{bmatrix} f_x \\\\ f_y \\\\ f_z \end{bmatrix} = \begin{bmatrix} \dot{u} \\\\ \dot{v} \\\\ \dot{w} \end{bmatrix} + \begin{bmatrix} 0 & w & -v \\\\ -w & 0 & u \\\\ v & -u & 0 \end{bmatrix} \begin{bmatrix} p \\\\ q \\\\ r \end{bmatrix} + g \begin{bmatrix} \sin{\theta} \\\\ -\cos{\theta}\sin{\phi} \\\\ -\cos{\theta}\cos{\phi}\end{bmatrix} \label{13.2}\tag{13.2}$

여기서 $u,v,w$는 이동 속도를 의미하고, $p,q,r$은 회전 각속도를 뜻한다. 그리고 $g$는 중력 가속도를 나타낸다.

식 $\ref{13.2}$의 우변에서 마지막 항을 보면 롤각($\phi$)과 피치각($\theta$)이 나온다. 따라서 나머지 항의 값을 모두 알면, 이 식으로 수평 자세를 계산할 수 있다. 그럼 하나씩 따져보겠다. 가속도($f_x,f_y,f_z$)와 각속도($p,q,r$)는 측정값이라 알고 있다. 중력 가속도($g$)도 이미 알고 있는 값이다. 이제 남은 것은 이동 속도($u,v,w$)와 이동 가속도($\dot{u},\dot{v},\dot{w}$)뿐이다. 그런데 이 두 값은 아주 고가의 항법센서가 아니면 측정할 수 없다. 따라서 평범한 항법센서로는 식 $\ref{13.2}$를 이용해 자세를 계산할 방법이 없다.

그렇다고 아주 불가능한 것은 아니다. 시스템이 정지해 있거나 일정한 속도로 직진하는 조건이라면 가능하다. 먼저 시스템이 정지해 있을 때를 생각해보겠다. 움직이지 않으면 이동 속도와 이동 가속도 모두 $0$이 된다.

$\dot{u} = \dot{v} = \dot{w} = 0 \\\\ u = v = w = 0$

일정한 속도로 직진하는 경우를 보겠다. 일단 이동 가속도($\dot{u}, \dot{v}, \dot{w}$)는 $0$ 이다. 그리고 자세 변화가 없으니 각속도($p,q,r$)도 $0$이 된다.

$\dot{u} = \dot{v} = \dot{w} = 0 \\\\ p = q = r = 0$

두 경우 모두 식 $\ref{13.2}$의 우변에 있는 첫 번째 항과 두 번째 항이 $0$이 된다. 그러면 식 $\ref{13.2}$는 다음과 같이 간단한 형태가 된다.

$\begin{bmatrix} f_x \\\\ f_y \\\\ f_z \end{bmatrix} = g \begin{bmatrix} \sin{\theta} \\\\ -\cos{\theta}\sin{\phi} \\\\ -\cos{\theta}\cos{\phi}\end{bmatrix}$

이 식에서 다음과 같은 롤각과 피치각의 공식을 유도해낼 수 있다.

$\phi = \sin^{-1}{\left(-f_y \over g\cos\theta \right)} \\\\ \theta = \sin^{-1}{\left( f_x \over g \right)} \label{13.4}\tag{13.4}$

움직이는 속도가 충분히 느리거나 속도의 크기와 방향이 빠르게 변하지 않는 경우에도 식 $\ref{13.4}$로 수평자세를 구할 수 있다. 이 경우에 식 $\ref{13.4}$는 근사식이 된다. 보통 헬기는 대부분의 임무를 제자리 비행이나 일정 속도로 나는 도중에 수행하므로, 이런 가정이 잘 맞는다. 보행 로봇도 대부분 비슷한 조건에서 움직이므로 식 $\ref{13.4}$를 수평 자세에 이용해도 무방하다.

식 $\ref{13.4}$로 수평 자세를 구할 때는 이 식이 근사식이라는 사실을 꼭 기억해야 한다. 빠른 속도로 회전하거나 속도 변화가 심하면 오차가 커질 수 있다.

아래 EulerAccel.m은 식 $\ref{13.4}$를 구현한 것이다. 가속도를 입력 받아 롤각과 피치각을 반환한다.

EulerAccel.m

function [ phi theta ] = EulerAccel(ax, ay)
% 가속도를 입력 받아 롤각과 피치각(오일러각)을 반환하는 함수
%

% 중력 가속도
g = 9.8;

theta = asin(  ax/ g );
phi   = asin( -ay/ (g*cos(theta)) );

end

다음 그래프는 앞절의 시험에서 측정한 가속도이다. 그림을 보면 롤축 기동을 할 때 피치축 가속도가 측정되고, 피치축이 움직일 때 롤축 가속도가 나타난다. 이 현상의 이유는 이렇다. 롤축 기동은 롤축을 회전축으로 하는 주기적인 정현파 진동이므로, 피치축이 수평면에 대해 올라가거나 내려간다. 그러면 중력가속도와 원심력 성분 등이 피치축 가속도계에 측정된다. 피치축 기동일 때도 마찬가지이다. 따라서 아래와 같은 결과는 물리 법칙에 위배되지 않는, 타당한 결과이다.

TestEulerAccel.m은 EulerAccel 함수를 테스트하기 위한 프로그램이다.

TestEulerAccel.m

clear all

Nsamples = 41500;
EulerSaved = zeros(Nsamples, 2);
GetAccelSaved = zeros(Nsamples, 3);

for k = 1:Nsamples
  [ax ay az] = GetAccel();   
  [phi theta] = EulerAccel(ax, ay);

  GetAccelSaved(k, :) = [ax ay az];
  EulerSaved(k, :) = [phi theta];
end

% radian -> degree
PhiSaved   = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;

dt = 0.01;
t  = 0:dt:Nsamples*dt-dt;

% 가속도센서가 측정한 가속도 확인
figure
subplot(3,1,1);
plot(t, GetAccelSaved(:, 1));
xlabel('Time[sec]')
ylabel('f_x[m/s^2]')
subplot(3,1,2);
plot(t, GetAccelSaved(:, 2));
xlabel('Time[sec]')
ylabel('f_y[m/s^2]')
subplot(3,1,3);
plot(t, GetAccelSaved(:, 3));
xlabel('Time[sec]')
ylabel('f_z[m/s^2]')

% 변환한 롤각과 피치각 확인
figure
plot(t, PhiSaved)
xlabel('Time[sec]')
ylabel('Roll angle[deg]')

figure
plot(t, ThetaSaved)
xlabel('Time[sec]')
ylabel('Pitch angle[deg]')

GetAccel 함수는 가속도를 측정해서 읽어온다. 미리 3축 방향 가속도가 저장된 파일(ArsAccel.mat)을 읽어 들인 다음, 차례로 데이터를 반환한다.

GetAccel.m

function [ax ay az] = GetAccel()
% 가속도를 측정해서 읽어오는 함수
% 미리 저장된 ArsAccel.mat에서 값 읽어옴
persistent fx fy fz
persistent k firstRun


if isempty(firstRun)
  load ArsAccel
  k = 1;

  firstRun = 1;
end

ax = fx(k);
ay = fy(k);
az = fz(k);

k = k + 1;

가속도로 계산한 수평 자세를 살펴보겠다. 먼저 롤각 그래프를 보면 정현파 기동의 추이가 잘 나타난다. 그리고 자이로와 달리 기동이 끝나면 다시 $0$도로 정확히 돌아온다. 오차가 누적되지 않는다. 그런데 최대값은 $\pm9^\circ$ 정도로 실제 값인 $\pm30^\circ$에 미치지 못한다. 오차가 상당히 커서 가속도계 단독으로 사용하기는 힘든 상태이다.

이제 피치각의 그래프를 보겠다. 피치각의 그래프에서도 같은 양상이 보인다. 시간이 지나도 누적 오차가 없고 피치각이 편향되지도 않는다. 하지만 롤각과 마찬가지로 오차가 너무 크다.

정리하면 가속도계는 오차를 발산하지 않고 일정한 범위 안에 머무르는 장점이 있다. 자세를 구하는 과정에서 적분을 하지 않기 때문이다. 하지만 안정성이 높은 대신 정밀도는 떨어진다. 왜냐면 식 $\ref{13.4}$의 자세 계산식이 가속도나 각속도가 충분히 작은 상황에서만 쓸 수 있는 근사식이기 때문이다.

Kalman Filter Study _ 12장. 영상 속의 물체 추적

Fri, 24 Mar 2017 00:00:00 +0000

이 장에서는 칼만 필터로 2차원 평면 위의 물체를 추적하는 방법에 대해 알아본다. 이 문제는 2차원 영상에서 특정 표적을 감시하고자 할 때 자주 등장한다. 예를 들어 레이더 영상에 나타난 표적을 추적한다거나, 카메라로 특정 대상을 실시간으로 감시하는 시스템 등을 개발할 때 필요한 기술이다.

먼저 분명히 해두어야 할 사항이 있다. 표적의 위치는 영상처리 알고리즘으로 알아내야 한다. 칼만 필터는영상에서 물체의 위치를 찾아내는 능력이 없다. 칼만 필터는 영상처리 기법으로 찾아낸 물체의 위치를 입력 받아 정확한 위치를 추정하는 역할을 한다. 앞에서 살펴 본 칼만 필터 예제를 상기해 보자. 전압이나 거리의 측정은 칼만 필터의 역할이 아니고 센서가 하는 일이였다. 영상 속의 표적을 추적하는 문제도 마찬가지다. 표적의 위치는 적절한 영상처리 기법을 동원해서 찾아내야 한다. 다시 한번 강조하지만 영상에서 표적의 위치를 찾아내는 일은 칼만 필터의 담당이 아니다.

그렇다면 칼만 필터의 역할은 무엇일까? 이 질문에 대한 답은 앞 장의 예제에 다 나와 있다. 그동안 칼만 필터가 어떤 용도로 쓰였는지를 살펴 보면 된다는 뜻이다. 지금까지 칼만 필터는 잡음을 제거하거나 측정하지 않은 값을 추정해내는 등의 역할을 했다. 표적 추적 문제에서도 칼만 필터의 역할은 동일하다. 영상처리 알고리즘으로 얻은 위치의 오차를 제거하고 이동 속도 등을 추정하는데 활용된다.

12.1 시스템 모델

평면 상의 표적 추적 문제에 칼만 필터를 적용하기 위해서는 앞 장에서 소개한 위치-속도 모델을 2차원으로 확장해야 한다. 즉 상태 변수를 가로 방향($x축$) 위치-속도와 세로 방향($y축$) 위치-속도로 잡고 시스템 모델을 유도한다.

$x = \begin{Bmatrix} 위치_x \\\\ 속도_x \\\\ 위치_y \\\\ 속도_y \end{Bmatrix}$

여기서 상태 변수의 순서는 중요하지 않다. 예를 들어 위치 변수 두 개를 먼저 쓰고, 속도는 속도끼리 모아도 된다. 어쨌든 상태 변수를 위와 같이 정의하면 시스템 모델은 다음과 같이 주어진다.

$x_{k+1} = Ax_k + W_k \\\\ z_k = Hx_k + v_k \label{12.1}\tag{12.1}$ $A = \begin{bmatrix} 1 & \triangle t & 0 & 0 \\\\ 0 & 1 & 0 & 0 \\\\ 0 & 0 & 1 & \triangle t \\\\ 0 & 0 & 0 & 1 \end{bmatrix} , \; H = \begin{bmatrix} 1 & 0 & 0 & 0 \\\\ 0 & 0 & 1 & 0 \end{bmatrix} \label{12.2}\tag{12.2}$

행렬 $A$를 식 $\ref{12.1}$에 대입해서 정리해보면, $x$ 축과 $y$ 축 방향으로 각각 다음과 같은 관계를 행렬식으로 표현하고 있음을 알 수 있다.

$\begin{Bmatrix} 위치_{k+1} \\\\ 속도_{k+1}\end{Bmatrix} = \begin{Bmatrix} 위치_{k} + 속도_{k} \cdot \triangle t \\\\ 속도_{k} \end{Bmatrix} + w_k$

이 두 식이 갖는 의미는 11장에서 이미 설명했다. 그리고 행렬 $H$를 식 $\ref{12.1}$의 측정값 관계식에 대입해서 정리해보면, $x$ 축과 $y$ 축 방향의 위치만 측정하고 속도는 측정하지 않는다는 의미가 담겨 있다.

지금까지의 내용을 정리해 보겠다. 평면 상의 물체를 추적하는 칼만 필터의 시스템 모델은 식 $\ref{12.1} , \ref{12.2}$와 같이 주어진다. 이 모델은 위치-속도 사이의 관계를 2차원 평면으로 확장한 것이다. 그리고 대상 표적의 위치만 영상처리 알고리즘으로 찾아서 칼만 필터에 입력한다.

시스템 모델을 보고 이미 짐작하셨겠지만, 이 장의 내용은 2차원으로 확장한 것 말고는 11장과 별다른 차이가 없다. 즉 11장에서 소개한 칼만 필터를 2차원 문제에 확장해서 적용해 보는 정도에 불과하다. 그렇지만 실용적인 응용 사례를 구체적으로 다뤄본다는 측면에서는 나름 의미가 있다. 더구나 영상처리 기술이 다양한 분야에서 점점 더 많이 활용되는 추세이니 칼만 필터와 영상처리 알고리즘을 함께 활용하는 방법을 알아두면 도움이 될 것이다.

12.2 칼만 필터 함수

아래 TrackKalman 함수는 영상처리 알고리즘이 찾아낸 위치($xm, ym$)를 인자로 받아, 추정 위치($xh, yh$)를 반환한다. 코드의 마지막 줄에 상태변수 추정값에서 위치만 따로 뽑아서 반환하는 코드가 있다. 만약 추정 속도도 필요하다면 변수 $x$의 두 번째와 네 번째 원소를 함께 반환하면 된다.

TrackKalman.m

function [xh yh] = TrackKalman(xm, ym)
% 위치를 인자로 받아 추정 위치를 반환하는 함수
% 영상처리 알고리즘이 찾아낸 위치(xm,ym)를 인자로 받아, 추정 위치(xh, yh)를 반환함
persistent A H Q R
persistent x P
persistent firstRun


if isempty(firstRun)
  dt = 1;

  A = [ 1  dt  0   0
        0  1   0   0
        0  0   1  dt
        0  0   0   1 ];

  H = [ 1  0  0  0
        0  0  1  0 ];

  Q = 1.0*eye(4);
  R = [ 50  0
         0 50 ];

  x = [0, 0, 0, 0]';
  P = 100*eye(4);

  firstRun = 1;
end


xp = A*x;
Pp = A*P*A' + Q;

K = Pp*H'*inv(H*Pp*H' + R);

z = [xm ym]';
x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;

% 추정 위치 가져옴
xh = x(1);
yh = x(3);

12.3 테스트 프로그램

이제 TrackKalman 함수가 제대로 작동하는지 확인해 보자. 아래 그림과 같이 공이 오른쪽 상단에서 왼쪽 하단까지 대각선 방향으로 이동하는 상황을 고려한다. 공의 이동 속도는 일정하고, 공의 이동 영상은 $1$초에 한 번씩 촬영된다고 가정한다.

아래 TestTrackKalman.m은 테스트 프로그램이다. 촬영 영상에서 공의 위치를 찾아내는 알고리즘은 GetBallPos 함수에 구현되어 있다. 테스트 프로그램은 GetBallPos 함수로 공의 위치를 측정해서 칼만 필터에 입력으로 주고, 칼만 필터에서 추정 위치를 전달 받는다.

TestTrackKalman.m

clear all

NoOfImg = 24;

Xmsaved = zeros(2, NoOfImg);
Xhsaved = zeros(2, NoOfImg);

% 이미지 보여주고 측정 위치, 예측 위치 띄워 줌
for k = 1:NoOfImg
  [xm, ym] = GetBallPos(k);
  [xh, yh] = TrackKalman(xm, ym);

  hold on
  plot(xm, ym, 'r*')
  plot(xh, yh, 'bs')
  xlabel('Horizonal[pixel]')
  ylabel('Vertical[pixel]')

  pause(1)

  Xmsaved(:, k) = [xm ym]';
  Xhsaved(:, k) = [xh yh]';

end


figure
hold on
plot(Xmsaved(1,:), Xmsaved(2,:), '*')
plot(Xhsaved(1,:), Xhsaved(2,:), 's')
xlabel('Horizonal[pixel]')
ylabel('Vertical[pixel]')
legend('Measured','Kalman Filter','location','NorthEast')

위 프로그램에서 for 문 내부에 있는 plot 함수는 촬영 이미지 위에 측정 위치와 추정 위치를 표시한다. GetBallPos 함수로 측정한 위치는 * 모양으로, 칼만 필터가 추정한 위치는 사각형(□)으로 표시한다. 테스트 프로그램을 실행하면 측정 위치와 추정 위치가 표시된 촬영 이미지가 $1$초마다 새로 그려진다.

아래 GetBallPos 함수는 영상에서 공의 위치를 찾아낸다. 모든 영상 이미지는 img 라는 서브 폴더에 있다고 가정했다. 영상에서 물체를 찾아내는 알고리즘은 매우 다양한데, 여기서는 배경이미지(imgBg)와 촬영 이미지(imgWork)의 차이를 이용하는 가장 간단한 알고리즘을 사용했다. 영상처리 알고리즘에 대한 자세한 설명은 MATLAB의 Image Processing Toolbox 문서를 참고하면 된다.

그리고 함수의 마지막 두 줄은 공의 중심 좌표를 반환하는 코드인데, 임의로 평균 $15$ 픽셀 정도의 잡음을 섞었다.

GetBallPos.m

function [xc, yc] = GetBallPos(index)
% 영상에서 공의 위치를 찾아내는 함수
% 배경 이미지와 촬영 이미지의 차이를 이용하여 구함
persistent imgBg
persistent firstRun

if isempty(firstRun)
  imgBg = imread('Img/bg.jpg');

  firstRun = 1;
end

xc = 0;
yc = 0;

imgWork = imread(['Img/', int2str(index), '.jpg']);
imshow(imgWork)

fore = imabsdiff(imgWork, imgBg);
fore = (fore(:,:,1) > 10) | (fore(:,:,2) > 10) | (fore(:,:,3) > 10);

L     = logical(fore);
stats = regionprops(L, 'area', 'centroid');
area_vector = [stats.Area];
[tmp, idx] = max(area_vector);

centroid = stats(idx(1)).Centroid;

% 임의로 평균 15 픽셀 정도의 잡음을 섞음
xc = centroid(1) + 15*randn;
yc = centroid(2) + 15*randn;

아래는 수행 결과 그림이다. 그림에서 * 모양으로 표시된 지점은 GetBallPos 함수의 측정값이고, 네모 모양은 칼만 필터의 추정 위치를 의미한다. 그리고 수행과정은 오른쪽 하단에서 시작되서 왼쪽 상단으로 진행된다.

이제 테스트 프로그램의 수행 결과를 검토해보자. 칼만 필터의 추정 위치가 대각선에 더 가깝게 배치되어 있다. 즉, 칼만 필터가 측정 잡음을 효과적으로 제거해서 더 참 값에 가까운 위치를 출력한다. 그런데 그림에서 공이 오른쪽 하단에 있을 때 한 점이 대각선에서 많이 벗어나 있는데, 그 이유는 초기 위치 오차 때문이다. TrackKalman 함수를 보면 초기 위치가 $(0,0)$으로 지정되어 있다. 즉 초기에 공이 왼쪽 상단에 있다고 가정하고 있다. 테스트 프로그램이 막 시작할 때는 칼만 필터가 충분히 수렴하기 전이라 오차가 크게 나타난 것이다.

12.4 테스트 프로그램 2

이 절에서는 행렬 $Q$와 $R$을 조율해서 칼만 필터의 성능을 개선하는 방법을 소개한다. 앞 절의 예제에서 $Q$와 $R$을 바쒀가면서 추정 성능이 어떻게 달라지는지를 살펴보겠다.

먼저 8.3 절의 내용을 회고해 보겠다. 행렬 $Q$와 $R$은 칼만 필터의 성능에 정반대의 영향을 준다. 행렬 $Q$의 크기를 키우면 칼만 이득이 커지게 되고 , 결과적으로 측정값의 반영 비율이 더 높아진다. 즉 추정 결과가 측정값에 더 가까워진다. 반대로 행렬 $Q$의 크기를 줄이면 추정 결과는 측정값의 영향을 덜 받아 직전 추정값과 비슷해진다. 즉 측정값에 포함된 잡음의 영향이 줄어 추정값이 더 매끄러운 궤적을 보인다.

반면 행렬 $R$은 $Q$와 정반대이다. 추정 결과에 측정값을 많이 반영하려면 행렬 $R$의 크기를 줄여야한다. 반대로 측정값의 영향을 줄이고 더 부드러운 궤적의 추정 결과를 원하면 행렬 $R$의 크기를 키워야 한다.

행렬 $Q$와 $R$이 추정값에 미치는 영향을 알아보기 위해 TrackKalmanQR 이라는 함수를 추가했다. 테스트 프로그램 TestTrackKalmanQR.m에서는 이 함수와 TrackKalman 함수의 추정 결과를 비교한다.

TrackKalmanQR.m

function [xh yh] = TrackKalmanQR(xm, ym)
% 위치를 인자로 받아 추정 위치를 반환하는 함수
% TrackKalman과 동일하나 Q와 R을 바꿔가며 실험
persistent A H Q R
persistent x P
persistent firstRun


if isempty(firstRun)
  dt = 1;

  A = [ 1  dt  0   0
        0  1   0   0
        0  0   1  dt
        0  0   0   1 ];

  H = [ 1  0  0  0
        0  0  1  0 ];

  % Q와 R을 변경하면서 실험 진행
  Q = 0.1*1.0*eye(4);
  R = 0.1*[ 50  0
         0 50 ];

  x = [0, 0, 0, 0]';
  P = 100*eye(4);

  firstRun = 1;
end


xp = A*x;
Pp = A*P*A' + Q;

K = Pp*H'*inv(H*Pp*H' + R);

z = [xm ym]';
x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;


xh = x(1);
yh = x(3);

TestTrackKalmanQR.m

clear all

NoOfImg = 24;

Xmsaved  = zeros(2, NoOfImg);
Xhsaved  = zeros(2, NoOfImg);
Xqrsaved = zeros(2, NoOfImg);

for k = 1:NoOfImg
  [xm,   ym] = GetBallPos(k);
  [xh,   yh] = TrackKalman(xm, ym);
  [xqr, yqr] = TrackKalmanQR(xm, ym);

  hold on
  plot(xm,  ym,  'r*')
  plot(xh,  yh,  'bs')
  plot(xqr, yqr, 'bo')

  pause(1)

  Xmsaved(:, k)  = [xm   ym]';
  Xhsaved(:, k)  = [xh   yh]';
  Xqrsaved(:, k) = [xqr, yqr];
end

x = [0 350];
y = [250 0];

figure
hold on
%plot(Xmsaved(1,:),  Xmsaved(2,:),  '*')
plot(Xhsaved(1,:),  Xhsaved(2,:),  's')
plot(Xqrsaved(1,:), Xqrsaved(2,:), 'o')
plot(x,y) % 비교 선
xlabel('Horizonal[pixel]')
ylabel('Vertical[pixel]')
legend('Q and R','1/10 Q and 1/10 R','location','NorthEast')

아래 그래프에 행렬 $Q$의 크기를 줄였을 때의 추정 결과를 비교했다. 그림을 보면 행렬 $Q$가 더 작은 칼만 필터의 추정값은 잘 움직이지 않는다는 사실을 알 수 있다. 앞에서 설명한 것처럼 측정값보다는 직전 추정값이 더 많이 반영되기 때문이다. 그리고 행렬 $Q$의 크기를 $1/100$로 줄였을 때 보다 $1/1000$으로 줄였을 때 시간 지연이 커진것을 확인할 수 있다.

아래 그래프에 행렬 $R$의 크기를 줄였을 때의 추정 결과를 비교했다. 그림을 보면 행렬 $R$의 크기를 줄였을 때 추정값들이 대각선에서 많이 벗어났다. 직전 추정값보다는 측정값이 더 많이 반영되어 시간 지연은 줄었으나 잡음이 잘 제거되지 않았다.

아래 그래프에 행렬 $Q$와 $R$의 크기를 동시에 줄였을 때의 추정 결과를 비교했다. 그림을 보면 크기를 $1/100$로 줄였을 때를 보자. 반 정도 진행된 이후, 두 함수가 같은 추정값을 내놓았다. 두 행렬의 크기를 $1/10$로 줄였을 때는 반 정도 진행된 이전부터 두 함수가 같은 추정값을 내놓았다.

Kalman Filter Study _ 11장. 위치로 속도 재기_2부

Fri, 17 Mar 2017 00:00:00 +0000

11.4 속도로 위치 측정하기

이 절에서는 앞의 예제를 반대로 뒤집어 보겠다. 속도를 측정해서 위치를 추정해 보는 것이다. 우선 측정값이 위치에서 속도로 변경되었으니 시스템 모델에서 행렬 $H$가 달라져야 한다.

$H = \begin{bmatrix} 0 & 1 \end{bmatrix}$

이 값을 시스템 모델에 대입해서 정리해서 보면, 행렬 $H$가 왜 이렇게 바뀌는지 이해가 된다.

$\begin{align} z_k &= Hx_k + v_k \\\\ &= \begin{bmatrix} 0 & 1 \end{bmatrix}\begin{bmatrix} 위치 \\\\ 속도 \end{bmatrix}_{k} + v_k \\\\ &= 속도_{k} + v_k \end{align}$

측정값이 속도라는 시스템 변경사항이 제대로 반영되었다. 그 밖에 시스템 모델에서 바뀌는 사항은 없다. 다음은 변경된 모델에 대한 칼만 필터 함수이다. 행렬 $H$를 초기화하는 코드를 제외하고는 똑같다.

IntKalman.m

function [ pos vel ] = IntKalman(z)
% 속도로 위치 측정하는 함수
%

persistent A H Q R
persistent x P
persistent firstRun


if isempty(firstRun)
  firstRun = 1;

  dt = 0.1;

  A = [ 1 dt;
        0 1  ];
  H = [0 1];

  Q = [ 1 0;
        0 3 ];
  R = 10;

  x = [ 0 20 ]';
  P = 5*eye(2);
end


xp = A*x;  
Pp = A*P*A' + Q;    

K = Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;   

pos = x(1);
vel = x(2);

end

테스트 프로그램도 앞의 예제와 거의 비슷하다. GetVel 함수로 측정 속도를 읽어오고, 새로운 칼만 필터를 호출하는 부분만 바뀌었다. 거리 대신 속도를 반환하는 점만 다를 뿐, GetVel 함수에서 속도와 거리를 생성해내는 방법도 동일하다.

TestIntKalman.m

clear all


dt = 0.1;
t  = 0:dt:10;

Nsamples = length(t);

Xsaved = zeros(Nsamples, 2);
Zsaved = zeros(Nsamples, 1);

for k=1:Nsamples
  z = GetVel();      
  [pos vel] = IntKalman(z);

  Xsaved(k, :) = [pos vel];
  Zsaved(k)    = z;
end


figure
plot(t, Xsaved(:, 1))
xlabel('Time[sec]')
ylabel('Position[m]')

figure
hold on
plot(t, Zsaved(:), 'r.')
plot(t, Xsaved(:, 2))
xlabel('Time[sec]')
ylabel('Velocity[m/s]')
legend('Measurement','Kalman Filter','location','NorthWest')

GetVel.m

function z = GetVel()
%
%
persistent Velp Posp


if isempty(Posp)
  Posp = 0;
  Velp = 80;
end

dt = 0.1;

v  = 0 +  10*randn;

Posp = Posp + Velp*dt;     % true position
Velp = 80 + v;             % true speed

z = Velp;

그럼 추정 결과를 보겠다. 아래 그래프에 측정 속도와 추정 속도를 비교했다. 실선이 추정 속도를 나타내고, 점은 속도 측정값을 의미한다. 측정 잡음이 많이 제거되었고, 기준 속도인 $80m/s$에 근처 추정 속도가 모여 있다. 만족할 만한 결과이다.

다음 그림은 추정 위치를 그린 그래프이다. 위치는 측정하지 않았지만 상당히 괜찮은 추정 결과를 얻었다. 평균 속도가 $80m/s$이니까 $10$초에 $800m$ 정도를 가는 게 정상이다. 그래프를 보면 $10$초 후에 $800m$ 정도 이동한 게 보인다.

칼만 필터는 이렇게 시스템 모델에 따라 용도가 달라진다.

11.5 초음파 거리계로 속도 재기

앞의 예제에서는 속도가 일정했다. 그 덕분에 칼만 필터가 속도를 추정해 낼 수 있었던 것 아니냐고 의심할 수 있다. 그래서 이 절에서는 속도가 일정하지 않은 경우에도 위치로 속도를 추정해 낼 수 있는지 알아보겠다. 예제로는 이전에 다뤘던 초음파 거리계를 다시 사용한다. 이 예제는 거리의 변화 즉 속도가 일정하지 않다.

먼저 칼만 필터의 시스템 모델을 선정해야 하는데, 앞의 예제와 동일한 모델을 사용한다. 시스템의 기동은 많이 다르지만, 시스템 모델은 같다. 거리와 속도의 물리적인 관계는 변하지 않는 진리이기 때문이다.

칼만 필터 함수도 앞의 DvKalman 함수를 그대로 사용한다.

TestDvKalman2.m

clear all

Nsamples = 500;

Xsaved = zeros(Nsamples, 2);
Zsaved = zeros(Nsamples, 1);

for k=1:Nsamples
  z = GetSonar();      
  [pos vel] = DvKalman(z);

  Xsaved(k, :) = [pos vel];
  Zsaved(k)    = z;
end


dt = 0.02;
t  = 0:dt:Nsamples*dt-dt;

figure
hold on
plot(t, Zsaved(:), 'r.')
plot(t, Xsaved(:, 1))
xlabel('Time[sec]')
ylabel('Distance[m]')
legend('Measurement','Kalman Filter','location','NorthWest')

figure
plot(t, Xsaved(:, 2))
hold on
plot(t, (Xsaved(:, 1)-40)/2,'r:')
xlabel('Time[sec]')
ylabel('Velocity[m/s]')
legend('Velocity','Distance','location','NorthWest')

GetSonar.m

function h = GetSonar()
%
%
persistent sonarAlt       % SonartAlt.mat
persistent k firstRun


if isempty(firstRun)
  load SonarAlt
  k = 1;

  firstRun = 1;
end

h = sonarAlt(k);

k = k + 1;

다음 그림은 거리의 궤적을 비교한 그래프이다. 칼만 필터의 추정값은 실선으로 그렸고, 측정 거리는 점으로 표시했다. 측정 잡음이 많이 제거 되었고, 거리의 변화 추이도 잘 드러나 있다. 여러 차례 설명했듯이 잡음 제거는 칼만 필터의 기본 기능이다. 그런데 그래프를 자세히 보면 시간 지연이 거의 보이지 않는다. 숙원이던 잡음 제거와 변화 민감성이라는 두 가지 목표를 달성해낸 것이다.

다음 그래프는 칼만 필터가 추정한 속도를 그렸다. 그래프에서 실선은 추정 속도의 궤적이고, 점선은 추정 거리를 나타낸다. 추정 속도와 같은 그래프에 그리기 위해 추정 거리에서 $40$ 미터를 빼고 크기를 반으로 줄였다. 추정 거리의 변화율과 추정 속도를 비교해 보면 추정 속도가 나름 타당하다는 것을 알 수 있다. 거리의 변화가 별로 없는 구간에서는 속도가 $0$에 가깝고, 변화가 심한 구간에서는 속도 값이 크게 나타난다. 거리의 변화율이 속도에 비례한다는 사실을 염두에 두고 찬찬히 비교해 보면 된다.

11.6 효율적인 칼만 필터 함수

이 절에서는 간단한 팁을 하나 소개하겠다. 5장 그림의 칼만 필터 알고리즘을 보면 칼만 이득을 계산할 때 역행렬을 한 번 계산해야 한다. 역행렬은 보통 수치해석 기법으로 구하는데, 계산시간이나 안정성을 고려하면 되도록 피하는 게 좋다. 특히 빠르게 실행되는 실시간 시스템에 칼만 필터를 적용할 때는 어떻게든 계산시간을 단축하는 것이 바람직하다. 이 장의 예제와 같이 시스템 모델의 행렬이 작으면 행렬 계산식을 풀어 쓰는 것도 계산시간과 오차를 줄이는 데 도움이 된다. 그럼 칼만 이득을 계산하는 행렬식을 풀어 써보겠다.

$K_k = P_k^{-}H^{T}(HP_k^{-}H^{T} + R)^{-1}$

먼저 반복해서 등장하는 $P_k^{-}H^{T}$ 계산부터 시작한다. 행렬은 앞 절의 시스템 모델을 그대로 사용한다.

$\begin{align} P_k^{-}H^{T} &= \begin{bmatrix} P_{11}^{-} & P_{12}^{-} \\\\ P_{21}^{-} & P_{22}^{-} \end{bmatrix}\begin{bmatrix} 1 \\\\ 0 \end{bmatrix} \\\\ &= \begin{bmatrix} P_{11}^{-} \\\\ P_{21}^{-} \end{bmatrix} \end{align}$

이제 이 식을 활용해서 역행렬을 계산하는 행렬식 전체를 정리해 보겠다.

$\begin{align} HP_k^{-}H^{T} + R &= \begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} P_{11}^{-} \\\\ P_{21}^{-} \end{bmatrix} + R \\\\ &= P_{11}^{-} + R \end{align}$

상당히 간단해졌다. 더구나 풀어 써보니 행렬인 줄 알았던 수식이 스칼라로 드러났다. 스칼라의 역행렬은 역수니까 계산도 굉장히 쉽다. 이제 위의 결과를 칼만 이득 계산식에 대입해서 정리해 보겠다.

$\begin{align} K_k &= P_k^{-}H^{T}(HP_k^{-}H^{T} + R)^{-1} \\\\ &= \begin{bmatrix} P_{11}^{-} \\\\ P_{21}^{-} \end{bmatrix}(P_{11}^{-} + R)^{-1} \\\\ &= {1 \over P_{11}^{-} + R}\begin{bmatrix} P_{11}^{-} \\\\ P_{21}^{-} \end{bmatrix} \end{align}$

식이 매우 간단해졌다. 그럼 이 식이 과연 칼만 필터 알고리즘의 행렬식을 대신할 수 있는지 확인해보겠다. 위의 식으로 칼만 이득을 계산하는 DeDvKalman 함수를 새로 작성했다. TestDeDvKalman.m은 DeDvKalman 함수를 테스트 하는 프로그램이다.

DeDvKalman.m

function [pos vel] = DeDvKalman(z)
% 행렬이 작은 경우 역행렬을 풀어서 사용하는 함수
%
persistent A H Q R
persistent x P
persistent firstRun


if isempty(firstRun)
  firstRun = 1;

  dt = 0.1;

  A = [ 1 dt;
        0 1  ];
  H = [1 0];

  Q = [ 1 0;
        0 3 ];
  R = 10;

  x = [ 0 20 ]';
  P = 5*eye(2);
end


xp = A*x;  
Pp = A*P*A' + Q;    

K = 1 / (Pp(1,1) + R) * [Pp(1,1) Pp(2,1)]';  % Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;   


pos = x(1);
vel = x(2);

TestDeDvKalman.m

clear all


dt = 0.1;
t  = 0:dt:10;

Nsamples = length(t);

Xsaved   = zeros(Nsamples, 2);
DeXsaved = zeros(Nsamples, 2);
Zsaved   = zeros(Nsamples, 1);

for k=1:Nsamples
  z = GetPos();      
  [pos vel]   = DvKalman(z);
  [dpos dvel] = DeDvKalman(z);

  Xsaved(k, :)   = [pos vel];
  DeXsaved(k, :) = [dpos dvel];
  Zsaved(k)      = z;
end


figure
hold on
plot(t, Xsaved(:, 1), 'ro')
plot(t, DeXsaved(:,1))
xlabel('Time[sec]')
ylabel('Position[m]')
legend('Matrix','Decomposed','location','NorthWest')

figure
hold on
plot(t, Xsaved(:, 2), 'ro')
plot(t, DeXsaved(:,2))
xlabel('Time[sec]')
ylabel('Velocity[m/s]')
legend('Matrix','Decomposed','location','NorthWest')

테스트 프로그램에 특별히 새로 추가된 코드는 없다. 두 칼만 필터 함수가 추정한 위치와 속도를 저장했다가 그래프로 그려 비교해 보는 게 전부이다.

아래 그래프에 속도 추정 결과와 위치 추정 결과를 비교했다. 실선으로 표시한 궤적이 행렬식을 풀어 쓴 결과이고, 원으로 표시한 값은 행렬식으로 계산한 칼만 필터의 출력이다. 두 값이 정확히 일치한다. 칼만 이득을 계산하는 두 개의 식이 수학적으로 동일하니 당연한 결과이다.

이 절에서는 칼만 이득의 행렬식을 풀어서 계산을 간단하게 하고 속도를 개선하는 기법을 살펴봤다. 그런데 이 방법은 상태변수가 많아서 시스템 모델의 행렬이 커지면 사용하기 어렵다. 이 때는 효율적인 수치해석 라이브러리를 사다 쓰거나 만들어 쓰는 수 밖에 없다.

11.7 시스템 모델의 힘

칼만 필터는 잡음도 제거하지만 위치 정보만 가지고 속도를 추정해내는 능력도 있다는 사실을 알았다. 그런데 칼만 필터는 어떻게 측정하지도 않은 속도를 추정해낼까? 지금부터 이 놀라운 능력이 어디에서 나오는지 파헤쳐 보겠다.

먼저 저주파 통과 필터를 생각해 보겠다. 저주파 통과 필터는 측정 신호에 대해 어떠한 가정도 하지않았다. 그저 새 측정값과 이전의 추정값에 가중치를 주고 더할 뿐이다. 따라서 측정하지 않은 물리량을 추정한다는 것은 애초부터 불가능한 이야기이다. 저주파 통과 필터에서는 측정값이 없으면 추정도 없다.

칼만 필터는 다르다. 시스템의 수학적 모델을 알고 있다고 가정한다. 즉 시스템이 어떤 법칙에 따라 측정값을 출력하는지를 정확히 알고 있다는 전제를 깔고 있다. 측정하지도 않은 물리량을 정확히 추정해내는 비밀은 바로 여기에 있다. 상태변수 사이의 연관 관계를 나타내는 시스템 모델을 통해, 측정하지 않은 상태변수를 간접적으로 추정해내는 것이다.

간단한 예를 들어 보겠다. 현재 속도가 $80m/s$ 이면 $1$ 초 뒤에는 $80m$ 만큼 이동해 있을 수 밖에 없다. 왜냐하면 속도를 적분하면 이동 거리가 되는 것은 불변의 물리법칙이기 때문이다. 측정 위치에 잡음이 섞여 있거나 시스템 모델에 오차가 다소 있다고 해도, 열차는 $80m$ 근처에 있어야지 $800m$ 위치에 있을 수는 없다. 반대의 경우도 마찬가지이다. $1$ 초 동안의 이동거리가 $80m$ 인데, 속도가 $800m/s$ 나 될 리는 없다. 대략 $80m/s$ 근방이어야 말이 된다. 이렇게 칼만 필터는 시스템 모델의 정보를 적극 활용해서 측정하지 않은 상태변수도 추정해낸다. 예제의 칼만 필터는 바로 이 점을 이용해서 위치로 속도를 추정해 낸 것이다. 추정 속도도 시스템의 물리 법칙을 따라야 하니까, 자연히 위치 변화의 추이가 속도 추정값에 반영될 수 밖에 없다.

요약하면 칼만 필터가 측정하지 않은 물리량을 추정해내는 능력은 바로 ‘시스템 모델’ 덕분이다. 그런데 추정에 시스템 모델을 이용하는 것은 양날의 칼과 같다. 시스템 모델이 실제 시스템과 많이 다르면 추정 결과가 엉망이 될 뿐만 아니라, 심하면 칼만 필터 알고리즘이 발산해서 전체 시스템이 망가질 수도 있다.

Kalman Filter Study _ 11장. 위치로 속도 재기_1부

Fri, 17 Mar 2017 00:00:00 +0000

앞 장에서 살펴 본 예제의 목표는 측정 잡음을 제거하는 것이었다. 그런데 잡음 제거라면 저주파 통과 필터도 훌륭하게 해낼 수 있다. 더구나 저주파 통과 필터는 칼만 필터 보다 더 간단한 구조라 구현하기도 쉽다. 이런 저주파 통과 필터를 놔두고 굳이 칼만 필터를 사용할 이유가 있을까?

그렇다 칼만 필터는 단지 잡음만 제거하는 필터가 아니다. 이제 아래 예제로 칼만 필터의 능력을 살펴보자.

“어떤 실험으로 직선 선로에서 열차가 $80m/s$의 속도를 유지하는지 확인하려 한다. 위치와 속도 정보는 $0.1$초 간격으로 측정해서 저장하도록 되어있다. 그런데 시험 결과를 살펴보니 속도 데이터가 모두 $0$으로 찍혀 있다. 실험에 드는 비용과 시간이 만만치 않다. 그나마 다행인 것은 위치 정보는 이상이 없어 보인다. 다시 실험을 하지 않고 이 위기에서 빠져 나갈 수 없을까?”

11.1 시스템 모델

이 에제는 그 동안 다뤄온 문제와는 많이 다르다. 지금까지는 주로 측정 잡음을 효과적으로 제거하는 방법을 고민해왔다. 그런데 이제는 위치 정보만 가지고 측정하지도 않은 속도를 알아내야 한다. 일단 저주파 통과 필터로는 이런 유형의 문제를 해결할 수 없다. 저주파 통과 필터는 측정 신호의 잡음만 제거할 수 있기 때문이다.

얼핏 생각하기에 이 예제가 문제를 위한 문제 같아 보일지 모르지만, 의외로 실전에서 종종 접하게 된다. 예를 들어 고도만 측정했는데 상승률을 계산해야 한다거나 각도만 가지고 회전 속도를 알아내야 하는 상황이 곧잘 있다.

잘 알다시피 속도는 이동 거리를 시간으로 나눈 값이다. 따라서 짧은 시간 동안의 이동 거리를 시간으로 나누면 그 구간의 속도를 구할 수 있다. 그런데 실제로 이 방법을 적용해보면 오차가 크다. 잡음 때문에 측정 거리는 들쭉날쭉한데 분모인 시간은 아주 작아서, 나누면 값이 크게 튀기 때문이다. 이러한 문제를 해결하기 위해 몇 개의 측정값을 평균 내서 이동 거리로 사용하거나(이동평균), 측정값을 다항식으로 근사한 다음 그 다항식을 미분하는 등의 방법을 동원하기도 한다. 하지만 이런 해결책은 지저분할 뿐만 아니라, 잘 들어맞지도 않는다. 칼만 필터는 바로 이런 유형의 문제에서 진가를 발휘한다.

그럼 칼만 필터를 설계해보겠다. 가장 먼저 시스템 모델을 찾아야 한다. 우리가 관심 있는 물리량은 열차의 위치와 속도이므로 이 두 변수를 상태변수로 정의한다.

$x = \begin{Bmatrix} 위치 \\\\ 속도 \end{Bmatrix}$

이 상태변수에 대해 예제의 시스템 모델은 다음과 같이 잡겠다.

$x_{k+1} = Ax_k + w_k \\\\ z_k = Hx_k + v_k$ $A = \begin{bmatrix} 1 & \triangle t \\\\ 0 & 1 \end{bmatrix} \\\\ H = \begin{bmatrix} 1 \\\\ 0 \end{bmatrix}$

여기서 $\triangle t$는 위치를 측정하는 시간 간격을 의미한다. 이미 언급했듯이 시스템 모델을 찾아내는 것은 예술이자 기술이다. 칼만 필터 설계자의 경험과 능력에 의존할 수 밖에 없다는 말이다. 참고로 비슷한 문제를 참고하는 것도 좋은 방법이다.

그럼 위 시스템 모델이 예제의 내용을 제대로 반영하고 있는지 확인해보자. 먼저 행렬 $A$를 시스템 모델에 대입해서 상태변수 관계식을 풀어 보자.

$\begin{align} x_{k+1} &= Ax_k + w_k \\\\ &= \begin{bmatrix} 1 & \triangle t \\\\ 0 & 1 \end{bmatrix} + w_k \end{align}$

이 식에 상태변수의 정의를 대입해서 정리하면 더 구체적인 의미를 알 수 있다.

$\begin{align} \begin{bmatrix} 위치 \\\\ 속도 \end{bmatrix}_{k+1} &= \begin{bmatrix} 1 & \triangle t \\\\ 0 & 1 \end{bmatrix}\begin{bmatrix} 위치 \\\\ 속도 \end{bmatrix}_{k} + \begin{bmatrix} 0 \\\\ w_k \end{bmatrix} \\\\ &= \begin{bmatrix} 위치+속도 \cdot \triangle t \\\\ 속도 + w \end{bmatrix} \end{align}$

먼저 위치와 관련된 관계식만 따로 떼어내서 살펴보겠다.

$위치_{k+1} = 위치_{k} + 속도_{k} \cdot \triangle t$

이 식은 ‘현재 위치 = 직전 위치 + 이동거리’ 라는 물리 법칙을 수식으로 표현한 것이다. 시스템 잡음이 관계식에 포함되지 않은 것은 이 때문이다. 이제 속도 관계식을 살펴보겠다.

$속도_{k+1} = 속도_{k} + w_k$

이 식이 의미하는 바는 열차 속도는 시스템 잡음($w_k$)의 영향만 받을 뿐, 다른 외부의 힘은 작용하지 않는다는 것이다. 즉 속도가 일정하다는 말이다. 예제에서 열차의 속도가 일정하다고 했으니 제대로 모델링 되었다. 여기서 시스템 잡음은 마찰, 엔진 제어기의 오차 등 열차 속도에 영향을 주는 모든 요인의 합을 의미한다.

그럼 이제 시스템 모델의 측정값 관계식을 살펴보겠다. 행렬 $H$를 측정값 관계식에 대입해서 정리한다.

$\begin{align} z_k &= Hx_k + v_k \\\\ &= \begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} 위치 \\\\ 속도 \end{bmatrix}_{k} + v_k \\\\ &= 위치_{k} + v_k \end{align}$

이 식이 의미하는 바는 측정하는 값은 위치이고, 측정값에는 잡음이 섞여 있다는 것 이다. 이러한 사실은 예제의 내용과 정확히 일치한다.

지금까지 시스템 모델이 실제 시스템과 어떻게 연관되어 있는지를 설명했다. 이처럼 행렬 $A$와 $H$는 임의로 선정한 것이 아니라, 시스템의 물리적인 관계를 모델링한 결과이다. 즉 시스템 모델은 속도와 거리 사이의 물리적 관계, 속도에 영향을 주는 오차 요인 등을 수학식으로 표현한 것이다.

마지막으로 잡음의 공분산 행렬($Q, R$)만 결정하면 시스템 모델의 유도는 끝난다. 측정 잡음($v_k$)의 오차 특성은 센서 제작사에서 제공하는 경우가 많다. 그렇지 않다면 실험과 경험을 통해 결정해야 한다. 시스템 잡음($w_k$)의 모델링은 더 어려운데, 시스템에 대한 지식과 경험에 의존할 수 밖에 없다. 만약 두 공분산 행렬을 해석적으로 구하기 어렵다면 이 행렬을 칼만 필터의 설계 인자로 보고 시행착오를 거쳐 선정해야 한다. 이 예제에서는 두 행렬을 다음과 같이 선정했다.

$Q = \begin{bmatrix} 1 & 0 \\\\ 0 & 3 \end{bmatrix} ,\; R = 10$

11.2 칼만 필터 함수

시스템 모델을 구하는 작업은 모두 끝났으니, 이제 칼만 필터를 구현해보자. 아래 DvKalman 함수가 칼만 필터의 구현 코드이다. 이 참수는 측정값을 인자로 받아 위치 추정값과 속도 추정값을 반환한다. 시스템 모델만 바뀌었을 뿐, 알고리즘 구현 코드는 앞 장의 예제에서 거의 달라진게 없다.

DvKalman.m

function [ pos vel ] = DvKalman(z)
% 위치로 속도 측정하는 함수
%

persistent A H Q R
persistent x P
persistent firstRun

if isempty(firstRun)
    firstRun = 1;

    dt = 0.1;

    A = [ 1 dt;
          0 1];

    H = [1 0];

    Q = [ 1 0;
          0 3];

    R = 10;

    x = [0 20]'; % 초기위치 0, 초기속도 20
    P = 5*eye(2);
end

xp = A*x;  
Pp = A*P*A' + Q;    

K = Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;   


pos = x(1);
vel = x(2);

end

11.3 추정 결과

측정한 열차 위치와 칼만 필터가 추정한 위치를 비교해서, 알고리즘이 잘 작동하는지 확인해보자. TestDvKalman.m은 앞 절의 칼만 필터 함수를 테스트하기 위한 프로그램이다.

TestDvKalman.m

clear all


dt = 0.1;
t  = 0:dt:10;

Nsamples = length(t);

Xsaved = zeros(Nsamples, 2);
Zsaved = zeros(Nsamples, 2);

for k=1:Nsamples
  [z trueV] = GetPos();
  Zsaved(k, :) = [z trueV];

  [pos vel] = DvKalman(Zsaved(k,1));
  Xsaved(k, :) = [pos vel];
end


figure
hold on
plot(t, Zsaved(:, 1), 'r.')
plot(t, Xsaved(:, 1))
xlabel('Time[sec]')
ylabel('Position[m]')
legend('Measurements','Kalman Filter','location','NorthWest')

figure
plot(t, Zsaved(:, 2), 'r:')
hold on
plot(t, Xsaved(:, 2), 'b')
xlabel('Time[sec]')
ylabel('Velocity[m/s]')
legend('True Speed', 'Kalman Filter','location','SouthEast')

GetPos.m

function [z , trueV] = GetPos()
%
%
persistent Velp Posp


if isempty(Posp)
  Posp = 0;
  Velp = 80;
end

dt = 0.1;

w  = 0 +  10*randn;
v  = 0 +  10*randn;

z = Posp + Velp*dt + v;

Posp = z - v;     % true position
Velp = 80 + w;    % true speed

trueV = Velp;

측정 위치는 GetPos 함수로 읽어 오는데, 열차 속도가 $80m/s$에서 약간씩 변한다고 가정했다. 실전에서는 GetPos 함수 대신 센서로 측정한 위치를 받아온다.

위 그래프에서 실선은 칼만 필터의 추정 위치를 나타내고, 점으로 표시된 데이터는 위치 측정값을 의미한다. 측정 잡음을 효과적으로 제거하여 추정 위치의 궤적이 더 매끄러워졌고, 열차의 이동 경향도 잘 나타나 있다.

위 그림은 칼만 필터가 추정한 속도를 그린 그래프이다. 그림에서 실선은 칼만 필터가 추정해낸 속도이고, 점선이 측정 잡음을 뺀 열차의 진짜 속도를 나타낸다. 그림을 보면 추정 속도의 경향이 진짜 속도와 아주 비슷하다. 그런데 처음에는 추정 속도 오차가 상당히 크게 나다가, $2$초 정도 지나서야 비교적 추정값이 정확해진다. 테스트 프로그램에서 초기 속도를 $20m/s$ 라는 어이 없는 값을 가정한 탓이다. 그럼에도 불구하고 속도를 정확하게 추정해냈다.

단지 칼만 필터 알고리즘을 돌렸을 뿐인데, 위치 정보만 가지고도 상당히 정확하게 속도를 알아냈다. 이게 바로 저주파 통과 필터와는 차원이 다른 칼만 필터의 진짜 능력이다.