[칼만필터] Chap 12. 확장칼만필터

 칼만필터가 나온지 오래되었지만, 아직도 사용되고 개발되고 있는 이유는 비선형시스템에 적용하기 위해서 계속 연구되고있는 것이다. 확장칼만필터(Extended Kalman Filter : EKF)는 비선형시스템까지도 확장하였기 때문에 칼만필터가 널리 확산될 수 있었다.

 

EKF의 단점: 알고리즘이 발산할 위험이 있다. 

 

12.1 선형화칼만필터

 

이제까지 배웠던 것은 선형 칼만필터(Linear Kalman Filter) 이다.

선형화 칼만필터(Linearized Kalman Filter)는 선형과 시스템모델에서 차이가 있다. 

 

선형칼만필터는 시스템모델이 선형

선형화칼만필터는 비선형시스템을 기준점 주위에서 선형화시켜 얻은 시스템모델A, H를 얻어낸다. 

실제 구현하는 것에 대해서는 선형, 선형화는 차이가 없다. 하지만 선형화모델은 기준점에서 선형화하였기 때문에 기준점 근처에서만 실제 시스템과 비슷하게 동작하고, 기준을 벗어나면 더이상 믿을 수가 없는 시스템이 된다는 사실이다. 

결국 선형화칼만필터의 핵심은 비선형시스템의 선형화 모델이며, 선형화모델의 유효범위내에서 사용되어야한다. 

 

12.2 EKF 알고리즘

원래 선형칼만필터에서는 이거였다. 

\(x_{k+1} = A x_{k} + w_{k}\)

\(z_{k} = H x_{k} + v_{k} \)

 

 

EKF에서는 다음과 같다. 

\(x_{k+1} = f(x_{k}) + w_{k}\)

\(z_{k} = h(x_{k}) + v_{k} \)

 

딱봐도 시스템모델이 이제는 측정값에 따라서 바뀌는 함수로 되어있다. 

즉, 식 자체부터 이제는 비선형시스템이라는 것이다. 

 

Chap 4.에서 배웠던 선형칼만필터 전체 흐름도에서도 변경된다. 

 

\(A x_{k} \leftrightarrow f(x_k) \)

\(H x_{k} \leftrightarrow h(x_k) \)

 

 

 

EKF는 여기 이식에서 시스템모델을 받아와야한다.

\(x_{k+1} = f(x_{k}) + w_{k}\)

\(z_{k} = h(x_{k}) + v_{k} \)

 

근데 이식에서는 A,H가 보이지 않는다. 

비선형모델을 선형화 시켜 얻은 선형모델을 사용한다. 

 

\(A \equiv \frac{\partial f}{\partial x} |_{\hat{x}_{k}}  , H \equiv  \frac{\partial h}{\partial x}  |_{\hat{x}_{k}}\)

 

\( \frac{\partial f}{\partial x}\) 는 함수f를 x에 대해서 편미분한 것.f,x가 벡터면 행렬(= jacobian)

 

\( \frac{\partial f}{\partial x} |_{\hat{x}_{k}} \) 는 행렬에 \(hat{x}_k\)를 대입해서 계산한 것

 

위의 비선형모델을 편미분해서 선형행렬을 구하는 것이 선형화의 대표적인 기법이다. 

 

특수하게 선형화의 기준을 알고있거나, 미리 정해져있는 운동을 하는 경우, 선형화칼만필터를 사용하면 된다. 

하지만 EKF의 경우 직전 추정값\(\hat{x}_k])을 기준으로 매번 새로운 선형모델을 구한다. 

선형화의 기준점을 사전에 설정하기어려운 시스템에서는 EKF를 사용하면 되겠다. 

 

요약

EKF는 비선형모델식 \(f(x_k), h(x_k) \)를 사용한다. 

EKF는 비선형모델의 야코비안(jacobian)으로 A,H를 계산한다. 

야코비안 행렬은 직전 추정값(\(\hat{x}_k\))를 기준으로 계산한다. 

EKF는 직전 추정값을 기준으로 선형모델을 계산한다. 

 

12.3 레이다 추적 예제

레이더에서 물체와의 직선거리를 측정하고있음. 
목표물체는 일정한 고도와 속도를 유지하면서 움직이고있음.(인공위성처럼?)
고도 1km, 100m/s의 속도로 이동중 

 

\(x_1\) 레이더에서 물체까지 수평거리
\(x_3\) 물체의 고도

시스템 모델

\(r\) 물체까지의 직선거리 
\(v\) 레이더의 측정잡음

 

일단 비선형이면 해당 모델행렬은 Jacobian을 수행해줘야한다. 

 

 

 

 

12.4 기울기 자세 추적하기

앞장에서 배웠던 오일러각도 식을 그대로 칼만필터에 적용해본다. 

지난번에는 쿼터니언으로 바꿔서 했지만, 지금은 비선형식을 그대로 사용한다. 

EKF에서는 어지간한 비선형시스템에서도 가능하다. 

발산할 염려도 있지만, 쓸만하다. 

 

 

여기서는 시스템모델을 Jacobian하고, discrete까지 해줘야한다.

 

 

 

결과는 비슷하고, EKF가 좀 더 범용성이 있다. 

비선형에서도 잘 동작하기 때문에... 물론 초기값 선정이 중요하다. 

초기값 오차가 크면 선형모델과 실제시스템이 달라지면서 발산하게