확장 칼만 필터에서의 오차 공분산

확장 칼만 필터에서 오차 공분산의 전파는 필터의 성능을 좌우하는 중요한 단계이다. 이 과정은 상태 추정에서 발생하는 오차를 어떻게 모델링하고 예측할 것인가를 다룬다. 먼저, 오차 공분산 행렬의 의미를 이해하고 확장 칼만 필터에서 이 행렬이 어떻게 시간에 따라 전파되는지 알아야 한다.

오차 공분산 행렬의 정의

오차 공분산 행렬은 상태 추정값과 실제 상태 사이의 오차 분포를 설명한다. 상태 추정의 오차 공분산 행렬을 \mathbf{P}로 나타낸다면, 이는 다음과 같이 정의된다:

\mathbf{P} = \mathbb{E}\left[(\mathbf{x} - \hat{\mathbf{x}})(\mathbf{x} - \hat{\mathbf{x}})^\top\right]

여기서: - \mathbf{x}는 실제 상태 벡터 - \hat{\mathbf{x}}는 추정된 상태 벡터 - \mathbb{E}[\cdot]는 기댓값을 나타냄 - \mathbf{P}는 오차 공분산 행렬

예측 단계에서의 오차 공분산의 전파

예측 단계에서 오차 공분산 행렬 \mathbf{P}는 시스템 모델의 선형화에 의해 업데이트된다. 확장 칼만 필터에서는 비선형 시스템을 처리하기 때문에 시스템 동역학 모델을 선형화한 후 이를 이용해 오차 공분산을 전파한다.

비선형 시스템의 상태 전이 방정식을 다음과 같이 정의할 수 있다:

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

여기서: - \mathbf{x}_k는 k번째 시간 단계에서의 상태 벡터 - f(\cdot)는 비선형 상태 전이 함수 - \mathbf{u}_k는 k번째 시간 단계에서의 입력 벡터 - \mathbf{w}_k는 시스템 노이즈 벡터로, \mathbf{Q}_k 공분산을 가짐

야코비을 이용한 선형화

확장 칼만 필터에서는 비선형 함수 f(\mathbf{x}, \mathbf{u})를 선형화하기 위해 야코비 행렬 \mathbf{F}_k를 사용한다. \mathbf{F}_k는 상태 전이 함수 f를 상태 벡터 \mathbf{x}에 대해 미분하여 구할 수 있으며, 이는 다음과 같이 정의된다:

\mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \Bigg|_{\mathbf{x} = \hat{\mathbf{x}}_k}

따라서, 예측 단계에서 오차 공분산 행렬 \mathbf{P}_{k+1}는 다음과 같이 전파된다:

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

여기서: - \mathbf{P}_k는 k번째 시간 단계에서의 오차 공분산 행렬 - \mathbf{F}_k는 상태 전이 함수의 야코비 행렬 - \mathbf{Q}_k는 시스템 노이즈 공분산 행렬

이 식은 시스템의 동역학 모델이 오차 공분산을 어떻게 변화시키는지를 나타내며, 특히 시스템의 불확실성이 시간에 따라 어떻게 증가하는지를 반영한다.

업데이트 단계에서의 오차 공분산 전파

업데이트 단계에서는 측정값을 바탕으로 상태 추정을 보정하며, 이 과정에서도 오차 공분산 행렬이 전파된다. 상태 추정의 정확도를 높이기 위해, 측정값과 상태 추정값 사이의 차이를 반영하는 칼만 이득을 계산하고 이를 통해 상태를 갱신한다.

비선형 측정 모델을 다음과 같이 정의할 수 있다:

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

여기서: - \mathbf{z}_k는 측정 벡터 - h(\cdot)는 비선형 측정 함수 - \mathbf{v}_k는 측정 노이즈 벡터로, \mathbf{R}_k 공분산을 가짐

야코비 행렬을 이용한 선형화

비선형 측정 모델을 선형화하기 위해 측정 함수 h(\mathbf{x})에 대해 상태 벡터 \mathbf{x}의 야코비 행렬 \mathbf{H}_k를 구한다. 이는 다음과 같이 정의된다:

\mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}} \Bigg|_{\mathbf{x} = \hat{\mathbf{x}}_k}

칼만 이득의 계산

칼만 이득 \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는 칼만 이득 - \mathbf{H}_k는 측정 모델의 야코비 행렬 - \mathbf{R}_k는 측정 노이즈 공분산 행렬

상태 및 오차 공분산의 갱신

상태 벡터 \hat{\mathbf{x}}_k가 갱신되면, 오차 공분산 행렬도 다음과 같이 갱신된다:

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

여기서: - \mathbf{I}는 항등 행렬 - \mathbf{K}_k \mathbf{H}_k는 칼만 필터가 측정 오차를 보정하는 정도를 나타냄

갱신된 오차 공분산 행렬은 상태 추정이 보정된 후의 불확실성을 나타내며, 측정값을 통해 상태 추정이 얼마나 개선되었는지 보여준다.

시스템 노이즈와 측정 노이즈의 영향

오차 공분산의 전파 과정에서 중요한 두 요소는 시스템 노이즈와 측정 노이즈이다. 시스템 노이즈 공분산 \mathbf{Q}_k와 측정 노이즈 공분산 \mathbf{R}_k는 상태 추정의 정확도와 필터 성능에 큰 영향을 미친다.

시스템 노이즈와 측정 노이즈를 적절히 설정하는 것은 확장 칼만 필터의 성능을 최적화하는 데 중요한 역할을 한다.