여러 종류의 칼만필터(Family members of Kalman Filter)(EKF)

Jinyong 님 블로그

[SLAM] Kalman filter and EKF(Extended Kalman Filter) · Jinyong

Kalman Filter & EKF (Cyrill Stachniss)

 

이전 포스팅에서 칼만필터에서 알아봤습니다. 칼만필터의 한계는 Linear model에서 효과적인 성능을 보이지만 현실 세계에선 대부분이 비선형적으로 모델링된다는 것입니다. 따라서 이를 해결하기 위해 여러 종류의 Kalman Filter들이 나오게됩니다.

 

EKF(Extended Kalman Filter)

EKF(Extended Kalman Filter)는 현실세계의 비선형성을 갖는 모델에도 칼만필터를 적용하기 위해 나온 칼만필터, 즉 non-linear Gaussian model이라고 생각하시면 됩니다.

 

기존 칼만필터의의 선형 Motion Model과 Observation Model을 비선형 모델로 수정하면 다음과 같습니다.

 

• Motion model : $$ {x_t=A_{t}x_{t-1} + B_{t}u_{t} + \epsilon_{t}} \Longrightarrow x_t=g(u_{t},x_{t-1}) + \epsilon_{t}$$
• Observation model : $$ {z_t=C_{t}x_{t-1} + \delta_{t}} \Longrightarrow z_t=h(x_{t}) + \delta_{t}$$

 

Gaussian's Linear Transform is still Gaussian

칼만필터에서 가정했던 선형 모델에서 칼만필터가 효과적인 성능을 보일 수 있었던 것은 위 그림과 같이 가우시안 분포를 선형 모델의 입력으로 주게되면 가우시안 분포가 출력되기 때문입니다.

Squeeze Gaussian distribtion through the non-linear function

하지만 모델이 비선형 모델로 확장하게 되면 위 그림처럼 입력으로 가우시안 분포가 주어지더라도 결과분포는 가우시안 분포가 아니게 됩니다. 기존 칼만필터 알고리즘에서 Prediction, Correction 과정에서 가우시안의 평균\( \bar{\mu_{t}} \)와 분산\( \bar{\Sigma_{t}} \)를 Predicted Belief와 Uncertainty로 이용하기 때문에 비선형 모델에서는 정확한 상태추정을 할 수 없게 됩니다.

 

Linear Approximation, Local Linearization

칼만필터를 이용하기 위해 비선형모델을 선형모델로 근사하는 것이 EKF의 방법입니다. 비선형 함수에서도 좁은 구간에선 선형성을 띄는것을 이용하는 것입니다. Tayler Series는 어떤 함수 \( f(x) \)를 아래의 식과 같은 다항함수로 표현하는 것을 뜻합니다.

$$ f(x) = f(a) + f'(a)(x-a)+\frac{f''(a)}{2!}(x-a)^2 +\cdots + \frac{f^{n}(a)}{n!}(x-a)^n $$

\( x \) 가 \( a \)에 근사할때 위의 다항식은 원 함수 \( f(x) \)에 근사합니다. 하지만 테일러 급수를 계산하는 비용이 있기 때문에 많은 경우 1차, 혹은 2차 도함수까지 전개합니다.

 

다시 Motion, Observation model로 돌아가서 Linear Approximation을 진행하면 다음과 같습니다.

 

• Motion model : $$ g(u_{t},x_{t-1}) \approx g(u_{t},\mu_{t-1}) + \frac{\partial{g(u_{t},\mu_{t-1})}}{\partial{x_{t-1}}}(x_{t-1}-\mu_{t-1})$$
• Observation model : $$ h(x_{t}) = h(\bar{\mu_{t}}) + \frac{\partial{h(\bar{\mu_{t}})}}{\partial{x_{t}}}(x_{t}-\bar{\mu_{t}}) $$

위 비선형 함수들의 편미분은 Jacobian Matrix 로 \(  \frac{\partial{g(u_{t},\mu_{t-1})}}{\partial{x_{t-1}}} \)를 \( G_{t}\), \(\frac{\partial{h(\bar{\mu_{t}})}}{\partial{x_{t}}}\) 를 \(H_{t}\) 로 표기합니다. 그리고 Jacobian Matrix는 다음과 같이 계산됩니다.

Jacobian Matrix

결과적으로 Kalman Filter를 사용할때 비선형 모델에서의 결과 분포의 평균과 분산으로 만든 가우시안 분포(파란선)보다 더 잘 들어맞는 테일러 급수를 통해 선형화를 진행 후의 가우시안 분포(빨간선)를 얻을 수 있습니다.

 

이때 입력으로 주는 가우시안 분포의 분산, Uncertainty가 작으면 결과로 얻은 가우시안 분포가 원 분포 \(p(y)\) 와 더잘 들어맞고, 반대로 가우시안 분포가 넓으면 원분포와 차이가 커지게 됩니다.

 

EKF Algorithm