기본 칼만 필터와의 비교

칼만 필터(Kalman Filter, KF)는 선형 시스템에서의 상태 추정을 위한 기법으로 널리 사용된다. 상태 벡터는 시스템의 동적 모델을 기반으로 예측되며, 센서로부터의 측정값을 이용하여 추정된 상태를 업데이트하는 과정을 반복한다. 기본 칼만 필터는 다음과 같은 두 단계로 구성된다.

예측 단계 (Prediction Step):
시스템의 상태 모델을 통해 다음 시간 단계에서의 상태를 예측한다. 이때 시스템은 선형적 관계를 기반으로 한다. 시스템 모델은 아래와 같이 표현된다.

$\mathbf{x}_{k|k-1} = \mathbf{A} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1}$

여기서, - $\mathbf{x}_{k|k-1}$ 은 $k$ 단계에서의 예측 상태 벡터 - $\mathbf{A}$ 는 상태 전이 행렬 - $\mathbf{B}$ 는 제어 입력 행렬 - $\mathbf{u}_{k-1}$ 은 이전 시간 단계에서의 제어 입력 벡터

또한, 상태 추정의 불확실성(공분산 행렬 $\mathbf{P}$ )도 갱신된다:

$\mathbf{P}_{k|k-1} = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^\top + \mathbf{Q}$

여기서, $\mathbf{Q}$ 는 시스템 노이즈 공분산 행렬이다.

업데이트 단계 (Update Step):
센서로부터 얻은 측정값을 이용하여 예측된 상태를 수정한다. 이때, 측정 모델은 선형적 관계를 가진다:

$\mathbf{z}_{k} = \mathbf{H} \mathbf{x}_{k} + \mathbf{v}_{k}$

여기서, - $\mathbf{z}_{k}$ 는 측정 벡터 - $\mathbf{H}$ 는 측정 행렬 - $\mathbf{v}_{k}$ 는 측정 잡음 벡터 (보통 평균이 0인 가우시안 잡음으로 가정)

칼만 이득(Kalman Gain) $\mathbf{K}_k$ 을 계산하고, 이를 통해 상태 벡터와 공분산 행렬을 업데이트한다:

$\mathbf{K}_{k} = \mathbf{P}_{k|k-1} \mathbf{H}^\top \left( \mathbf{H} \mathbf{P}_{k|k-1} \mathbf{H}^\top + \mathbf{R} \right)^{-1}$

$\mathbf{x}_{k} = \mathbf{x}_{k|k-1} + \mathbf{K}_{k} \left( \mathbf{z}_{k} - \mathbf{H} \mathbf{x}_{k|k-1} \right)$

$\mathbf{P}_{k} = \left( \mathbf{I} - \mathbf{K}_{k} \mathbf{H} \right) \mathbf{P}_{k|k-1}$

여기서, $\mathbf{R}$ 은 측정 잡음 공분산 행렬, $\mathbf{I}$ 는 단위 행렬이다.

확장 칼만 필터의 필요성

기본 칼만 필터는 시스템이 선형적일 때만 사용할 수 있다. 그러나 실제 세계의 대부분의 시스템은 비선형적인 특성을 가지며, 이러한 경우 기본 칼만 필터를 적용할 수 없다. 예를 들어, 로봇의 위치 추정 문제에서 센서 데이터와 상태 변수 간의 관계는 종종 비선형적이다. 따라서, 비선형 시스템에서 사용할 수 있는 확장 칼만 필터(Extended Kalman Filter, EKF)가 필요하다.

확장 칼만 필터의 개요

확장 칼만 필터(EKF)는 기본 칼만 필터의 비선형 확장 버전으로, 비선형 시스템에서도 상태 추정을 가능하게 한다. EKF는 비선형 시스템의 상태 전이 및 측정 모델을 선형화하는 방법을 사용한다. 이를 위해 테일러 급수의 1차 항을 사용하여 선형 근사화를 수행하고, 이를 바탕으로 칼만 필터 알고리즘을 적용한다.

확장 칼만 필터에서 사용되는 시스템 모델은 다음과 같이 비선형이다:

상태 전이 모델 (State Transition Model):

$\mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k$

여기서, $\mathbf{f}(\mathbf{x}_k, \mathbf{u}_k)$ 는 비선형 함수이며, $\mathbf{w}_k$ 는 시스템 노이즈을 나타낸다.

측정 모델 (Measurement Model):

$\mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k$

여기서, $\mathbf{h}(\mathbf{x}_k)$ 는 상태 벡터 $\mathbf{x}_k$ 와 측정 벡터 $\mathbf{z}_k$ 간의 비선형 관계를 나타내며, $\mathbf{v}_k$ 는 측정 잡음이다.

비선형 시스템에서의 선형화

비선형 시스템에서 칼만 필터를 적용하려면, 비선형 함수 $\mathbf{f}$ 와 $\mathbf{h}$ 를 선형화해야 한다. 이를 위해 EKF는 테일러 급수의 1차 항을 사용하여 선형 근사를 수행한다. 이 과정에서 야코비 행렬을 계산하여 시스템을 선형화한다.

상태 전이의 선형화:

$\mathbf{A}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \mathbf{x}_{k|k-1}}$

여기서 $\mathbf{A}_k$ 는 상태 전이 행렬의 야코비이다.

측정의 선형화:

$\mathbf{H}_k = \frac{\partial \mathbf{h}}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \mathbf{x}_{k|k-1}}$

$\mathbf{H}_k$ 는 측정 모델의 야코비 행렬이다.

확장 칼만 필터의 예측 단계

EKF의 예측 단계는 비선형 상태 전이 함수 $\mathbf{f}(\mathbf{x}_k, \mathbf{u}_k)$ 를 사용하여 상태 벡터와 공분산 행렬을 예측하는 과정이다. 비선형 시스템의 경우 다음과 같은 수식을 사용한다:

상태 벡터 예측:

$\mathbf{x}_{k+1|k} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k)$

오차 공분산 행렬 예측:

$\mathbf{P}_{k+1|k} = \mathbf{A}_k \mathbf{P}_{k} \mathbf{A}_k^\top + \mathbf{Q}_k$

여기서, $\mathbf{Q}_k$ 는 시스템 노이즈 공분산 행렬이다.

확장 칼만 필터의 업데이트 단계

업데이트 단계에서는 측정값을 기반으로 상태 벡터와 오차 공분산 행렬을 갱신한다. 비선형 측정 모델을 선형화한 후 칼만 이득을 계산하고 상태를 업데이트하는 과정은 다음과 같다:

칼만 이득 계산:

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

상태 벡터 업데이트:

$\mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + \mathbf{K}_k \left( \mathbf{z}_k - \mathbf{h}(\mathbf{x}_{k|k-1}) \right)$

오차 공분산 행렬 업데이트:

$\mathbf{P}_{k|k} = \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_{k|k-1}$

이와 같이, 확장 칼만 필터는 비선형 시스템에서의 상태 추정을 위해 선형 근사화를 사용하며, 기본 칼만 필터와 유사한 형태로 예측 및 업데이트 단계를 수행한다. 비선형 시스템에 적합한 확장된 형태를 제공하면서도, 기본 칼만 필터의 이론적 기초를 유지하는 것이 특징이다.