Localization : Kalman filter

앞서 포스팅한  Histogram filter와 같이 Kalman filter도 관측데이터로 부터 위치를 예측하는 방법에 자주 등장한다. Kalman filter는 공학분야에서 매우 다양하게 사용하므로 매우 중요한 위치를 차지하며, 널리 알려지진 않았지만 인류 기술발전에 지대한 공헌을 한 사람으로 이 filter를 제안한 Kalman이 선정되었다는 기사를 본 기억이 있다.

Kalman filter는 일종의 control system을 포함한 dynamics system의 선형 미분방정식의 해를 구하는 방법이라고 할 수 있다. 방법은 관측결과와 예측치의 차(error)를 최소화 하도록 recursive iteration을 이용하는 접근방법을 사용한다. 그러므로, system dynamics를 선형화하기 위한 Jacobian matrix가  필요하며, 지속적을 관측데이터를 받아 예측에 사용해야 한다. 공학이나 수학분야에서 시간에 따라 변하는 system의 수학적 모델링을 해본 사람은 이것이 얼마나 유용하고 중요한것인지 짐작 할 수 있을 것이다.

System Dynamics

앞서 언급하였듯이 Kalman filter는 system을 선형화하여 해석하려는 방법이다. System의 state는 우리가 예측하고자 하는 state variable이며,  x \in R^n의 dimension을 가진다. 선형화를 통해 원래 system의 수학적 모델링인 differential equation을 아래와 같이 difference equation으로 표현한다.

x_k = Ax_{k-1} + Bu_{k-1} +w_{k-1}

 z_k = Hx_k + v_k

관측치/측정치(measurements)는 z \in R^m dimension을 가지며, A는 process matrix, B는 control matrix, H는 state정보와 measurements와의 관계를 나타내는 matrix이다. 이와 더불어  w,v는 각각 process와 measurements의 noise를 표현하는 변수로, 아래와 같이 Gaussian distribution 형태를 띤다.

 p(w) \sim N(0, Q)

 p(v) \sim N(0, R)

여기서 Q는 process noise covariance, R은 measurement noise covariance 를 의미한다. 그냥 쉽게 생각하면 \sigma^2 값으로 noise 또는 오차의 크고 적음을 나타낸다.

Kalman Filter

Kalman filter를 유도하기 위한 방정식은 아래와 같이 posteriori state estimate,  x_k 을 priori state estimate,  x_k^- 과 실제 관측치인 z_k와 measurements 예측치인 Hx_k^-의 차(residual이라고 한다)로부터 구한다고 정의하는것 부터 시작한다.

 x_k = x_k^- + K(z_k - Hx_k^- )

여기서, K는 Kalman gain이며, 우리가 지정해야 하는 것은 아니다. 일반적인 사용자는 이 Kalman filter의 수학적 해를 유도하는 것이 목적이 아니므로, 식을 유도하는 과정은 필요하지 않다고 생각되지만, 위에 나타낸 모델링 식들의 의미는 정확히 알아야 하는데, 그것은 각 matrix를 잘 정의 해줘야 Kalman filter의 예측치가 의미가 있기 때문이다. 위의 식들은 LTI(Linear Time-Invariant) system의 정의와 유사하기 때문에 함께 참조하면 좋을 것이다.

kalman-filter-cycle

Kalman filter 알고리즘은 간단히 위 그림과 같이 time updatemeasurement update의 연속과정이라고 할 수 있다. Time update는 next time step state를 사전예측하는 것을 의미하며,  measurement update는 관측데이터를 사전 예측에 적용하여 예측의 정확도를 향상시키려는 사후예측단계라 할 수 있다. 이러한 관점에서 time update를 predictor, measurement update를 corrector라 부른다. 아래에 state 예측을 위한 Kalman filter 식들을 정리하였다.

kalman-filter-eq

Kalman filter는 위와 더불어 Gaussian distribution에 대한 개념이 필요하다. 그 이유는 Kalman filter가 Gaussian distribution을 가지는 변수를 다루기 때문이다. 수학적으로 일정한 값이 아닌 stochastic variable을 사용하여 정의한다고 생각하면 이해가 쉽다.

Kalman filter 사용예

Kalman filter의 code는 사실 matrix를 풀어내기만 하면 된다. 단, linear algebra에 대한 개념과 각 matrix의 dimension그리고 matrix element의 값들을 정확히 이해하고 입력해야 한다. 아래 그림은 움직이는 물체의 좌표를 시간에 따른 변화를 나타낸 것이다. 이 그림에서 초기 좌표는 (-4, 8) 이며 시간에 따라 관측된 위치좌표를 나타낸 것이다. 이 system의 state는 (x,y,\dot{x},\dot{y})이다. 이에 대해 Kalman filter를 적용하면 아래와 같이 최종 좌표에 대해 예측된 값을 얻어 낼 수 있다.

kalman-filter-fig

kalman-filter-result

관측 값은 (16.0, -8.0)이며, 예측 결과의 x 및 y좌표값이 거의 일치하는 것을 알 수 있다. 세번째와 네번째의 값은 계산에 사용한 time difference값 0.1을 곱해서 얻어지는 x축과 y축의 속도값을 예측한 것이다. 어느 필터도 마찬가지 이지만 이 system과 같이 단순 움직임을 보이는 물체의 예측결과는 매우 정확하다는 것을 알 수 있다.

Non-linear system Kalman filter

System이 non-linear인 경우에는 기본적으로 Kalman filter를 적용할수 없다. 왜냐하면, normal Gaussian distribution을 가지는 state vector가 non-linear dynamic system에 도입되면 그 출력이 Non-Gaussian distribution을 보이기 때문이다. 그러므로 non-linear system을 linearize한후 적용해야 하며, linearization은 유명한 Taylor series expansion의 1차항까지만 사용하여 구한다. 그러므로 state function의 Jacobian은 선형화된 함수에 대한 편미분하여 구하는 것이 순리적인 방법이다.

다음은 nonlinear system의 적용예이다. 비선형으로 표현되는 state에 Kalman filter를 적용하는 경우를 EKF(Extended Kalman Filter)라고 하며, 다음 결과는 원형으로(비선형 거동) 움직이는 로봇의 위치를 EKF가 추적하는 모습을 보여주고 있다. 녹색은 ground truth 로봇 위치를 나타내며, 붉은색은 measurement에 의한 위치 값, 푸른색은 EKF에 의해 예측된 위치를 나타낸다. 결과그림중 왼쪽은 measurement noise/error가 작은 경우, 오른쪽은 그 값이 큰 경우를 나타낸다. EKF가 로봇의 원형 궤도 움직임을 잘 추적하는 것을 볼 수 있다.

(Note) Kalman filter가 수학적 기법에 의존한 것이라면, 다음에 소개 예정인 particle filter는 멋진 아이디어와 컴퓨터의 강력한 계산 속도에 의존한 매우 간단한 기법이다.

Advertisements

Localization : Kalman filter”에 대한 답글 1개

답글 남기기

아래 항목을 채우거나 오른쪽 아이콘 중 하나를 클릭하여 로그 인 하세요:

WordPress.com 로고

WordPress.com의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Google+ photo

Google+의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Twitter 사진

Twitter의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

Facebook 사진

Facebook의 계정을 사용하여 댓글을 남깁니다. 로그아웃 /  변경 )

%s에 연결하는 중