10.66 쿼터니언 기반 무향 칼만 필터(UKF)

1. UKF의 개요

무향 칼만 필터(Unscented Kalman Filter, UKF)는 비선형 시스템의 상태 추정을 위한 필터링 기법이다. Julier와 Uhlmann(1997)이 제안하였으며, 확장 칼만 필터(EKF)의 선형화 기반 접근과 달리 결정적 시그마 포인트 샘플링을 사용한다.

쿼터니언 기반 UKF는 자세 추정에 UKF를 적용한 것이며, 매니폴드 구조를 고려한 특별한 처리가 필요하다.

2. UKF의 기본 원리

2.1 시그마 포인트

UKF는 상태 분포를 대표하는 결정적 점들(시그마 포인트)을 생성한다. 이 점들이 상태의 평균과 공분산을 정확히 포착한다.

2.2 비선형 변환

각 시그마 포인트를 비선형 함수에 통과시킨다. 결과 점들로부터 출력의 평균과 공분산을 계산한다.

2.3 차 정확도

UKF는 비선형 변환의 2차 정확도를 가진다. 이는 EKF의 1차 정확도보다 우수하다.

3. 시그마 포인트 생성

3.1 표준 형태

$n$ 차원 상태 $\mathbf{x}$ 와 공분산 $\mathbf{P}$ 에 대해 $2n + 1$ 개의 시그마 포인트를 생성한다.

$\boldsymbol{\chi}_0 = \mathbf{x}$

$\boldsymbol{\chi}_i = \mathbf{x} + \left(\sqrt{(n + \lambda)\mathbf{P}}\right)_i, \quad i = 1, \ldots, n$

$\boldsymbol{\chi}_{i+n} = \mathbf{x} - \left(\sqrt{(n + \lambda)\mathbf{P}}\right)_i, \quad i = 1, \ldots, n$

여기서 $\lambda$ 는 스케일링 매개변수, $(\sqrt{\mathbf{P}})_i$ 는 행렬 제곱근의 $i$ 번째 열이다.

3.2 가중치

각 시그마 포인트에 가중치를 할당한다.

$W_0^{(m)} = \frac{\lambda}{n + \lambda}$

$W_0^{(c)} = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)$

$W_i^{(m)} = W_i^{(c)} = \frac{1}{2(n + \lambda)}$

여기서 $W^{(m)}$ 은 평균 계산용, $W^{(c)}$ 은 공분산 계산용 가중치이다.

4. 쿼터니언 UKF의 도전

쿼터니언을 UKF에 직접 적용하기 어려운 이유는 다음과 같다.

4.1 단위 노름 제약

시그마 포인트가 단위 노름을 유지해야 한다. 단순 덧셈/뺄셈은 이 제약을 깨뜨린다.

4.2 매니폴드 구조

쿼터니언은 $S^3$ 매니폴드의 점이다. 유클리드 공간의 평균은 일반적으로 $S^3$ 에 속하지 않는다.

4.3 공분산의 과잉 매개화

4차원 쿼터니언 공간에서 공분산이 과잉 매개화된다.

5. 쿼터니언 UKF의 접근

이러한 도전을 해결하기 위해 다양한 접근이 제안되었다.

5.1 USQUE (Unscented Quaternion Estimator)

Crassidis와 Markley(2003)가 제안한 USQUE는 쿼터니언 UKF의 표준 접근이다. 핵심 아이디어는 시그마 포인트를 회전 벡터 공간에서 생성하고, 이를 쿼터니언으로 변환하는 것이다.

5.2 오차 상태 형태

MEKF와 유사하게, 명목 쿼터니언과 오차 상태(회전 벡터)를 분리한다. UKF는 오차 상태에 적용된다.

6. USQUE 알고리즘

USQUE의 주요 단계를 살펴본다.

6.1 시그마 포인트 생성

오차 상태 $\delta\mathbf{x}$ 의 공분산 $\mathbf{P}$ 로부터 시그마 포인트를 생성한다.

$\delta\boldsymbol{\chi}_i = \text{sigma points of } \delta\mathbf{x}$

각 시그마 포인트는 작은 회전 벡터이다.

6.2 쿼터니언 시그마 포인트

회전 벡터 시그마 포인트를 쿼터니언으로 변환한다.

$\mathbf{q}_i = \hat{\mathbf{q}}\otimes\exp(\delta\boldsymbol{\chi}_i/2)$

여기서 $\hat{\mathbf{q}}$ 는 현재 명목 쿼터니언이다. 결과 $\mathbf{q}_i$ 는 모두 단위 쿼터니언이다.

6.3 시그마 포인트 전파

각 쿼터니언 시그마 포인트를 비선형 동역학으로 전파한다.

$\mathbf{q}_{i,k+1} = f(\mathbf{q}_{i,k}, \boldsymbol{\omega}_k, \Delta t)$

6.4 예측된 쿼터니언 평균

시그마 포인트의 가중 평균을 쿼터니언 평균으로 계산한다. 쿼터니언의 평균 계산은 선형 평균이 아니라 특별한 절차를 사용한다.

6.5 예측된 오차 상태

명목 쿼터니언(가중 평균)에 대한 각 시그마 포인트의 오차를 회전 벡터로 계산한다.

$\delta\boldsymbol{\chi}_{i,k+1} = 2\log(\hat{\mathbf{q}}_{k+1}^{-1}\mathbf{q}_{i,k+1})^\vee$

6.6 예측된 공분산

$\mathbf{P}_{k+1}^- = \sum_i W_i^{(c)}\delta\boldsymbol{\chi}_{i,k+1}\delta\boldsymbol{\chi}_{i,k+1}^T + \mathbf{Q}$

6.7 측정 업데이트

측정 시그마 포인트 $\mathbf{z}_i$ 를 계산하고, 측정 평균과 공분산, 교차 공분산을 구한다.

6.8 칼만 이득과 업데이트

$\mathbf{K}_k = \mathbf{P}_{xz}\mathbf{P}_{zz}^{-1}$

$\delta\mathbf{x} = \mathbf{K}_k(\mathbf{z}_k - \hat{\mathbf{z}}_k)$

6.9 명목 쿼터니언 업데이트

$\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_{k+1}^-\otimes\exp(\delta\boldsymbol{\phi}/2)$

7. 쿼터니언 평균 계산

시그마 포인트 쿼터니언의 평균을 계산하는 방법은 여러 가지가 있다.

7.1 선형 평균과 정규화

$\bar{\mathbf{q}} = \frac{\sum_i W_i\mathbf{q}_i}{\lVert\sum_i W_i\mathbf{q}_i\rVert}$

이는 단순하지만 정확하지 않을 수 있다.

7.2 고유값 기반 평균

Markley 등이 제안한 방법으로, $4\times 4$ 행렬의 고유값 문제를 푼다.

$\mathbf{M} = \sum_i W_i\mathbf{q}_i\mathbf{q}_i^T$

$\bar{\mathbf{q}}$ 는 $\mathbf{M}$ 의 최대 고유값에 해당하는 고유 벡터이다.

7.3 반복적 방법

작은 회전의 가중 평균을 계산하여 평균 쿼터니언을 업데이트하는 반복적 방법이다.

8. UKF vs. MEKF

8.1 정확도

UKF는 2차 정확도, MEKF는 1차 정확도이다. 이론적으로 UKF가 더 정확하다.

8.2 계산 비용

UKF는 $2n + 1$ 개의 시그마 포인트를 전파하므로 MEKF보다 계산 비용이 높다.

8.3 선형화 없음

UKF는 자코비안 계산이 필요 없다. 이는 비선형이 심한 경우 유리하다.

8.4 구현

MEKF가 더 간단하게 구현된다. UKF는 시그마 포인트 생성, 쿼터니언 평균 계산 등 추가 작업이 필요하다.

9. UKF의 장점

9.1 2차 정확도

비선형 변환의 2차 효과까지 포착한다.

9.2 자코비안 불필요

비선형 함수의 자코비안을 계산할 필요가 없다. 이는 복잡한 함수에서 유용하다.

9.3 견고성

심한 비선형성에서 EKF보다 견고하다.

10. UKF의 단점

10.1 계산 비용

EKF보다 계산 비용이 높다. 시그마 포인트를 전파해야 한다.

10.2 구현 복잡성

시그마 포인트 생성, 가중치 선택, 평균 계산 등이 복잡하다.

10.3 매개변수 튜닝

$\alpha$ , $\beta$ , $\lambda$ 등의 매개변수를 튜닝해야 한다.

11. 쿼터니언 UKF의 응용

11.1 우주선 자세 추정

USQUE는 우주선 자세 추정에서 사용된다. 특히 비선형성이 심한 경우에 유리하다.

11.2 드론 상태 추정

드론의 곡예 비행 등 심한 비선형 운동에서 UKF가 활용된다.

11.3 매니퓰레이터 기구학

매니퓰레이터의 역기구학 해법에 UKF가 적용될 수 있다.

12. UKF의 변형

12.1 축소 시그마 포인트

매개변수 수를 줄여 계산 비용을 감소시킨다.

12.2 Cubature Kalman Filter (CKF)

구면적 적분 규칙을 사용한다. UKF와 유사한 정확도와 더 단순한 형태이다.

12.3 Iterated UKF

측정 업데이트를 반복하여 정확도를 향상시킨다.

13. 실용적 고려

13.1 매개변수 설정

일반적인 매개변수 값:

$\alpha = 10^{-3}$ (시그마 포인트 분산)
$\beta = 2$ (가우시안 가정 시)
$\kappa = 0$ 또는 $3 - n$

13.2 공분산 제곱근

공분산의 제곱근(Cholesky 분해 등)이 필요하다. 수치적 안정성에 주의한다.

13.3 양정치성 유지

공분산이 양정치성을 유지해야 한다. Joseph 형태나 제곱근 형태의 업데이트가 도움이 된다.

14. 결론

쿼터니언 기반 UKF(USQUE)는 자세 추정의 고급 필터링 기법이다. MEKF의 1차 선형화보다 2차 정확도를 제공하지만 계산 비용이 더 높다. 특히 비선형성이 심한 상황에서 유리하며, 우주선 자세 추정 등에 사용된다. 구현 복잡성과 계산 비용의 트레이드오프를 고려하여 선택해야 한다.

15. 참고 문헌

Julier, S. J., & Uhlmann, J. K. (1997). “A New Extension of the Kalman Filter to Nonlinear Systems.” International Symposium on Aerospace/Defense Sensing, Simulation and Controls.
Crassidis, J. L., & Markley, F. L. (2003). “Unscented Filtering for Spacecraft Attitude Estimation.” Journal of Guidance, Control, and Dynamics, 26(4), 536–542.
Wan, E. A., & Van Der Merwe, R. (2000). “The Unscented Kalman Filter for Nonlinear Estimation.” IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium.
Cheng, Y., & Crassidis, J. L. (2010). “Particle Filtering for Sequential Spacecraft Attitude Estimation.” AIAA Guidance, Navigation, and Control Conference.
Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.

version: 1.0