수치적 안정성 확보 - 소프트웨어 융합

필터의 수치적 불안정성 문제

확장 칼만 필터에서 발생할 수 있는 수치적 불안정성은 필터의 예측 및 업데이트 과정에서 발생하는 연산 오차 또는 상태 및 공분산의 지나친 변화로 인해 필터가 발산하거나 비정상적인 결과를 도출하는 상황을 말한다. 이러한 불안정성은 특히 상태 전이 행렬이나 측정 모델의 선형화 과정에서 발생하는 오차로 인해 발생할 수 있으며, 이는 필터의 성능에 치명적인 영향을 줄 수 있다.

공분산 행렬의 양의 정부호 유지

확장 칼만 필터에서 공분산 행렬 $\mathbf{P}$ 는 상태 추정의 불확실성을 나타내는 중요한 요소이다. 이 공분산 행렬은 반드시 양의 정부호(Positive Semi-Definite)를 유지해야 하는데, 이는 필터의 수치적 안정성을 보장하는 중요한 조건 중 하나이다. 그러나 수치적 오차나 잘못된 계산으로 인해 $\mathbf{P}$ 가 양의 정부호가 되지 않는 상황이 발생할 수 있다.

공분산 행렬 $\mathbf{P}$ 의 양의 정부호성을 보장하기 위한 방법 중 하나는 계산 과정에서의 수치적 오류를 최소화하는 것이다. 이를 위해 다음과 같은 기법들이 자주 사용된다.

수치적으로 안정된 행렬 연산: 공분산 행렬의 업데이트 과정에서 수치적 오차를 줄이기 위해서는 행렬 연산에 대한 수치적 안정성을 확보해야 한다. 이는 행렬 분해 방법(예: 치올레스키 분해)을 통해 공분산 행렬을 계산하거나 업데이트할 때 더 높은 수치적 안정성을 제공할 수 있다.

예를 들어, 공분산 행렬 $\mathbf{P}$ 를 업데이트하는 과정에서 직접적인 곱셈 대신 치올레스키 분해를 활용하여 다음과 같이 계산할 수 있다.

$\mathbf{P} = \mathbf{L}\mathbf{L}^{\top}$

여기서 $\mathbf{L}$ 은 공분산 행렬의 치올레스키 분해된 하삼각 행렬이다. 이를 통해 공분산 행렬의 양의 정부호성을 더 쉽게 유지할 수 있다.

작은 값 클리핑(Clipping): 공분산 행렬의 고유값 중 음수나 너무 작은 값을 가지는 요소가 있다면, 해당 값을 일정 임계치 이상으로 클리핑(잘라내는) 방법도 수치적 안정성을 높이는 데 유용하다. 이는 공분산 행렬이 음의 고유값을 갖지 않도록 보장하는 간단한 방법이다.

수치적 미분 대신 차분 기법

확장 칼만 필터는 비선형 시스템을 다루기 때문에 선형화 과정에서 수치적 미분을 사용하는 경우가 있다. 그러나 수치적 미분은 오차를 크게 증폭시킬 수 있으며, 필터의 수치적 안정성에 악영향을 미칠 수 있다. 이를 해결하기 위한 방법으로 차분 기법을 사용하는 것이 더 수치적으로 안정적인 대안이 될 수 있다.

중앙 차분(Central Difference)은 수치 미분 대신 사용할 수 있는 대표적인 기법으로, 다음과 같은 방식으로 상태 전이 함수나 측정 함수의 야코비 행렬을 근사할 수 있다.

$\mathbf{F} \approx \frac{f(\mathbf{x} + \epsilon) - f(\mathbf{x} - \epsilon)}{2\epsilon}$

여기서 $\epsilon$ 은 작은 값으로, 상태 변수 $\mathbf{x}$ 의 변화를 나타낸다. 중앙 차분은 수치적 안정성을 높이고 오차를 줄이는 효과적인 방법이다.

공분산 행렬의 대칭성 유지

공분산 행렬 $\mathbf{P}$ 는 대칭성을 유지해야 한다. 이는 수치적 불안정성을 피하고 필터의 신뢰성을 높이는 중요한 조건이다. 그러나, 수치적으로 불안정한 연산으로 인해 대칭성이 깨질 수 있다. 이러한 문제를 방지하기 위해 공분산 행렬 업데이트 후에 대칭성을 강제로 복원하는 방법을 사용할 수 있다.

다음과 같은 방식으로 공분산 행렬의 대칭성을 복원할 수 있다.

$\mathbf{P} = \frac{\mathbf{P} + \mathbf{P}^{\top}}{2}$

이 식은 공분산 행렬 $\mathbf{P}$ 가 비대칭성을 가질 경우, 이를 대칭으로 만들어 수치적 오류를 줄이는 역할을 한다.

예측 단계에서의 수치적 안정성

확장 칼만 필터의 예측 단계에서는 상태와 공분산을 다음과 같이 예측한다.

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

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

여기서 $\mathbf{F}_k$ 는 상태 전이 함수의 야코비 행렬, $\mathbf{Q}_k$ 는 시스템 노이즈 공분산 행렬이다.

이 과정에서 수치적 안정성을 유지하려면 다음 사항을 고려해야 한다.

야코비 행렬의 정확한 계산: 야코비 행렬 $\mathbf{F}_k$ 는 예측 단계에서 매우 중요한 역할을 하므로, 정확하게 계산해야 한다. 앞서 언급한 차분 기법을 사용하거나 미리 정의된 함수의 기울기를 사용하는 방법을 고려할 수 있다.
잡음 공분산 행렬의 적절한 설정: 시스템 노이즈 공분산 행렬 $\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}$

$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \left( \mathbf{z}_k - h(\hat{\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}$

여기서 $\mathbf{K}_k$ 는 칼만 이득, $\mathbf{H}_k$ 는 측정 모델의 야코비 행렬, $\mathbf{R}_k$ 는 측정 잡음 공분산 행렬이다.

칼만 이득 계산의 수치적 안정성: 칼만 이득 $\mathbf{K}_k$ 를 계산할 때, 특히 행렬의 역행렬 계산에서 수치적 불안정성이 발생할 수 있다. 이를 방지하기 위해 수치적 안정성이 보장된 역행렬 계산 방법을 사용하는 것이 좋다. 대표적인 방법으로 Singular Value Decomposition (SVD) 혹은 QR 분해가 있다.
측정 잡음 공분산 행렬의 설정: $\mathbf{R}_k$ 는 필터의 측정 신뢰도를 나타낸다. $\mathbf{R}_k$ 의 값이 너무 크면 필터는 측정값을 신뢰하지 않고 예측값을 더 많이 반영하며, 너무 작으면 측정값에 지나치게 의존하게 된다. 이 값을 적절하게 설정하는 것이 필터의 안정성을 유지하는 중요한 요소이다.
차분 방법을 통한 선형화: 측정 모델 $h(\mathbf{x})$ 는 비선형 함수이므로, 이를 선형화하는 과정에서 앞서 언급한 중앙 차분 기법을 사용할 수 있다. 이 기법은 수치적 오차를 최소화하면서 선형 근사를 제공한다.

필터 재초기화

필터가 수치적 불안정성으로 인해 발산하거나 비정상적인 결과를 도출하는 경우, 필터를 다시 초기화하는 방법이 필요할 수 있다. 재초기화는 필터의 상태와 공분산을 적절한 값으로 설정하는 중요한 과정이며, 이러한 작업을 통해 필터가 다시 안정된 상태로 돌아올 수 있다.

일반적으로 재초기화는 다음과 같은 상황에서 고려된다.

필터의 상태 추정이 실측값과 지나치게 차이가 날 때
공분산 행렬 $\mathbf{P}$ 가 양의 정부호를 유지하지 않을 때
필터의 예측 단계에서 과도한 불확실성이 발생할 때