항법 시스템에서의 적용 - 소프트웨어 융합

항법 시스템에서 확장 칼만 필터의 필요성

항법 시스템에서는 이동체의 위치, 속도, 그리고 방향을 실시간으로 추정하는 것이 중요하다. 이러한 항법 시스템에서 사용되는 센서들은 비선형적 특성을 보이는 경우가 많다. 예를 들어, GPS, 관성 측정 장치(IMU), 그리고 자이로스코프 등이 결합된 시스템에서는 센서들이 제공하는 데이터의 정확도가 비선형적인 특성에 의해 제한될 수 있다. 이를 보정하고 최적의 추정치를 얻기 위해 확장 칼만 필터(EKF)가 사용된다. 확장 칼만 필터는 센서 데이터의 노이즈를 제거하고 시스템 상태를 예측, 추정할 수 있는 강력한 도구로 항법 시스템에서 널리 사용된다.

비선형 시스템 모델링

확장 칼만 필터를 적용하려면 항법 시스템을 수학적으로 모델링해야 한다. 비선형 시스템에서 상태 변수는 일반적으로 시간에 따라 변화하는 벡터 $\mathbf{x}(t)$ 로 표현되며, 다음과 같은 상태 방정식으로 기술된다.

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

여기서, - $\mathbf{x}_k$ 는 $k$ 번째 시간 단계에서의 상태 벡터이다. - $\mathbf{u}_k$ 는 제어 입력 벡터이다. - $f(\cdot)$ 는 시스템의 비선형 상태 전이 함수이다. - $\mathbf{w}_k$ 는 시스템 노이즈를 나타내는 백색 가우시안 잡음 벡터로, 공분산 $\mathbf{Q}_k$ 를 가진다.

측정 방정식은 다음과 같이 표현된다.

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

여기서, - $\mathbf{z}_k$ 는 $k$ 번째 시간 단계에서의 측정 벡터이다. - $h(\cdot)$ 는 측정 함수로, 상태 벡터 $\mathbf{x}_k$ 에서 측정값을 추출하는 비선형 함수이다. - $\mathbf{v}_k$ 는 측정 노이즈로, 공분산 $\mathbf{R}_k$ 를 가진다.

항법 시스템의 비선형 모델은 이처럼 상태 전이 함수와 측정 함수 모두에서 비선형성이 존재할 수 있다. 이로 인해 기본 칼만 필터로는 처리하기 어렵고, 확장 칼만 필터를 통해 선형화하여 해결한다.

확장 칼만 필터의 동작 원리

확장 칼만 필터는 비선형 시스템을 선형화하는 과정에서 테일러 급수를 사용한다. 이 때 1차 항까지만 사용하는 선형 근사 방법이 적용된다. 필터의 동작은 예측 단계와 갱신 단계로 나뉜다.

예측 단계

예측 단계에서는 상태 방정식과 시스템 노이즈를 기반으로 다음 상태를 예측한다. 상태 벡터의 예측은 다음과 같이 계산된다.

$\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{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}}$ 는 상태 전이 함수 $f(\cdot)$ 의 야코비이다. - $\mathbf{P}_{k|k-1}$ 는 예측된 오차 공분산 행렬이다. - $\mathbf{Q}_{k-1}$ 는 시스템 노이즈의 공분산 행렬이다.

갱신 단계

갱신 단계에서는 측정값을 기반으로 상태 벡터와 오차 공분산을 업데이트한다. 먼저 측정 예측을 구한다.

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

측정 잔차(residual)는 다음과 같이 계산된다.

$\mathbf{y}_k = \mathbf{z}_k - \hat{\mathbf{z}}_k$

오차 공분산 업데이트는 다음과 같다.

$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k$

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

최종적으로 칼만 이득 $K_k$ 는 다음과 같이 계산된다.

$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}$

상태 벡터 업데이트는 다음과 같다.

$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \mathbf{y}_k$

오차 공분산 업데이트는 다음과 같이 이루어진다.

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

센서 융합을 통한 항법 시스템 구현

항법 시스템에서 확장 칼만 필터를 사용하면 여러 센서 데이터를 결합하여 더 정밀한 위치 및 속도 정보를 얻을 수 있다. 특히, GPS와 IMU(관성 측정 장치)를 함께 사용하는 경우가 많으며, 각각의 센서는 상호 보완적인 정보를 제공한다.

GPS와 IMU 데이터의 융합

GPS는 위치 정보를 제공하는 데 유용하지만, 업데이트 속도가 느리기 때문에 고속 이동체의 경우 실시간 추적에 한계가 있다. 반면 IMU는 자이로스코프, 가속도계, 자력계를 포함하여 매우 높은 샘플링 속도로 운동 데이터를 제공한다. 그러나 IMU만으로는 위치 드리프트(오차 누적 현상)가 발생하기 때문에 GPS의 절대적인 위치 정보와 결합하여 상호 보완적인 역할을 수행한다.

GPS 데이터: GPS는 다음과 같은 비선형 측정 방정식으로 나타낼 수 있다.

$\mathbf{z}^{\text{GPS}}_k = \mathbf{H}^{\text{GPS}} \mathbf{x}_k + \mathbf{v}^{\text{GPS}}_k$

여기서, - $\mathbf{z}^{\text{GPS}}_k$ 는 GPS로부터 얻은 위치 측정 벡터이다. - $\mathbf{H}^{\text{GPS}}$ 는 GPS 측정 모델의 측정 행렬이다. - $\mathbf{v}^{\text{GPS}}_k$ 는 측정 노이즈 벡터로, 공분산 $\mathbf{R}^{\text{GPS}}$ 를 가진다.

IMU 데이터: IMU로부터 얻은 자이로스코프 및 가속도계 데이터는 속도 및 가속도의 추정에 사용되며, 이 데이터는 상태 벡터에 다음과 같이 반영된다.

$\mathbf{x}_{k+1} = \mathbf{F}^{\text{IMU}} \mathbf{x}_k + \mathbf{G}^{\text{IMU}} \mathbf{u}_k + \mathbf{w}^{\text{IMU}}_k$

여기서, - $\mathbf{F}^{\text{IMU}}$ 는 IMU 데이터를 기반으로 하는 상태 전이 행렬이다. - $\mathbf{G}^{\text{IMU}}$ 는 가속도계를 통한 제어 입력 행렬이다. - $\mathbf{u}_k$ 는 IMU 센서로부터의 가속도 데이터이다. - $\mathbf{w}^{\text{IMU}}_k$ 는 시스템 노이즈 벡터이다.

센서 데이터의 시간 동기화

GPS와 IMU는 다른 속도로 데이터를 제공하므로, 확장 칼만 필터에서 센서 데이터를 융합하기 위해 시간 동기화가 필요하다. 일반적으로 IMU는 고속으로 데이터를 제공하고, GPS는 상대적으로 느린 속도로 위치 정보를 제공한다. 확장 칼만 필터에서는 IMU로부터 들어오는 데이터를 바탕으로 예측 단계를 수행하고, GPS 데이터가 도착했을 때만 상태 업데이트 단계를 실행한다.

GPS의 위치 데이터를 통해 위치 드리프트를 보정하는 동시에 IMU의 고속 데이터를 통해 실시간 동작 변화를 추적할 수 있다. 이를 통해 항법 시스템은 고속의 정확한 위치 추정을 가능하게 한다.

항법 시스템에서의 적용 사례

항법 시스템에서 확장 칼만 필터를 사용한 센서 융합은 다양한 실세계 응용에서 사용된다. 대표적인 사례로는 다음과 같은 시스템들이 있다.

자율 주행 자동차: 자율 주행 자동차에서는 GPS와 IMU를 결합하여 차량의 위치를 추정하며, 이는 경로 계획 및 장애물 회피를 위한 중요한 정보로 사용된다.
드론 내비게이션: 드론은 GPS와 IMU, 그리고 가끔은 비전 센서를 결합하여 3차원 공간에서의 위치와 자세를 추정한다.
로봇 항법: 로봇 항법 시스템은 실내외를 포함한 다양한 환경에서 정확한 위치 추정을 위해 확장 칼만 필터를 사용하여 센서 데이터를 융합한다.

확장 칼만 필터의 성능 최적화

항법 시스템에서 확장 칼만 필터의 성능을 최적화하기 위해서는 센서 융합 과정에서 몇 가지 중요한 요소를 고려해야 한다. 특히, 비선형 시스템에서의 계산 복잡성, 센서의 노이즈 특성, 그리고 실시간 처리 요구 사항 등이 필터의 성능에 영향을 미친다.

1. 상태 변수와 노이즈의 정확한 모델링

확장 칼만 필터의 성능은 상태 변수와 노이즈 모델링에 크게 좌우된다. 상태 변수 $\mathbf{x}_k$ 와 노이즈 $\mathbf{w}_k$ , $\mathbf{v}_k$ 는 항법 시스템에서의 현실적인 특성을 반영해야 한다.

시스템 노이즈 $\mathbf{w}_k$ : 시스템 노이즈는 상태 변수의 예측 과정에서 발생하는 오차를 나타낸다. 예를 들어, IMU에서 얻은 가속도 데이터는 정확하지 않을 수 있으며, 이로 인해 시스템 노이즈가 발생할 수 있다. 시스템 노이즈의 공분산 행렬 $\mathbf{Q}_k$ 는 IMU 데이터의 정확도와 관련이 있다.

$\mathbf{Q}_k = E[\mathbf{w}_k \mathbf{w}_k^T]$

측정 노이즈 $\mathbf{v}_k$ : 측정 노이즈는 GPS나 IMU에서 얻은 데이터의 정확도와 관련된다. GPS는 대기 상태나 위성 신호 품질에 따라 노이즈가 변동될 수 있다. 측정 노이즈의 공분산 행렬 $\mathbf{R}_k$ 는 센서의 특성에 따라 적절하게 설정되어야 한다.

$\mathbf{R}_k = E[\mathbf{v}_k \mathbf{v}_k^T]$

2. 야코비 행렬의 정확한 계산

확장 칼만 필터에서 비선형 시스템을 선형화하기 위해 사용되는 야코비 행렬 $\mathbf{F}_k$ 와 $\mathbf{H}_k$ 는 시스템 및 측정 함수의 정확한 선형 근사를 제공해야 한다. 이러한 야코비 행렬은 각 상태 변수에 대한 미분으로 구할 수 있으며, 잘못된 선형화는 필터의 성능을 저하시킬 수 있다.

상태 전이 함수의 야코비 $\mathbf{F}_k$ :

$\mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k-1}}$

측정 함수의 야코비 $\mathbf{H}_k$ :

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

야코비 행렬의 계산에서 수치적 불안정성이 발생할 수 있으므로, 계산 과정에서의 수치 안정성도 고려해야 한다.

3. 필터의 수치적 안정성

확장 칼만 필터의 반복적인 계산 과정에서 발생할 수 있는 수치적 불안정성을 방지하기 위한 다양한 방법들이 적용될 수 있다. 예를 들어, 칼만 이득 $\mathbf{K}_k$ 를 계산할 때, 오차 공분산 행렬 $\mathbf{P}_k$ 가 음수가 되는 경우가 발생하지 않도록 조정이 필요하다.

또한, 센서 노이즈의 공분산 행렬이 적절하게 설정되지 않은 경우, 필터의 성능이 급격히 저하될 수 있다. 이때 필터의 안정성을 확보하기 위한 몇 가지 방법들이 있다.

공분산 행렬 재조정: 공분산 행렬 $\mathbf{P}_k$ 가 비대칭이거나 음수가 되는 경우, 이를 재조정하여 수치적 안정성을 확보할 수 있다.

$\mathbf{P}_k = \mathbf{P}_k + \epsilon \mathbf{I}$

여기서 $\epsilon$ 은 작은 양수로, 공분산 행렬이 음수가 되는 것을 방지한다.

게인 클리핑: 칼만 이득 $\mathbf{K}_k$ 가 너무 커질 경우, 필터가 과하게 반응할 수 있다. 이때 칼만 이득의 크기를 제한하는 게인 클리핑 기법을 사용할 수 있다.

4. 실시간 처리 요구사항

항법 시스템은 실시간으로 데이터를 처리해야 하므로 확장 칼만 필터의 계산 복잡성을 줄이는 것이 중요하다. 특히, 필터의 상태 업데이트 단계에서 야코비 행렬의 계산이 복잡해질 수 있는데, 이를 최적화하기 위한 방법들이 존재한다.

야코비 근사: 정확한 야코비을 계산하는 대신, 수치적으로 근사된 야코비을 사용할 수 있다. 이는 계산 시간을 줄이면서도 충분한 정확도를 제공할 수 있는 방법이다.

$\mathbf{F}_k \approx \mathbf{F}_{k-1}$

코드 최적화: 필터의 구현 과정에서 계산 효율성을 높이기 위해 코드 최적화를 적용할 수 있다. 특히, 다중 센서 데이터를 실시간으로 처리하기 위해서는 알고리즘의 병렬화나 하드웨어 가속을 고려할 수 있다.