Kalman Filter

워,,, 이거하나만 가지고도 책 한권이 나올정도구나

아무튼

 

 

칼만필터

비행기 위치 제어, 네비게이션, 영상처리 등 여러 분야에서 각 시스템 작동 중에 필수적으로 요구되는 주요 변수(비행기의 자세, 이동체의 위치 또는 트래킹하는 객체의 위치 등)를 예측하는데 사용하는 필터

수학적으로는 선형시스템의 상태를 예측해서 발생할 수 있는 오류를 최소화 하면서 예측한다.

 

 

선형 시스템 (Linear System)

시스템을 모델링한 수식이 선형 연산자들로 표현이 가능한 시스템을 말한다.

칼만필터는 선형시스템의 예측을 위한 필터

칼만필터가 추출된 신호에서 잡음을 제거하기 위해서는 현재 모델링하는 프로세스가 선형시스템으로 서술이 가능해야 한다. (즉, Y = aX + b같은 수식으로 표현이 가능해야 한다)

길을 따라 주행 중인 자동차, 지구 주위를 도는 위성 뭐 요런거는 선형시스템으로 근사화 시킬 수 있다.

 - 어?,,,,,, 사람의 움직임은 선형적이지 않은데 적용시킬 수 있는 건가?

 

선형 시스템을 일반적으로 아래의 두개의 수식으로 표현한다.

상태수식 - Sate Equation, 시스템의 전반적인 신호 파형을 표현한다.

출력수식 - Output Equation, 시스템 신호 중 측정 가능한 값들을 표현

요 위의 식들 중 A, B, C는 매트릭스,, 행렬이야

k : 시간의 인덱스

x : 시스템의 상태를 나타내는 벡터

y_{k} : 센서 등을 이용하여 측정한 값

w, z : 잡음 (w - 프로세스 잡음, z - 측정 잡음)

x, y, w, z는 대부분 벡터

 

잡음은 무시해도 될 것 같지만, 사실 그렇지 않다.

칼만필터에서 기본적인 동작이 가능해지는 시점에 튜닝을 위해서는 이 잡음을 나타내는 Covariance Matrix의 값들을 조절해서 칼만필터의 감도나 응답 특성을 변경할 수 있게 된다.

 

먼저, 프로세스 상태 벡터 X의 경우 시스템의 현재 상태에 대한 모든 정보 (정맥주사 시스템이라 하면 이동 벡터의 위치)를 나타낸다, 하지만, 요고를 직접적으로 측정할 수 없다. 대신, 측정 잡음 Z에 의해서 어느정도 값이 깨어진 Y(센서에서 읽어들인 값)를 측정할 수 있고, 이 Y를 이용해서 거꾸로 X를 계산해 낼 수 있다. (단순 선형식을 이용해 X를 계산하고 측정된 Y값을 이용하여 이를 다시 보정한다. 아래에서 다시 설명할게)

 

시스템의 상태인 X를 Y의 값을 이용하면 추정(estimate)해 낼 수 있다고는 하지만, 이는 측정 잡음 Z에 의해서 값이 변형된 상태이기 때문에 그대로 값을 사용할 수 는 없다. 모든 센서에는 기본적으로 가지고 있는 오차가 있다, 정확한 튜닝을 위해서 이 오차들을 잡아줘야 한다. 그러니깨 측정된 모든 내용을 신뢰하지는 말라 이거다.

 - 그래 내 주사기 적외선 카메라도 값이 튀더라구

 

직선으로 주행중인 자동차를 예를들어보자

p : 자동차의 현재 위치

v : 속도

u : 시스템의 입력, 현재 지시된 가속도

y : 출력치, 현재위치

매 T초마다 가속도를 변경하면서 위치를 측정한다고하면 기본적인 물리법칙에 의해 속도 v가 아래와 같이 표현될 수 있다.

요고는 T초 이후의 속도는 현재 속도에 지시된(그니까, 자동차의 악셀을 밟거나 브레끼를 밟는 등) 가속도에 시간 T를 곱한 값이 된다. 그렇지만, 위의 식만 가지고는 정확한 y_{k+1}의 값을 알 수 없다, 왜냐하면 갑작스럽게 바람이 분다거나 도로가 울퉁불퉁한다든가 여러가지 알 수 없는 많은 이유들로 인해 속도가 변할 수 있다는거~

이 때, 속도에 관여한 잡음은 시간에 따라서 변화하는 랜덤변수다.

 

그래서, 좀 더 실제에 가까운 수식은 아래와 같다.

위 식에서 +된 v~_{k}는 잡음

 

위치 p를 표현하는 공식도 아래와 같이 기술될 수 있다.

위 식에서 p~_{k}는 위치잡음이다.

그럼 이제 우리가 표현하려는 시스템의 상태 (State Vector, X)를 아래와 같이 현재 위치와 속도로 구성되어있다고 가정하겠습니다

이제 최종적인 측정 값이 현재 위치라고 했으니, 전체 시스템을 위한 선형 식은 아래와 같이 표현할 수 있다.

위에서 x_{k+1}을 p, v로 각각 행별로 간단한 행렬계산을 해보면 이전에 언급했던 물리공식과 같다는걸 알 수 있다. (맨 위 상태와 출력수식)

즉, 프로세스 상태 식은 우리가 표현하려는 프로세스의 물리식을 행렬로 표현한 것이라고 보면 된다.

 

y_{k}는 측정하려는 위치에 잡음이 고려된 공식이다.

 - y_{k}는 실제 측정 가능한 값

 

z_{k}는 센서나 측정 장치의 오류등에 의한 측정잡음(measurement noise)이다.

현재 고려하고 있는 예제에 피드백 시스템을 도입하려고한다면, 위치 p와 속도 v가 어느정도는 정확하게 예측되어야 한다.

 

즉 위 식의 x_{k+1}를 정확하게 예측해야한다는건데,,

그걸 위해 칼만필터를 사용한다.

 - 이야~ 이제 왜 쓰는지가 나왔어

 

 

칼만필터 이론과 알고리즘

이미 시스템이 어떻게 동작하는지 표현하는 공식의 모델링도 했고, 위치를 측정할 수도 있다. (방금까지 위의 내용이 그거) 

 - 수의대 내용은 시스템의 동작, 위치측정 이미 다 할 수 있다.

그럼 어떻게 상태 x를 예측할 수 있을까요? 그러니까 지금 얻어낸 값이 최적의 값이라고 할 수 있을까?

 - 그럴 수 없으니까 지금 칼만필터 공부하는거야ㅠㅠ

 

일단 시스템 동작의 결과(원하는 상태 x) 모두를 정확하고도 직접적으로 측정할 수는 없더라도 정확하게 예측할 수 있는 예측 시스템이 있다면 얼마나 좋것니, 그리고 그 시스템을 고르는 결정 방법은 또 뭘까잉

 

그럴라면, 일단 아래의 두가지 수학적 요구조건이 있다.

 

1) 예측한 상태의 평균값이 실제 시스템의 (100%, Exact TRUTH!) 진짜 상태의 평균과 동일하다면 좋다. 당연한거 아니겠니

 - 이것의 의미는 통계적인 평균값이 다를경우 (즉, 칼만필터는 통계학적 기반) 예측이 어느 특정 방향으로 바이어스가 발생한다는 뜻이야, 그러니까 통계학적으로 추정(예측)의 기대값이 실제 시스템의 기대값과 같아야 한다는 것!

 

2) 현재 예측된 시스템의 상태와 실제 상태의 차이가 작으면 작을 수록 좋다.

 - 역시 수학적으로 가능하면 가장 작은 오류 변형이 발생하는 Estimator를 찾아야 한다.

 

칼만필터를 이용한다고해서 모든 부분에서 최상의 결과를 얻을 수 있는 것은 아니며, 이를 위해 시스템에 영향을 미치는 잡음이 어느정도의 가정에 맞아들어가야만 한다.

 

위에 써놨던 프로세스 잡음 w와 측정잡음 z의 경우 이들 잡음의 평균이 0이라고 (White Noise : 스펙트럼의 잡음 밀도가 일정한 경우)가정하고 있다. 또한, w와 z와의 상호작용이 없어야 한다. 즉, 어떤 시간 k에서든 w_{k}와 z_{k}는 독립적인 랜덤변수여야 한다는 것이다.

 

이제 노이즈 코베리언스 매트릭스 (발음좋코~ Noise Covariance Matrix : 잡음 공분산 행렬)인 S_{w}와 S_{z}(프로세스와 측정잡음)는 각각 아래와 같이 정의된다.

여기서 w^{t}와 z^{t}는 w와 z 노이즈 벡터의 전치를, E(...)는 기대값을 말한다.

 

이것이 바로 전치

 

그럼 칼만필터는 뭘까요, 칼만필터를 표현하는 여러가지 공식이 있지만, 가장 일반적인 공식(첨부한 원문,, pdf 파일의 공식)을 보면....

 - ......아 나 지금까지 수식도 좀 힘들었는데 이자식이

우선 맨 처음 수식의 -1 : 역행렬

T : 전치

K : 칼만 게인(Kalman Gain)

P : 예측 오류 공분산 행렬

 

두번째 공식에 꺾어진 모자를 쓰고 있는 x는 상태 예측 공식

우선 요 공식에 집중해보면,,

앞의 는 이전 시점에 예측된 상태 벡터 x를 A로 곱하고, 현재 확인된 입력(가속도 변화 등)을 B로 곱한 결과를 더한 것

 - 노이즈를 제거한 선형 시스템의 상태 예측 공식, 센서 계측이 전혀 반영되지 않았음

 - 수의대를 예로들면 이전 시점의 이동행렬과 지금 막 움직인 이동행렬을 곱한 결과를 더함

 

그래서, 두번째 항인 아래 수식을 보정항(Correction term)이라고 하고 새로 수집된 계측 자료에 의해서 얼마나 많은 보정을 예의 시스템 상태 예측에 가해야 하는지를 나타낸다.

아래의 공식에서도 유의해야하는건 계측 자료에서 이전의 출력 수식 (Measurement : 측정 역시 예측한다는 뜻)을 뺀 값(많은 칼만필터 관련 문서에서는 이 수식을 Innovation이라 한다.)을 칼만 게인과 곱하는 부분

 

보정 항 :

칼만필터 공식에서 칼만게인 부분을 주의깊게 살펴보면, 만일 센서의 노이즈가 큰 경우 결국 S_{z} (측정 잡음 공분산)가 커지게 되고 k의 값이 작아지게 된다.

결과적으로 다음에 이어질 상태 예측 공식에서 새로 수집된 자료 내용에 대한 신뢰가 떨어지게 되며 시스템 상태 예측에 무게가 실리게 된다.

반대로, 노이즈가 작은 경우 S_{z}가 작아지고 K는 커지며 다음 상태 예측 공식에서 더 많은 신뢰를(즉, 실측 자료의 내용이 좀 더 많이 반영되는) 가져오게 된다.

 

마지막의 오류 공분산 행렬의 경우에는 프로세스 잡음과 측정 잡음, 이전에 (즉, k번째)계산된 오류 공분산 행렬을 이용해서 업데이트 된다.

 

원문에서 별다른 스포트라이트를 받고있지는 못하지만, 실제 이 행렬은 시스템의 상태 및 측정 오류등의 변화량 등의 값들을 추적/기억하고 있다.

로보틱스(특히 SLAM)에서는 이 행렬과 상태 벡터를 새로 확인되어진 랜드마크(센서에 의해 지표로 인지 가능한 마크)를 추가해서 전반적인 상태 변화를 추적하는데 사용되기도 한다.

 - 어후 어려워

 

 

예제

자동차 관련 예측

 - 원문 저자가 미쿡인이라서 단위는 피트를 쓴다......ㅠㅠ

 

주요 파라미터

1. 입력 가속 : 1 foot / s^2

2. 가속 노이즈 : 0.2 feet / s^2

3. 위치 결정 노이즈 : 10 feet

4. 측정은 1초당 10회 (즉, 10Hz, T = 0.1)

 

위의 오류들은 모두 1-sigma (one standard deviation : 표준편차 1 내외) 내의 값이다.

 

아래의 수식에서 바뀐 부분은 T와 관련된 부분이 실제 값으로 바뀐 것 뿐

원문에서는 T(시간변화율)를 0.1초로 보고있다.

출력 노이즈인 S_{z}의 경우는 단순 스칼라 값이며, 위의 파라미터를 보면 10 feet 이므로, 결국 100( = 10 * 10)이 된다.

S_{w}를 구하는 과정을 보면, 상태는 위치와 속도의 2개 정보를 가지고 있다.

위치는 가속도에 대해 0.005배에 비례하고, 가속 노이즈가 0.2feet/s^2이기 때문에 위치 노이즈의 편차(variance)는 (0.005^2) * 0.2^2 = 10^-6 (10의 -6승)이 된다.

유사하게, 속도의 경우 가속에 대해서 0.1배 만큼 비례하며 결국 (0.1^2) * 0.2^2) = 4 * 10^-4가 된다.

결국 행렬은 아래와 같이 표현된다,

여기서 vp는 vp = pv = (0.005 * 0.2) * (0.1 * 02) = 2 * 10^-5 요고다.

여기까지 수집된 결과들과 수식들을 아래의 문서에 도식화하고 있는것과 같이 무한루프에 빠지도록 프로그램을 작성하면 되것습니다

 

여기까지 이론적 예제였고,

첨부해놓은 cpp파일은 칼만필터 소스파일인데... 헤더가없네요?

 

 

 

kalman_sample.cpp

 

칼만필터 라이브러리

http://kalman.sourceforge.net/

칼만필터 라이브러리 API

http://kalman.sourceforge.net/doc/index.html

원문 블로그  

http://blog.naver.com/hangondragon?Redirect=Log&logNo=20066836673