Chapter 12 확장 칼만 필터

칼만 필터를 비선형 시스템까지 확장하는 것이 확장 칼만 필터(Extended Kalman Filter)이다. 단 EKF는 발산할 위험이 있기 때문에 유의해서 사용해야 한다.

12.1 선형화 칼만 필터

선형화 칼만 필터는 비선형 시스템을 하나의 기준점 주위에서 선형화*시켜 얻은 선형 모델 $A$와 $H$를 사용한다.

*Tayler Expension으로 하는듯...

선형화 칼만 필터는 기준점 근처에서만 실제 시스템과 비슷한 특성을 보인다. 따라서 선형화 모델의 한계를 무시하고 선형화 칼만 필터를 적용하면 제대로 된 성능을 내지 못할 수 있다.

12.2 EKF 알고리즘

비선형 시스템 모델

다음과 같이 비선형 시스템 모델이 있다고 하자.
$x_{k+1}=f(x_k)+w_k$
$z_k=h(x_k)+v_k$

EKF 알고리즘

0. 초깃값 선정
   $\hat{x}_0$, $P_0$
1. 추정값과 오차 공분산 예측
   $\hat{x}_k^-=f(\hat{x}_{k-1})$
   $P_k^-=AP_{k-1}A^T+Q$
2. 칼만 이득 계산
   $K_k=P_k^-H^T(HP_k^-H^T+R)^{-1}$
3. 추정값 계산
   $\hat{x}_k=\hat{x}_k^-+K_k(z_k-h(\hat{x}_k^-))$
4. 오차 공분산 계산
   $P_k=P_k^--K_kHP_k^-$

EKF 알고리즘은 선형 알고리즘의 모델식 $Ax_k$와 $Hx_k$ 자리에 비선형 모델의 관계식을 사용한다.

단 시스템 행렬 $A$와 $H$는 그대로 남아있는데, 이 행렬은 시스템 모델에서 받아와야 한다. 즉, 비선형 모델을 이용해 어떻게든 선형 모델의 행렬 $A$와 $H$를 유도해야 한다.

A=\frac{\partial f}{\partial x}|_{\hat{x}_k} $H=\frac{\partial h}{\partial x}|_{\hat{x}_k} $

위 방식대로 편미분을 이용해서 선형 행렬을 구하는 방법은 선형화의 대표적인 기법이다. 즉 자코비언으로 $A$와 $H$를 구하게 된다.

12.3 예제1: 레이다 추적

레이다에서 물체까지 직선 거리를 측정해서 위치와 고도를 추정하는 문제를 풀어보자. 단, 물체는 2차원 평면 상에서 일정한 고도와 속도를 유지하면서 움직인다고 가정하자.

시스템 모델

시스템의 상태 변수를 다음과 같이 정의하자.
$x=\begin{pmatrix}x_1 \\ x_2 \\ x_3 \end{pmatrix}$

여기서 $x_1$은 수평 거리, $x_2$는 이동 속도, $x_3$는 고도라고 하자.

물제의 이동 속도와 고도는 일정하므로 시스템 모델은 다음과 같이 표현된다.

$\begin{pmatrix}\dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{pmatrix} = \begin{pmatrix}0 & 1 & 0 \\0 & 0 & 0 \\ 0 & 0 & 0\end{pmatrix}\begin{pmatrix}x_1 \\ x_2 \\ x_3\end{pmatrix}+\begin{pmatrix}0 \\ w_1 \\ w_2 \end{pmatrix}$

위 시스템 모델은 수평 거리와 이동 속도의 관계를 나타낸 것이다. 문제의 가정에서 이동 속도와 고도는 일정하다고 했으므로, $\dot{x}_2$와 $\dot{x}_3$는 이론상으로 0이 된다. 따라서 시스템 잡음 $w_1$과 $w_2$만 존재한다고 하자.

여기서 레이다가 측정하는 값은 이동 물체까지의 직선 거리, 즉 아래와 같다.

$r=\sqrt{x_1^2+x_3^2}+v:=h(x)+v$

여기서 $v$는 레이다의 측정 잡음을 의미한다.

위 시스템에서 $A$는 선형이지만, $h(x)$는 비선형이다. 따라서 칼만 필터를 바로 적용할 수 없다.

EKF 함수

위 예제에서 $A$는 선형이기 때문에 $f(x)=A$로 두면 된다. 단, 시간 간격 $dt$를 $A$에 집어넣으면,

$A=I+\begin{pmatrix}0 & 1 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0\end{pmatrix}dt$가 된다.

측정 모델의 자코비언을 계산하면 아래와 같다.
$H=\begin{pmatrix}\frac{\partial h}{\partial x_1} & \frac{\partial h}{\partial x_2} & \frac{\partial h}{\partial x_3}\end{pmatrix} = \begin{pmatrix}\frac{x_1}{\sqrt{x_1^2+x_3^2}} & 0 & \frac{x_3}{\sqrt{x_1^2+x_3^2}}\end{pmatrix}$

테스트 프로그램

(생략)