10.63 쿼터니언 기반 칼만 필터 설계

10.63 쿼터니언 기반 칼만 필터 설계

1. 쿼터니언 기반 칼만 필터의 개요

칼만 필터는 노이즈가 있는 측정값으로부터 시스템 상태를 추정하는 최적 필터이다. 자세 추정의 경우, 쿼터니언을 상태의 일부로 사용하는 것이 표준이다. 본 절에서는 쿼터니언 기반 칼만 필터의 설계 원리와 주요 변형을 다룬다.

2. 자세 추정 필터의 도전

2.1 비선형성

자세 동역학은 본질적으로 비선형이다. 쿼터니언의 곱셈과 지수 사상이 비선형 연산이다. 이는 선형 칼만 필터를 직접 적용할 수 없게 한다.

2.2 매니폴드 구조

자세는 SO(3) 매니폴드의 원소이다. 단순 덧셈은 매니폴드를 벗어날 수 있다.

2.3 단위 노름 제약

쿼터니언은 단위 노름을 유지해야 한다. 단순 선형 연산은 이 제약을 깰 수 있다.

이러한 도전을 해결하기 위해 여러 변형의 칼만 필터가 개발되었다.

3. 확장 칼만 필터 (EKF)

가장 기본적인 변형은 확장 칼만 필터(Extended Kalman Filter, EKF)이다.

3.1 상태 벡터

\mathbf{x} = \begin{bmatrix}\mathbf{q} \\ \mathbf{b}_g\end{bmatrix}

여기서 \mathbf{q}는 쿼터니언, \mathbf{b}_g는 자이로스코프 편향이다. 상태 벡터의 차원은 4 + 3 = 7이다.

3.2 예측 단계

쿼터니언을 곱셈적으로 업데이트한다.

\mathbf{q}_{k+1} = \mathbf{q}_k\otimes\exp(\boldsymbol{\omega}_k\Delta t/2)

편향은 랜덤 워크로 모델링한다.

\mathbf{b}_{g,k+1} = \mathbf{b}_{g,k} + \mathbf{w}_b

3.3 공분산 예측

\mathbf{P}_{k+1}^- = \boldsymbol{\Phi}_k\mathbf{P}_k\boldsymbol{\Phi}_k^T + \mathbf{Q}_k

여기서 \boldsymbol{\Phi}_k는 선형화된 전이 행렬이다.

3.4 측정 업데이트

측정값(가속도계, 자력계 등)을 사용하여 상태를 업데이트한다.

3.5 EKF의 한계

  • 쿼터니언의 단위 노름 제약을 직접 처리하기 어려움
  • 비선형 선형화의 오차
  • 공분산 행렬이 7\times 7로 과잉 매개화됨

4. 곱셈 확장 칼만 필터 (MEKF)

MEKF(Multiplicative Extended Kalman Filter)는 EKF의 개선 형태이며, 쿼터니언의 매니폴드 구조를 고려한다.

4.1 상태 분리

  • 명목 상태: 쿼터니언 \hat{\mathbf{q}}
  • 오차 상태: 작은 회전 벡터 \delta\boldsymbol{\phi}

4.2 오차 상태

오차 상태는 작은 회전 벡터이므로 선형 공간에서 처리할 수 있다.

\delta\mathbf{x} = \begin{bmatrix}\delta\boldsymbol{\phi} \\ \delta\mathbf{b}_g\end{bmatrix}

이 벡터의 차원은 3 + 3 = 6이다.

4.3 예측 단계

명목 쿼터니언을 곱셈적으로 업데이트한다.

\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k\otimes\exp(\boldsymbol{\omega}_k\Delta t/2)

오차 공분산을 선형화로 전파한다.

4.4 측정 업데이트

측정값으로부터 오차 상태를 업데이트한다.

4.5 리셋

오차 상태를 명목 상태에 합치고 오차를 0으로 리셋한다.

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

\delta\boldsymbol{\phi}_k = \mathbf{0}

4.6 MEKF의 장점

  1. 단위 노름 자동 보존: 명목 쿼터니언이 곱셈적으로 업데이트되므로 단위 노름이 보존됨
  2. 매니폴드 구조 준수: SO(3) 구조가 보존됨
  3. 효율성: 오차 상태가 6차원으로 과잉 매개화 없음

5. 오류 상태 칼만 필터 (ESKF)

ESKF는 MEKF와 유사하지만 더 일반화된 형태이다. 자세뿐만 아니라 위치, 속도 등도 함께 추정한다.

5.1 상태 분리

  • 명목 상태: 현재 최선의 추정 (위치, 속도, 쿼터니언, 편향)
  • 오차 상태: 작은 오차 변수

5.2 오차 상태 정의

\delta\mathbf{x} = \begin{bmatrix}\delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldsymbol{\phi} \\ \delta\mathbf{b}_a \\ \delta\mathbf{b}_g\end{bmatrix}

여기서 \delta\mathbf{p}, \delta\mathbf{v}는 위치/속도 오차, \delta\boldsymbol{\phi}는 자세 오차, \delta\mathbf{b}_a, \delta\mathbf{b}_g는 가속도/자이로 편향 오차이다.

5.3 명목 동역학

명목 상태는 비선형 동역학으로 전파된다.

\hat{\mathbf{p}}_{k+1} = \hat{\mathbf{p}}_k + \hat{\mathbf{v}}_k\Delta t + \frac{1}{2}\mathbf{R}(\hat{\mathbf{q}}_k)(\mathbf{a}_m - \hat{\mathbf{b}}_a)\Delta t^2

\hat{\mathbf{v}}_{k+1} = \hat{\mathbf{v}}_k + \mathbf{R}(\hat{\mathbf{q}}_k)(\mathbf{a}_m - \hat{\mathbf{b}}_a)\Delta t + \mathbf{g}\Delta t

\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_k\otimes\exp((\boldsymbol{\omega}_m - \hat{\mathbf{b}}_g)\Delta t/2)

5.4 오차 동역학

오차 상태는 선형화된 동역학으로 전파된다.

5.5 측정 업데이트

측정값을 사용하여 오차 상태를 업데이트한다. 표준 칼만 필터 업데이트이다.

5.6 리셋

오차 상태를 명목 상태에 합친 후 오차를 0으로 리셋한다.

6. ESKF의 장점

6.1 매니폴드 보존

명목 상태가 매니폴드 구조를 유지한다.

6.2 선형 필터링

오차 상태가 선형 공간에서 처리되어 표준 칼만 필터 이론을 적용할 수 있다.

6.3 수치 안정성

단위 쿼터니언과 기타 제약이 자동으로 보존된다.

6.4 효율성

오차 상태의 차원이 최소이다. 공분산 행렬이 작다.

7. 무향 칼만 필터 (UKF)

UKF(Unscented Kalman Filter)는 EKF보다 더 정확한 비선형 필터링 기법이다. 선형화 대신 시그마 포인트 샘플링을 사용한다.

7.1 시그마 포인트

상태의 공분산으로부터 대표 점들(시그마 포인트)을 생성한다. 이를 비선형 함수에 통과시켜 평균과 공분산을 추정한다.

7.2 쿼터니언 처리

쿼터니언의 시그마 포인트 생성은 회전 벡터 공간에서 수행된다. 이는 매니폴드 구조를 고려한 샘플링이다.

7.3 장점

  • 2차 정확도 (EKF는 1차)
  • 선형화가 필요 없음
  • 비선형성이 심한 경우 EKF보다 우수

7.4 단점

  • EKF보다 계산 비용이 높음
  • 쿼터니언의 특수 처리 필요

8. 입자 필터

입자 필터(particle filter)는 몬테카를로 기반 필터이며, 비선형 비가우시안 시스템에 적용된다.

8.1 입자

상태 공간에서 많은 입자를 생성하고, 각 입자의 가중치로 상태 분포를 근사한다.

8.2 쿼터니언 처리

쿼터니언 입자는 리 군 구조를 고려하여 생성되고 갱신된다.

8.3 장점

비가우시안 분포와 매우 비선형인 시스템을 처리할 수 있다.

8.4 단점

많은 입자가 필요하며 계산 비용이 매우 높다.

9. 구체적 알고리즘: MEKF

MEKF의 구체적 알고리즘을 설명한다.

9.1 초기화

초기 쿼터니언 \hat{\mathbf{q}}_0와 오차 공분산 \mathbf{P}_0를 설정한다.

9.2 예측 단계

  1. 각속도 측정: \boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g

  2. 편향 보정: \hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_m - \hat{\mathbf{b}}_g

  3. 명목 쿼터니언 업데이트: \hat{\mathbf{q}}_{k+1}^- = \hat{\mathbf{q}}_k\otimes\exp(\hat{\boldsymbol{\omega}}\Delta t/2)

  4. 오차 공분산 전파: \mathbf{P}_{k+1}^- = \boldsymbol{\Phi}\mathbf{P}_k\boldsymbol{\Phi}^T + \mathbf{Q}

9.3 측정 업데이트

  1. 측정: \mathbf{z}_k

  2. 예측 측정: \hat{\mathbf{z}}_k = h(\hat{\mathbf{q}}_{k+1}^-)

  3. 잔차: \mathbf{r}_k = \mathbf{z}_k - \hat{\mathbf{z}}_k

  4. 칼만 이득: \mathbf{K}_k = \mathbf{P}_{k+1}^-\mathbf{H}^T(\mathbf{H}\mathbf{P}_{k+1}^-\mathbf{H}^T + \mathbf{R})^{-1}

  5. 오차 상태 업데이트: \delta\mathbf{x}_k = \mathbf{K}_k\mathbf{r}_k

  6. 명목 쿼터니언 리셋: \hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_{k+1}^-\otimes\exp(\delta\boldsymbol{\phi}_k/2)

  7. 공분산 업데이트: \mathbf{P}_{k+1} = (\mathbf{I} - \mathbf{K}_k\mathbf{H})\mathbf{P}_{k+1}^-

10. 측정 모델

자세 추정에서 일반적인 측정은 다음과 같다.

10.1 가속도계

중력 벡터를 측정한다. 정지 상태에서는 중력만, 가속 중에는 중력과 가속도의 합이다.

\mathbf{a}_m = \mathbf{R}^T\mathbf{g} + \mathbf{a}_{\text{body}} + \mathbf{b}_a + \mathbf{n}_a

10.2 자력계

지자기 벡터를 측정한다.

\mathbf{m}_m = \mathbf{R}^T\mathbf{m}_{\text{ref}} + \mathbf{b}_m + \mathbf{n}_m

10.3 별 추적기

별의 방향을 측정한다(우주선).

\mathbf{s}_{\text{measured}} = \mathbf{R}^T\mathbf{s}_{\text{ref}}

10.4 GPS

절대 위치와 속도를 측정한다.

10.5 카메라

시각적 특징의 이미지 좌표로부터 자세 정보를 추출한다.

11. 필터 튜닝

11.1 공정 잡음 공분산 \mathbf{Q}

모델의 불확실성을 반영한다. 자이로스코프 잡음과 편향 드리프트가 주요 원인이다.

11.2 측정 잡음 공분산 \mathbf{R}

각 측정의 잡음 특성을 반영한다. 각 센서의 잡음 파라미터로부터 계산된다.

11.3 초기 공분산 \mathbf{P}_0

초기 추정의 불확실성이다. 보수적으로 큰 값을 설정한다.

11.4 튜닝 방법

  1. 이론적 값에서 시작
  2. 실제 성능을 관찰하여 조정
  3. 시뮬레이션으로 검증

12. 실용적 고려

12.1 실시간 구현

필터가 센서의 샘플링 주기 내에 실행되어야 한다. 쿼터니언 기반 필터는 일반적으로 효율적이다.

12.2 수치 안정성

공분산 행렬이 양정치성을 유지해야 한다. Joseph 형태 업데이트가 이를 보장한다.

12.3 초기화

초기 자세 추정이 필요하다. 가속도계와 자력계의 단일 측정으로 추정할 수 있다(TRIAD 알고리즘).

12.4 아웃라이어 처리

잘못된 측정값을 감지하고 제외한다.

12.5 관측 가능성

특정 상태 변수가 관측 가능한지 확인한다. 관측 불가능한 변수는 추정이 발산할 수 있다.

13. 필터 성능 평가

13.1 정확성

추정 오차의 평균과 표준 편차이다.

13.2 일관성

추정된 공분산이 실제 오차와 일치하는지 (Normalized Innovation Squared, NIS 테스트).

13.3 수렴

초기 오차에서 정확한 추정으로 수렴하는 속도이다.

13.4 강건성

외란과 이상치에 대한 반응이다.

14. 다양한 응용

14.1 IMU 기반 자세 추정

IMU(가속도계, 자이로스코프, 자력계)만을 사용한 자세 추정. 드론, 로봇, 휴대폰 등 널리 사용된다.

14.2 시각 관성 오도메트리 (VIO)

카메라와 IMU를 융합한 자세와 위치 추정. ESKF가 표준이다.

14.3 GPS/INS 융합

GPS와 관성 항법 시스템을 융합한다. 장시간 정확한 위치와 자세를 얻는다.

14.4 우주선 자세 추정

별 추적기, 자이로스코프, 자력계를 융합한 우주선 자세 추정. MEKF가 표준이다.

14.5 로봇 상태 추정

이동 로봇의 자세와 위치 추정. 다양한 센서 융합 기법이 사용된다.

15. 라이브러리 지원

15.1 C++

  • Sophus: 리 군 연산
  • GTSAM: 인수 그래프 기반 추정
  • ROS: 로봇 통합

15.2 Python

  • NumPy/SciPy: 기본 연산
  • filterpy: 칼만 필터 구현

15.3 전문 라이브러리

  • OKVIS: 시각 관성 오도메트리
  • VINS-Mono: 시각 관성 SLAM

16. 결론

쿼터니언 기반 칼만 필터는 자세 추정의 표준 기법이다. EKF, MEKF, ESKF, UKF 등 다양한 변형이 있으며, 각각 장단점이 있다. ESKF가 가장 일반적이며, 명목 상태와 오차 상태의 분리로 매니폴드 구조 보존과 선형 필터 이론 적용을 결합한다. 자세 추정의 정확성, 견고성, 실시간 성능이 로봇 공학과 우주 항공의 핵심 요소이며, 쿼터니언 기반 필터가 이를 달성한다.

17. 참고 문헌

  • Lefferts, E. J., Markley, F. L., & Shuster, M. D. (1982). “Kalman Filtering for Spacecraft Attitude Estimation.” Journal of Guidance, Control, and Dynamics, 5(5), 417–429.
  • Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
  • Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.
  • Crassidis, J. L., Markley, F. L., & Cheng, Y. (2007). “Survey of Nonlinear Attitude Estimation Methods.” Journal of Guidance, Control, and Dynamics, 30(1), 12–28.
  • Trawny, N., & Roumeliotis, S. I. (2005). “Indirect Kalman Filter for 3D Attitude Estimation.” Technical Report, University of Minnesota.

version: 1.0