확장 칼만 필터의 개념

확장 칼만 필터(Extended Kalman Filter, EKF)는 비선형 시스템에서 상태 추정을 수행하기 위한 방법이다. 기본 칼만 필터는 선형 시스템에 한정되지만, 실제 많은 시스템은 비선형적인 특성을 가진다. 이러한 경우, 칼만 필터는 적용하기 어렵기 때문에 확장 칼만 필터가 도입되었다. EKF는 비선형 시스템을 선형화하여 칼만 필터의 적용 범위를 확장하는 방식으로 동작한다.

비선형 시스템의 상태 방정식과 관측 방정식은 다음과 같이 표현될 수 있다.

\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1}
\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k

여기서: - \mathbf{x}_k는 k 시점에서의 상태 벡터이다. - f(\cdot)는 상태 전이 함수로, 시스템의 비선형성을 나타낸다. - \mathbf{u}_k는 k 시점에서의 입력 벡터이다. - \mathbf{w}_k는 시스템 노이즈(process noise)이며, 통상적으로 가우시안 분포를 따른다고 가정한다. - \mathbf{z}_k는 k 시점에서의 관측 벡터이다. - h(\cdot)는 관측 함수로, 관측의 비선형성을 나타낸다. - \mathbf{v}_k는 측정 잡음(measurement noise)이다.

선형화 과정

EKF의 핵심은 비선형 상태 전이 함수 f(\cdot)와 관측 함수 h(\cdot)를 선형화하는 것이다. 이 과정은 테일러 급수를 사용한 1차 근사로 이루어지며, 이를 통해 선형 칼만 필터의 구조를 유지하면서 비선형 시스템에 적용할 수 있다.

우선, 상태 전이 함수 f(\cdot)의 선형화는 상태 벡터 \mathbf{x}_{k-1}에서의 야코비 행렬 \mathbf{F}_{k-1}을 사용하여 다음과 같이 근사할 수 있다:

\mathbf{F}_{k-1} = \frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \mathbf{x}_{k-1}}

마찬가지로, 관측 함수 h(\cdot)의 선형화는 상태 벡터 \mathbf{x}_k에서의 야코비 행렬 \mathbf{H}_k을 사용하여 근사한다:

\mathbf{H}_k = \frac{\partial h(\mathbf{x})}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \mathbf{x}_k}

이러한 선형화 과정을 통해 EKF는 기존의 선형 칼만 필터에서 사용하는 예측 및 갱신 과정과 동일한 구조를 따르면서도 비선형 시스템에서의 상태 추정을 수행할 수 있다.

예측 단계

확장 칼만 필터에서 예측 단계는 상태 벡터와 오차 공분산 행렬을 예측하는 과정이다. 비선형 시스템의 경우, 상태 전이 함수 f(\cdot)를 사용하여 상태 벡터를 예측한다. 이 과정에서, 잡음 \mathbf{w}_{k-1}의 영향을 고려하여 오차 공분산 행렬도 함께 계산한다.

우선, 상태 벡터의 예측은 다음과 같이 이루어진다:

\hat{\mathbf{x}}_k^- = f(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1})

여기서: - \hat{\mathbf{x}}_k^-는 k 시점에서 예측된 상태 벡터이다. - \hat{\mathbf{x}}_{k-1}는 k-1 시점에서의 추정된 상태 벡터이다.

오차 공분산 행렬 \mathbf{P}_k^-는 상태 전이 함수 f(\cdot)의 야코비 \mathbf{F}_{k-1}과 시스템 노이즈 공분산 행렬 \mathbf{Q}_{k-1}을 이용하여 다음과 같이 계산한다:

\mathbf{P}_k^- = \mathbf{F}_{k-1} \mathbf{P}_{k-1} \mathbf{F}_{k-1}^\top + \mathbf{Q}_{k-1}

여기서: - \mathbf{P}_k^-는 k 시점에서 예측된 오차 공분산 행렬이다. - \mathbf{P}_{k-1}는 k-1 시점에서의 오차 공분산 행렬이다. - \mathbf{Q}_{k-1}는 시스템 노이즈 공분산 행렬이다.

이 예측 단계에서 상태와 오차 공분산이 각각 비선형 함수 f(\cdot)에 의해 업데이트되며, 이를 통해 미래의 상태를 예측한다.

갱신 단계

갱신 단계에서는 새로운 관측값을 이용해 예측된 상태를 보정한다. 관측 함수 h(\cdot)의 비선형성을 고려하여 상태 벡터와 오차 공분산 행렬을 수정하게 된다.

관측값에 대한 예측은 다음과 같이 계산된다:

\hat{\mathbf{z}}_k = h(\hat{\mathbf{x}}_k^-)

여기서: - \hat{\mathbf{z}}_k는 관측 함수 h(\cdot)를 이용하여 예측된 관측값이다.

오차 벡터는 실제 관측값 \mathbf{z}_k와 예측된 관측값 \hat{\mathbf{z}}_k의 차이로 정의된다:

\mathbf{y}_k = \mathbf{z}_k - \hat{\mathbf{z}}_k

이후, 칼만 이득(Kalman Gain) \mathbf{K}_k을 계산하는데, 이는 관측의 신뢰도를 반영하여 상태를 업데이트하는 데 사용된다. 칼만 이득은 다음과 같이 계산된다:

\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^\top \left( \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^\top + \mathbf{R}_k \right)^{-1}

여기서: - \mathbf{K}_k는 k 시점에서의 칼만 이득이다. - \mathbf{R}_k는 관측 잡음 공분산 행렬이다.

최종적으로, 상태 벡터는 다음과 같이 업데이트된다:

\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \mathbf{y}_k

오차 공분산 행렬도 칼만 이득을 이용하여 업데이트된다:

\mathbf{P}_k = \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_k^-

이러한 갱신 과정을 통해 예측된 상태가 실제 관측값을 반영하여 더욱 정확해진다.