디버깅 단계

확장 칼만 필터(EKF)를 소프트웨어로 구현할 때, 가장 중요한 부분 중 하나는 디버깅 과정이다. 특히 EKF는 비선형성을 다루기 때문에, 예상하지 못한 동작이 발생할 수 있다. 이를 방지하기 위해 디버깅 단계에서 주의해야 할 몇 가지 핵심 사항이 있다.

  1. 초기화 검증 초기화 단계에서 필터의 상태 추정치(초기 상태 \mathbf{x}_0)와 오차 공분산 행렬(\mathbf{P}_0)을 잘못 설정하면 필터의 성능이 크게 저하될 수 있다. 이때, \mathbf{x}_0는 시스템 모델에서 설정된 초기 조건과 일치해야 하며, \mathbf{P}_0는 시스템의 초기 상태에 대한 불확실성을 나타내므로 너무 작거나 큰 값으로 설정하지 않도록 주의해야 한다.
\mathbf{x}_0 = \begin{bmatrix} x_{1,0} \\ x_{2,0} \\ \vdots \\ x_{n,0} \end{bmatrix} \quad \text{및} \quad \mathbf{P}_0 = \begin{bmatrix} P_{11} & P_{12} & \cdots & P_{1n} \\ P_{21} & P_{22} & \cdots & P_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ P_{n1} & P_{n2} & \cdots & P_{nn} \end{bmatrix}
  1. 예측 단계 디버깅 예측 단계는 다음과 같은 상태 예측 식에 의해 수행된다:
\mathbf{x}_{k|k-1} = f(\mathbf{x}_{k-1})

여기서 f(\cdot)는 상태 전이 함수이다. 예측 단계에서는 이 함수의 정확한 선형화와 시간 업데이트가 중요하다. 수식적으로, 야코비 행렬 \mathbf{F}_k가 잘못 계산되면, 전체 필터의 성능이 저하될 수 있다. 야코비 행렬의 계산 과정을 디버깅할 때는, 수치 미분을 활용해 계산된 값과 비교하는 방법이 유용하다.

\mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\mathbf{x}_{k-1}}
  1. 갱신 단계 디버깅 갱신 단계에서는 관측치와 예측치 사이의 차이를 보정하는 과정이 중요하다. 이를 위해 관측 모델 h(\cdot)의 선형화가 적절하게 이루어져야 하며, 야코비 행렬 \mathbf{H}_k를 올바르게 계산해야 한다. 이 단계에서 문제가 발생하면, 필터의 보정 성능이 크게 저하된다.

갱신 식은 다음과 같다:

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

이때, 칼만 이득 \mathbf{K}_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}

따라서 \mathbf{H}_k의 계산과 \mathbf{K}_k의 계산이 정확히 이루어졌는지 확인하는 것이 필수적이다.

테스트 방법

디버깅 과정이 완료된 후, 확장 칼만 필터의 성능을 검증하기 위해 적절한 테스트 방법을 사용하는 것이 중요하다. 테스트는 주로 시뮬레이션 환경에서 이루어지며, 필터가 예상한 대로 동작하는지 확인한다.

  1. 상태 추정 정확도 평가 필터의 상태 추정치 \mathbf{x}_k가 실제 시스템 상태에 얼마나 근접한지를 평가하는 것이 가장 중요한 테스트 항목 중 하나이다. 이를 위해 필터가 추정한 값과 실제 값(또는 시뮬레이션에서 생성된 참값) 사이의 차이를 계산한다. 대표적으로 사용되는 오차 지표는 평균 제곱 오차(MSE)이다.
\text{MSE} = \frac{1}{N} \sum_{k=1}^{N} \left\| \mathbf{x}_k - \mathbf{x}_{\text{true},k} \right\|^2

여기서 \mathbf{x}_{\text{true},k}는 참값을 의미한다.

  1. 오차 공분산 행렬 검증 확장 칼만 필터에서 오차 공분산 행렬 \mathbf{P}_k는 필터가 추정하는 상태의 불확실성을 나타낸다. 이 값이 시스템의 실제 불확실성을 잘 반영하고 있는지 확인하는 것이 중요하다. 테스트 시, \mathbf{P}_k의 대각 성분이 예상되는 범위 내에 있는지 확인해야 하며, 만약 불합리하게 작은 값이 나온다면 이는 필터가 과적합(overconfidence)된 것일 수 있다.
\mathbf{P}_k = \begin{bmatrix} P_{11} & P_{12} & \cdots & P_{1n} \\ P_{21} & P_{22} & \cdots & P_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ P_{n1} & P_{n2} & \cdots & P_{nn} \end{bmatrix}
  1. 계측 노이즈 모델 검증 필터는 계측 노이즈를 고려하여 작동한다. 따라서, 노이즈가 필터에 올바르게 적용되었는지를 확인하는 테스트가 필요하다. 필터에서 사용하는 노이즈 공분산 행렬 \mathbf{R}_k와 실제 노이즈 간의 차이를 확인하는 방법을 통해 필터의 튜닝이 적절히 이루어졌는지 평가할 수 있다. 특히, 노이즈가 과소추정되면 필터의 성능이 저하될 수 있다.
\mathbf{R}_k = \text{Cov}(\mathbf{z}_k - h(\mathbf{x}_k))

  1. 테스트 데이터 생성 확장 칼만 필터의 성능을 평가하기 위한 테스트 데이터는 다음과 같은 두 가지 방법으로 생성할 수 있다.

  2. 시뮬레이션 데이터: 시스템 모델을 기반으로 인위적으로 생성된 데이터. 이를 통해 필터가 다양한 환경에서 어떻게 동작하는지 확인할 수 있다. 특히, 비선형 시스템에서의 필터 성능을 평가할 때 유용하다.

  3. 실제 센서 데이터: 실제 환경에서 계측한 데이터를 사용하여 필터를 테스트하는 방법. 실험 장비를 사용해 수집된 데이터를 활용하면 필터가 실제 환경에서도 적절하게 작동하는지 확인할 수 있다.

  4. 파라미터 튜닝 필터의 성능을 최적화하기 위해서는 필터의 여러 파라미터를 조정하는 과정이 필요하다. 주요 파라미터로는 상태 전이 모델의 오차 공분산 행렬(\mathbf{Q}_k)과 측정 오차 공분산 행렬(\mathbf{R}_k)이 있다. 이 값들이 실제 시스템의 특성과 맞지 않으면 필터 성능이 저하될 수 있으므로, 다양한 실험을 통해 적절한 값을 찾는 과정이 필요하다.

\mathbf{Q}_k = \begin{bmatrix} q_1 & 0 & \cdots & 0 \\ 0 & q_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & q_n \end{bmatrix} \quad \text{및} \quad \mathbf{R}_k = \begin{bmatrix} r_1 & 0 & \cdots & 0 \\ 0 & r_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & r_m \end{bmatrix}