예측 단계

확장 칼만 필터(EKF)의 첫 번째 단계는 예측(prediction) 단계이다. 이 단계에서는 시스템의 현재 상태를 바탕으로 다음 상태를 예측한다. 비선형 시스템에서 상태의 변화는 일반적으로 다음과 같은 상태 방정식으로 표현된다:

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

여기서: - \mathbf{x}_{k}는 시점 k에서의 상태 벡터이다. - f(\cdot)는 비선형 상태 전이 함수이다. - \mathbf{u}_{k-1}는 시점 k-1에서의 제어 입력이다. - \mathbf{w}_{k-1}는 시점 k-1에서의 시스템 노이즈로, 평균이 0이고 공분산이 \mathbf{Q}_{k-1}인 가우시안 노이즈로 가정한다.

예측된 상태 \hat{\mathbf{x}}_{k|k-1}는 다음과 같이 구할 수 있다:

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

이때, 상태 추정값은 \hat{\mathbf{x}}_{k-1|k-1}으로, 이전 시간 시점 k-1에서의 최적 상태 추정값이다.

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

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

여기서: - \mathbf{F}_{k-1} = \frac{\partial f}{\partial \mathbf{x}} \Bigg|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1}}는 상태 전이 함수의 야코비 행렬이다. - \mathbf{P}_{k-1|k-1}는 이전 시점의 오차 공분산 행렬이다. - \mathbf{Q}_{k-1}는 시스템 노이즈의 공분산 행렬이다.

측정 업데이트 단계

측정(update) 단계에서는 새로운 측정값을 바탕으로 상태 추정값을 갱신한다. 측정은 일반적으로 비선형 방정식으로 나타낼 수 있다:

\mathbf{z}_{k} = h(\mathbf{x}_{k}) + \mathbf{v}_{k}

여기서: - \mathbf{z}_{k}는 시점 k에서의 측정 벡터이다. - h(\cdot)는 상태와 측정값을 연결하는 비선형 측정 함수이다. - \mathbf{v}_{k}는 시점 k에서의 측정 노이즈로, 평균이 0이고 공분산이 \mathbf{R}_{k}인 가우시안 노이즈로 가정한다.

상태 추정값을 갱신하기 위한 칼만 이득(Kalman gain) \mathbf{K}_{k}는 다음과 같이 계산된다:

\mathbf{K}_{k} = \mathbf{P}_{k|k-1} \mathbf{H}_{k}^T (\mathbf{H}_{k} \mathbf{P}_{k|k-1} \mathbf{H}_{k}^T + \mathbf{R}_{k})^{-1}

여기서: - \mathbf{H}_{k} = \frac{\partial h}{\partial \mathbf{x}} \Bigg|_{\hat{\mathbf{x}}_{k|k-1}}는 측정 함수의 야코비 행렬이다. - \mathbf{R}_{k}는 측정 노이즈의 공분산 행렬이다.

갱신된 상태 추정값 \hat{\mathbf{x}}_{k|k}는 다음과 같이 계산된다:

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

또한, 갱신된 오차 공분산 행렬 \mathbf{P}_{k|k}는 다음과 같이 구할 수 있다:

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

알고리즘의 수렴 조건

확장 칼만 필터가 정상적으로 작동하기 위해서는 몇 가지 수렴 조건이 필요하다. 이러한 조건들은 필터가 상태를 정확하게 추정하고 오차 공분산 행렬이 수렴하도록 보장하는 데 필수적이다.

1. 상태 전이 모델과 측정 모델의 적절한 선형화

확장 칼만 필터는 비선형 시스템을 선형화하여 상태를 추정한다. 이 과정에서 야코비 행렬을 계산하는데, 이 선형화 과정이 적절하지 않으면 필터의 성능이 저하될 수 있다. 따라서, 선형화는 시스템의 실제 동작을 충분히 근사할 수 있을 정도로 정확해야 한다. 선형화가 과도하게 단순화될 경우, 필터는 수렴하지 않거나 부정확한 추정을 제공할 수 있다.

2. 초기 추정값의 설정

확장 칼만 필터의 성능은 초기 추정값에 크게 의존한다. 초기 상태 추정값 \hat{\mathbf{x}}_{0|0}과 초기 오차 공분산 행렬 \mathbf{P}_{0|0}이 잘못 설정되면, 필터가 불안정해지거나 수렴하지 않을 수 있다. 초기화는 가능한 한 실제 상태에 근접하게 설정해야 하며, 초기 오차 공분산은 불확실성에 비례해 설정하는 것이 중요하다.

3. 프로세스 및 측정 노이즈의 공분산 설정

확장 칼만 필터에서 중요한 파라미터 중 하나는 시스템 노이즈 공분산 행렬 \mathbf{Q}_{k}와 측정 노이즈 공분산 행렬 \mathbf{R}_{k}의 설정이다. 이 행렬들이 적절하게 설정되지 않으면, 필터가 수렴하지 않거나 오차가 커질 수 있다. 일반적으로 \mathbf{Q}_{k}와 \mathbf{R}_{k}는 시스템의 물리적인 특성을 기반으로 설정된다. 시스템의 불확실성이 클수록 이 값들은 크게 설정되며, 불확실성이 적을수록 작게 설정된다.

확장 칼만 필터의 상태 추정 흐름

확장 칼만 필터의 전체 흐름은 다음과 같은 과정을 따른다:

  1. 초기화: 초기 상태 추정값 \hat{\mathbf{x}}_{0|0}과 오차 공분산 행렬 \mathbf{P}_{0|0}를 설정한다.
  2. 예측: 상태 방정식을 사용하여 다음 시간 스텝의 상태 \hat{\mathbf{x}}_{k|k-1}를 예측하고, 오차 공분산 행렬 \mathbf{P}_{k|k-1}을 계산한다.
  3. 측정: 측정값 \mathbf{z}_{k}가 들어오면, 칼만 이득 \mathbf{K}_{k}을 계산하여 상태 추정값을 갱신한다.
  4. 갱신: 갱신된 상태 추정값 \hat{\mathbf{x}}_{k|k}과 오차 공분산 행렬 \mathbf{P}_{k|k}을 계산하여 최적 상태 추정을 수행한다.

이 과정을 매 시간 스텝마다 반복하여 필터는 지속적으로 상태를 추정하게 된다.

계산 비용과 최적화

확장 칼만 필터의 계산 비용은 상태 벡터의 차원과 비선형 시스템의 복잡도에 의해 결정된다. 특히, 야코비 행렬의 계산과 역행렬 연산은 계산 비용을 증가시키는 요인이 될 수 있다. 계산 비용을 줄이기 위한 방법으로는 다음과 같은 전략을 고려할 수 있다:

  1. 상태 벡터 축소: 불필요한 변수는 상태 벡터에서 제외하여 계산량을 줄일 수 있다.
  2. 야코비 행렬 근사: 정확한 야코비 행렬 대신 근사값을 사용할 수 있다. 다만, 이 경우 필터 성능이 저하될 수 있으므로 신중한 고려가 필요하다.
  3. 역행렬 계산 최적화: 역행렬 계산을 효율적으로 처리하기 위한 방법들을 사용할 수 있다. 예를 들어, 행렬이 대칭 행렬일 경우 치올레스키 분해(Cholesky decomposition)를 사용하면 계산 비용을 줄일 수 있다.