업데이트 단계 - 실험 도서관

확장 칼만 필터의 업데이트 단계에서는 새로운 측정값을 기반으로 상태를 업데이트한다. 먼저, 측정 모델을 정의해야 한다. 측정값 $\mathbf{z}_k$ 는 일반적으로 다음과 같이 표현된다.

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

여기서 $h(\mathbf{x}_k)$ 는 비선형 측정 함수이고, $\mathbf{v}_k$ 는 측정 노이즈이다. 측정 노이즈는 공분산이 $\mathbf{R}_k$ 인 가우시안 분포를 따른다.

비선형 시스템에서는 측정 함수 $h(\mathbf{x}_k)$ 가 선형이 아니므로, 이를 선형화하기 위해 테일러 급수를 사용한다. 이때 야코비 행렬 $\mathbf{H}_k$ 는 다음과 같이 정의된다.

$\mathbf{H}_k = \frac{\partial h(\mathbf{x})}{\partial \mathbf{x}} \bigg|_{\mathbf{x} = \hat{\mathbf{x}}_{k}^{-}}$

여기서 $\hat{\mathbf{x}}_{k}^{-}$ 는 업데이트 전의 추정 상태이다. 야코비 행렬 $\mathbf{H}_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{P}_k^{-}$ 는 업데이트 전의 오차 공분산 행렬이다. - $\mathbf{R}_k$ 는 측정 노이즈 공분산 행렬이다. - $\mathbf{H}_k$ 는 위에서 계산한 야코비 행렬이다.

칼만 이득은 측정값과 예측값 사이의 불확실성을 조정하는 역할을 한다.

칼만 이득 $\mathbf{K}_k$ 를 사용하여 예측된 상태를 측정값을 기반으로 업데이트한다. 업데이트된 상태 $\hat{\mathbf{x}}_k$ 는 다음과 같이 계산된다.

$\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^{-} + \mathbf{K}_k \left( \mathbf{z}_k - h(\hat{\mathbf{x}}_k^{-}) \right)$

여기서: - $\hat{\mathbf{x}}_k^{-}$ 는 예측된 상태이다. - $\mathbf{z}_k$ 는 실제 측정값이다. - $h(\hat{\mathbf{x}}_k^{-})$ 는 예측된 상태에서의 측정값이다.

이 수식은 예측된 상태와 실제 측정값 간의 차이, 즉 잔차(residual)를 기반으로 상태를 보정하는 과정이다. 잔차는 다음과 같이 정의된다.

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

잔차가 크면, 상태 추정이 측정값에 더 많이 의존하게 되고, 잔차가 작으면 예측된 상태를 더 신뢰하게 된다.

업데이트된 상태 추정과 함께, 오차 공분산 행렬 $\mathbf{P}_k$ 도 업데이트된다. 이는 추정된 상태의 불확실성을 나타내며, 다음과 같은 식으로 계산된다.

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

여기서: - $\mathbf{I}$ 는 항등 행렬이다. - $\mathbf{K}_k$ 는 칼만 이득이다. - $\mathbf{H}_k$ 는 측정 모델의 야코비 행렬이다. - $\mathbf{P}_k^{-}$ 는 업데이트 전의 오차 공분산 행렬이다.

이 수식은 업데이트 후 상태의 불확실성을 줄이는 과정이다. $\mathbf{K}_k \mathbf{H}_k$ 항은 측정값이 반영된 후 상태 추정의 불확실성을 줄이는 역할을 한다.