확장 칼만 필터의 기본 구조

확장 칼만 필터(EKF)는 두 가지 주요 단계로 나눌 수 있다: 예측 단계와 업데이트 단계. 이 과정에서 상태 변수와 측정값을 기반으로 시스템의 상태를 지속적으로 추정하게 된다.

1. 초기화

확장 칼만 필터에서 먼저 상태 벡터 \mathbf{x}와 공분산 행렬 \mathbf{P}를 초기화해야 한다. 여기서 \mathbf{x}는 시스템의 초기 상태를 나타내고, \mathbf{P}는 초기 상태에 대한 불확실성을 나타낸다.

초기 상태 벡터 \mathbf{x}_0는 다음과 같이 정의된다:

\mathbf{x}_0 = \begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix}

공분산 행렬 \mathbf{P}_0는 다음과 같이 대각 성분에 초기 불확실성을 반영한다:

\mathbf{P}_0 = \begin{bmatrix} p_{11} & 0 & \cdots & 0 \\ 0 & p_{22} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & p_{nn} \end{bmatrix}

2. 예측 단계

예측 단계에서는 상태 전이 함수 \mathbf{f}(\mathbf{x})를 통해 다음 상태를 예측한다. 이때, 시스템의 동역학을 나타내는 함수 \mathbf{f}(\mathbf{x})는 일반적으로 비선형이다. 상태 예측은 다음과 같이 이루어진다:

\mathbf{x}_{k|k-1} = \mathbf{f}(\mathbf{x}_{k-1})

공분산 행렬의 예측은 야코비 행렬 \mathbf{F}를 사용하여 계산된다. 야코비 행렬은 상태 전이 함수 \mathbf{f}(\mathbf{x})의 편미분으로 구성되며 다음과 같이 정의된다:

\mathbf{F} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \Bigg|_{\mathbf{x}_{k-1}}

예측된 공분산 행렬 \mathbf{P}_{k|k-1}는 다음과 같이 계산된다:

\mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1} \mathbf{F}_{k-1}^T + \mathbf{Q}

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

3. 업데이트 단계

업데이트 단계에서는 측정값 \mathbf{z}_k를 통해 상태를 보정한다. 측정 함수 \mathbf{h}(\mathbf{x}) 역시 비선형일 수 있으며, 상태 벡터를 측정값 공간으로 사상하는 역할을 한다. 측정 예측은 다음과 같이 이루어진다:

\mathbf{z}_{k|k-1} = \mathbf{h}(\mathbf{x}_{k|k-1})

측정 오차 \mathbf{y}_k는 실제 측정값과 예측 측정값의 차이로 정의된다:

\mathbf{y}_k = \mathbf{z}_k - \mathbf{z}_{k|k-1}

측정 오차 공분산 행렬 \mathbf{S}_k는 야코비 행렬 \mathbf{H}_k와 측정 노이즈 공분산 행렬 \mathbf{R}_k를 사용하여 계산된다:

\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k

4. 칼만 이득 계산

칼만 이득 \mathbf{K}_k는 상태 업데이트에서 측정 오차를 얼마나 반영할지 결정하는 요소이다. 칼만 이득은 다음과 같이 계산된다:

\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}

5. 상태 및 공분산 업데이트

최종적으로 상태 벡터는 칼만 이득과 측정 오차를 사용하여 업데이트된다:

\mathbf{x}_k = \mathbf{x}_{k|k-1} + \mathbf{K}_k \mathbf{y}_k

공분산 행렬은 다음과 같이 업데이트된다:

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

예제 코드: 확장 칼만 필터 구현

아래는 확장 칼만 필터(EKF)를 Python으로 구현한 예제 코드이다. 이 코드는 위에서 설명한 EKF의 예측 및 업데이트 단계를 포함하고 있다. 구체적인 수학적 알고리즘을 코드로 변환하여 직관적으로 이해할 수 있도록 작성되었다.

1. 상태 및 공분산 초기화

먼저, 상태 벡터와 공분산 행렬을 초기화한다.

import numpy as np

# 상태 벡터 초기화 (예: 위치와 속도)
x = np.array([0, 0, 0, 0])  # [x_position, y_position, x_velocity, y_velocity]

# 공분산 행렬 초기화
P = np.eye(4) * 1000  # 초기 불확실성이 크기 때문에 대각 성분을 크게 설정

# 시스템 노이즈 공분산 행렬 Q 초기화
Q = np.eye(4) * 0.1

# 측정 노이즈 공분산 행렬 R 초기화
R = np.eye(2) * 5  # 측정값은 위치만 포함한다고 가정

2. 예측 단계

다음은 상태 전이 함수와 공분산 행렬을 사용하여 예측을 수행하는 부분이다.

# 상태 전이 함수 정의 (여기서는 단순한 선형 이동 모델)
def f(x):
    F = np.array([[1, 0, 1, 0], 
                  [0, 1, 0, 1], 
                  [0, 0, 1, 0], 
                  [0, 0, 0, 1]])
    return F @ x

# 예측 단계
def predict(x, P, Q):
    # 상태 예측
    x_pred = f(x)

    # 상태 전이 함수의 야코비 행렬
    F = np.array([[1, 0, 1, 0], 
                  [0, 1, 0, 1], 
                  [0, 0, 1, 0], 
                  [0, 0, 0, 1]])

    # 공분산 행렬 예측
    P_pred = F @ P @ F.T + Q

    return x_pred, P_pred

# 예측 수행
x_pred, P_pred = predict(x, P, Q)

3. 업데이트 단계

다음은 측정값을 바탕으로 상태를 업데이트하는 부분이다.

# 측정 함수 정의 (위치만 측정하는 경우)
def h(x):
    return np.array([x[0], x[1]])  # 측정값은 위치(x_position, y_position)

# 측정 함수의 야코비 행렬
H = np.array([[1, 0, 0, 0], 
              [0, 1, 0, 0]])

# 측정값 z (실제 측정된 값)
z = np.array([2, 3])  # 예: 측정된 위치가 [2, 3]

# 칼만 이득 계산 및 상태 업데이트
def update(x_pred, P_pred, z, H, R):
    # 측정값 예측
    z_pred = h(x_pred)

    # 측정 오차
    y = z - z_pred

    # 측정 오차 공분산 계산
    S = H @ P_pred @ H.T + R

    # 칼만 이득 계산
    K = P_pred @ H.T @ np.linalg.inv(S)

    # 상태 업데이트
    x_update = x_pred + K @ y

    # 공분산 업데이트
    P_update = (np.eye(len(x_pred)) - K @ H) @ P_pred

    return x_update, P_update

# 상태 및 공분산 업데이트 수행
x_update, P_update = update(x_pred, P_pred, z, H, R)

위의 코드는 간단한 선형 모델을 바탕으로 확장 칼만 필터를 구현한 예시이다. 비선형 시스템에 대한 실제 확장 칼만 필터는 상태 전이 함수 \mathbf{f}(\mathbf{x})와 측정 함수 \mathbf{h}(\mathbf{x})가 비선형이므로, 야코비 행렬을 구하는 과정이 필수적이다.