Category Archives: Kalman Filter

칼만필터에 대한 알기쉬운 설명, Kalman Filter explanation.

인터넷 여기저기에 칼만 필터에 대한 설명 자료를 찾아봤지만 딱 이거다 싶은 글은 많지 않은거 같군요. 또 어떻게 작동하는고 어떻게 내가 원하는 프로그램에 적용하는가에 대한 정보를 거의 구할 수가 없어서 마침 Robabilistic Robotics 관련 자료 정리할 겸해서 몇개의 글을 쓰기로 했습니다.

본 내용은 가장 칼만필터에 대한 설명이 잘 되어있는 아래 문서의 내용을 정리한 것임을 밝혀 둔다.

http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

칼만필터 (이후 KF)는 비행체 위치 제어, 네비게이션이나 영상 처리등 여러 분야에서 각 시스템의 작동 중에 필수적으로 요구되는 주요 변수 (비행체의 자세, 이동체의 위치 또는 Tracking 하는 객체의 위치등)를 예측하는데 사용 되는 도구 입니다. 수학적으로 칼만 필터는 선형 시스템(Linear System)의 상태를 예측해서 발생할 수 있는 오류를 최소화 하면서 예측합니다.

선형시스템 (Linear Systems)

선형 시스템(http://en.wikipedia.org/wiki/Linear_system) 이란 시스템을 모델링한 수식이 선형 연사자 (linear operator)들로 표현이 가능한 시스템을 말합니다. 실제 환경에서 이러한 선형 시스템을 찾기는 쉽지 않으며 많은 경우 비선형 시스템을 위한 EKF나 UKF등을 사용하는 경우가 많습니다만 KF는 선형 시스템의 예측을 위한 필터이니 만큼 선형 시스템을 위주로 설명하도록 하겠습니다.

KF가 추출된 신호 (signal)에서 잡음(noise)를 제거하기 위해서는 현재 모델링하려는 프로세스 (즉, 위치제어 또는 영상처리 시스템의 주요 기능)가 선형 시스템으로 기술이 가능해야 합니다. (즉, Y = aX + b와 같은 수식으로 표현이 되어야 한다는 뜻입니다) 길을 따라 주행 중인 자동차, 지구 주위를 도는 위성, 지나가는 전하 흐름에 의해서 돌아가는 모터 샤프트나 라디오 신호 전파에 사용되는 사인파 의 경우는 선형 시스템으로 근사화(approximated) 시킬 수 있습니다.

선형 시스템을 기술 할때 일반적으로 아래의 2개의 간단한 수식으로 표현 합니다.

상태 수식 (Sate Equation – 시스템의 전반적인 신호 파형을 표현 합니다. Process Model, Plant Model등으로 불립니다):

출력 수식 (Output Equation – 시스템 신호 중 측정 가능한 값들을 표현 합니다. Measurement Model, Sensor Model등으로 불립니다):

우선, A, B, C는 메트릭스(Matrix – 모피우스를 빨랑 찾아야 할거 같네요) 입니다. k는 시간의 index이며 x는 시스템의 상태를 나타내는 벡터(Vector)입니다. y_{k}는 센서등을 이용해서 측정된 값을 의미하며, w, z는 잡음 (에러) 입니다. 특히, w는 프로세스 잡음(Process Noise), z를 측정 잡음 (Measurement Noise)라고 합니다. x, y, w, z 는 대부분의 경우 벡터 입니다.
이 부분에서 잡음은 무시 하면 될 듯 합니다만 KF에서 가장 중요한 것이 잡음 입니다. 실제 KF의 기본적인 동작이 가능해지는 시점에 튜닝을 위해서는 이들 잡음를 나타내는 Covariance Matrix의 값들을 조절해서 KF의 감도나 응답 특성을 변경할 수 있게 됩니다. 실제 주요 튜닝 방법은 이후 다른 포스트에서 논의 하도록 하겠습니다.

우선, 프로세스 상태 벡터인 X의 경우 시스템의 현재 상태에 대한 모든 정보 (비행체의 위치 정보 – X, Y, Z좌표와 각도, 가속도 바이어스등)를 나타냅니다. 하지만, 중요한 것이 이들을 직접적으로 측정할 수가 없습니다. 대신, 측정 잡음 Z에 의해서 어느 정도 값이 깨어진 Y를 (즉, 센서에서 읽어들인 값들을) 측정할 수 있습니다. 이 측정된 Y를 이용해서 거꾸로 X를 계산 해 낼 수 있습니다. (단순 선형식을 이용해서 X를 계산하고 측정된 Y값을 이용해서 이를 다시 보정합니다. 이는 아래 쪽에서 좀더 자세히 설명 됩니다)

시스템의 상태인 X를 Y의 값을 이용하면 추정(estimate)해 낼 수 있다고는 합니다만 이는 측정 잡음 Z에 의해서 값이 변형이 된 상태이기 때문에 그대로 값을 사용할 수는 없습니다. 모든 센서에는 기본적으로 가지고 있는 오차가 있습니다. GPS의 경우 5-6개의 위성이 Fix된 상태라고 한다 하더라도 오차는 작게는 2-3미터 크게는 10-20미터까지 날 수 있습니다. 물론 Gyro, Magneto 센서나 가속도 센서 (Accelerometer)의 경우도 역시 마찬가지 오차를 가지고 있으며 Datasheet에 어느정도 기술 되어 있습니다. 하지만 정확한 튜닝을 위해서는 이값들을 조금씩 변행해야 합니다.

원저에서 저자는 Y를 정치가에 비유하네요. 어느정도 믿어야 하지만 (믿을 수 밖에 없지만) 모든 내용을 신뢰하지는 말아야 한다는 거죠 ^.^

그럼 직선으로 주행 중인 자동차의 예를 들어 보죠. 자동차의 현재 위치는 p, 속도는 v로 이동 하고 있습니다. 시스템의 입력 u는 현재 지시된 가속도 이고 출력치(측정치) 인 y는 현재 위치 입니다.  매 T 초 마다 가속도를 변경하면서 위치를 측정한다고 합시다.

이때 우리는 기본적인 물리 법칙에 의해서 속도 v가 다음의 식에 의해서 표현 된다고 할 수 있습니다:

이는 T초 후의 속도는 현재 속도에 지시된 (가령, 자동차의 가속 페달을 밟아서 속도를 올리거나 브레이크를 밟는등) 가속도에 시간 T를 곱한 값이 됩니다. 하지만 위의 식에 의해서 정확한 v_{k+1}의 값을 알수는 없습니다. 이유는 도로상에 갑작스럽게 바람이 분다던가 노면이 울퉁 불퉁 하다거나 다른 상상할수 있는 많은 이유에 의해서 속도에 변화가 발생하게 될 수 있다는 것이죠. 이때 속도에 관여한 잡음은 시간에 따라서 변화하는 랜덤변수 (Random variable) 입니다.

그래서, 좀더 실제에 가까운 식은 아래와 같이 기술 될 수 있습니다:

위 식에서 v~_{k}는 속도 잡음 입니다.

위치p 를 표현하는 공식도 다음과 같이 기술 될 수 있습니다:


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

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

이제 최종적인 측정 값이 현재 위치라고 했으니 전체 시스템을 위한 선형 식은 다음과 같이 표현이 가능합니다:


위에서 x_{ k+1}을 p, v로 각각 Row 별로 간단한 행렬 계산을 해 보시면 이전에 언급했던 간단한 물리 공식과 같음을 아실 수 있을 겁니다. 즉, 프로세스 상태 식은 우리가 표현 하려는 프로세스의 물리식을 행렬로 표현 한 것이라고 보시면 됩니다.
y_{k} 의 경우는 측정하려는 위치에 잡음이 고려된 공식 입니다.

z_{k}는 센서나 측정 장비의 오류등에 의한 측정 잡음(measurement noise) 입니다. 현재 고려 하고 있는 예제에 피드백 시스템을 도입하려고 한다면 위치 p와 속도 v가 어느 정도 정확하게 측정되어야 합니다.

즉,  x를 정확하게 예측 해야 한다는 것이지요. 이를 위해서 칼만 필터가 사용 됩니다.

다음 포스팅에서 KF에 대해서 논의 해 보죠.