11.18 리 군 기반 상태 추정 알고리즘

1. 리 군 기반 상태 추정의 개요

리 군 기반 상태 추정(Lie group-based state estimation)은 추정 변수가 리 군의 원소이거나 리 군의 원소를 포함하는 상태 추정 문제를 다루는 방법론이다. 자세, 강체 변환, 회전 등이 자연스럽게 리 군의 원소이므로, 자세 추정과 위치 추정 등 로봇공학의 핵심 문제들이 본질적으로 리 군 기반 상태 추정 문제이다. 일반적인 칼만 필터는 유클리드 공간 위에서 정의되지만, 리 군 기반 알고리즘은 다양체 구조를 활용하여 더 정확하고 일관된 추정을 제공한다.

2. 표준 칼만 필터의 한계

2.1 가산 갱신의 문제

표준 칼만 필터는 가산 갱신을 사용한다.

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

자세 변수에 적용하면 단위성이 손상된다. 예를 들어 회전 행렬에 가산 갱신을 적용하면 결과가 더 이상 회전 행렬이 아니다.

2.2 가우시안 가정의 부적합성

표준 칼만 필터는 상태 분포가 가우시안임을 가정한다. 그러나 회전 행렬이나 단위 쿼터니언의 가우시안 분포는 자연스럽게 정의되지 않는다. 다양체 위의 분포는 다른 방식으로 정의되어야 한다.

2.3 선형화의 부정확성

비선형 시스템의 선형화는 다양체의 구조를 무시한다. 이는 큰 자세 변화에서 부정확한 결과를 낳을 수 있다.

3. 리 군 기반 접근법의 핵심

3.1 명목 상태와 오차 상태의 분리

리 군 기반 알고리즘은 상태를 명목 상태(nominal state)와 오차 상태(error state)로 분리한다.

• 명목 상태: 리 군 위의 원소
• 오차 상태: 리 대수의 원소(접선 공간의 작은 벡터)

이러한 분리는 다양체 구조를 자연스럽게 반영한다.

3.2 곱셈 갱신

오차 상태가 명목 상태에 곱셈으로 적용된다.

\hat{\mathbf{R}}_{\text{new}} = \hat{\mathbf{R}}_{\text{old}}\exp([\delta\boldsymbol{\theta}]_\times)

이 갱신은 자세를 다양체 위에 유지한다.

3.3 오차의 가우시안 분포

오차 상태는 작은 벡터(접선 공간의 원소)이므로 가우시안 분포를 자연스럽게 정의할 수 있다. 이는 칼만 필터의 통계적 기반을 유지한다.

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

4.1 상태 표현

곱셈 확장 칼만 필터(Multiplicative Extended Kalman Filter, MEKF)는 자세 추정을 위한 표준 알고리즘이다. 상태는 다음과 같이 표현된다.

• 명목 자세: \hat{\mathbf{q}} \in \mathbb{S}^3 (단위 쿼터니언)
• 오차 자세: \delta\boldsymbol{\theta} \in \mathbb{R}^3 (작은 회전 벡터)

오차 자세는 명목 자세 주위의 작은 변화를 나타내며, 항상 0 근처에 머문다.

4.2 예측 단계

자이로 측정값 \boldsymbol{\omega}_{\text{meas}}로부터 명목 자세를 예측한다.

\hat{\mathbf{q}}_{k+1|k} = \hat{\mathbf{q}}_k \otimes \exp\!\left(\frac{1}{2}\boldsymbol{\omega}\Delta t\right)

여기서 \boldsymbol{\omega} = \boldsymbol{\omega}_{\text{meas}} - \hat{\mathbf{b}}_g는 바이어스 보정된 각속도이다.

오차 자세는 예측 단계에서 0으로 유지되며, 공분산만 갱신된다.

4.3 측정 갱신 단계

측정값 \mathbf{z}_k가 도착하면 오차 상태가 갱신된다.

\delta\boldsymbol{\theta} = \mathbf{K}_k(\mathbf{z}_k - h(\hat{\mathbf{q}}_{k+1|k}))

여기서 \mathbf{K}_k는 칼만 게인이다.

4.4 명목 상태로의 통합

오차 상태가 명목 상태에 통합된다.

\hat{\mathbf{q}}_{k+1} = \hat{\mathbf{q}}_{k+1|k} \otimes \exp\!\left(\frac{1}{2}\delta\boldsymbol{\theta}\right)

통합 후 오차 상태는 0으로 재설정된다.

4.5 장점

MEKF의 장점은 다음과 같다.

• 자세가 항상 단위 쿼터니언으로 유지된다.
• 오차 상태가 작아 선형화가 정확하다.
• 표준 칼만 필터의 기본 구조를 유지한다.
• 우주선 자세 결정에서 검증된 알고리즘이다.

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

5.1 상태 분리

오차 상태 칼만 필터(Error State Kalman Filter, ESKF)도 명목 상태와 오차 상태를 분리한다. MEKF와 비슷하지만, 더 풍부한 상태 변수를 가진다.

5.2 일반적 상태

ESKF의 명목 상태는 다음을 포함할 수 있다.

• 위치 \mathbf{p}
• 속도 \mathbf{v}
• 자세 쿼터니언 \mathbf{q}
• 자이로 바이어스 \mathbf{b}_g
• 가속도계 바이어스 \mathbf{b}_a

오차 상태는 각각에 대응하는 작은 변화이다.

5.3 예측 단계

명목 상태는 IMU 측정값으로 적분된다. 오차 상태의 동역학이 선형화되어 공분산이 전파된다.

5.4 측정 갱신

외부 측정(GPS, 카메라 등)이 오차 상태를 갱신한다. 갱신된 오차가 명목 상태에 통합되고, 오차는 0으로 재설정된다.

5.5 적용 분야

ESKF는 IMU와 외부 측정의 융합에 광범위하게 사용된다. PX4, ArduPilot 등의 비행 제어 시스템에서 표준이다.

6. 비주얼-관성 항법(VIO)

6.1 문제 정의

비주얼-관성 항법(Visual-Inertial Odometry, VIO)은 카메라와 IMU의 측정을 결합하여 카메라 자세를 추정한다. 상태 변수에 카메라 자세, 속도, IMU 바이어스 등이 포함된다.

6.2 다양체 구조

카메라 자세는 SE(3)의 원소이며, 다양체 구조를 활용한 추정이 필수적이다. 작은 자세 변화가 리 대수의 원소로 표현된다.

6.3 알고리즘

VIO 알고리즘에는 EKF 기반 방법과 최적화 기반 방법이 있다.

• EKF 기반: ROVIO, MSCKF 등
• 최적화 기반: VINS-Mono, OKVIS, ORB-SLAM 등

두 종류 모두 다양체 구조를 활용한다.

6.4 미리 적분

미리 적분(preintegration)은 IMU 측정값을 두 키프레임 사이에서 미리 적분하는 기법이다. 리 군 위에서의 미리 적분이 정확성과 효율성을 동시에 제공한다.

7. 입자 필터

7.1 다양체 위의 입자

입자 필터는 비가우시안 분포를 다룰 수 있다. 다양체 위의 입자 필터는 입자가 리 군의 원소이며, 다양체 위에서 운동한다.

7.2 가중치와 재샘플링

각 입자에 가중치가 부여되고, 측정값에 따라 갱신된다. 재샘플링은 다양체 위에서 수행된다.

7.3 응용

다양체 위의 입자 필터는 모바일 로봇의 위치 추정, 비주얼 객체 추적 등에 사용된다.

8. 무향 칼만 필터(UKF)

8.1 시그마 점

무향 칼만 필터(Unscented Kalman Filter, UKF)는 상태 분포를 시그마 점들로 표현한다. 이러한 시그마 점들은 평균과 공분산을 정확히 표현한다.

8.2 다양체 위의 UKF

리 군 위의 UKF는 시그마 점들을 다양체 위에 위치시킨다. 명목 상태에서 시그마 점들로의 변환과 그 역이 지수 사상과 로그 사상으로 이루어진다.

8.3 장점

UKF는 EKF보다 더 정확한 비선형 처리를 제공하며, 자코비안 계산이 필요 없다. 다양체 구조와 결합하면 강건한 자세 추정이 가능하다.

9. 그래프 SLAM

9.1 문제 정의

그래프 SLAM은 노드(자세)와 엣지(상대 자세 측정)로 구성된 그래프 위의 추정 문제이다.

9.2 비용 함수

비용 함수는 모든 엣지의 잔차의 제곱합이다.

J = \sum_{(i, j) \in E}\lVert\log(\mathbf{T}_{ij}^{-1}\mathbf{T}_i^{-1}\mathbf{T}_j)\rVert^2_{\Sigma_{ij}}

여기서 \mathbf{T}_{ij}는 측정된 상대 자세, \Sigma_{ij}는 측정 공분산이다.

9.3 최적화

비용 함수의 최적화는 가우스-뉴턴법이나 레벤버그-마쿼트법으로 수행된다. 자세 변수의 매개변수화에 리 군 이론이 활용된다.

9.4 라이브러리

그래프 SLAM을 위한 라이브러리에는 g2o, GTSAM, Ceres Solver 등이 있다.

10. 비선형 관측기

10.1 상보 필터

마호니의 비선형 상보 필터는 SO(3) 위에서 정의된 자세 추정 알고리즘이다. 가속도계와 자이로의 측정을 결합하여 자세를 추정한다.

10.2 리아푸노프 안정성

마호니 필터는 리아푸노프 안정성 이론에 의해 거의 전역적 점근 안정성이 증명된다. 이는 다양체 위의 알고리즘 분석의 우수한 예이다.

10.3 매개변수 튜닝

비선형 상보 필터는 적은 수의 매개변수만으로 동작하며, 튜닝이 간단하다.

11. 미리 적분(Preintegration)

11.1 동기

비주얼-관성 융합에서 IMU 측정값은 매우 빠르게(수백 Hz) 도착한다. 모든 측정을 키프레임 변수로 직접 처리하면 비효율적이다.

11.2 미리 적분의 정의

미리 적분은 두 키프레임 사이의 IMU 측정값을 누적하여 단일 측정값으로 표현하는 기법이다.

\Delta\mathbf{R}_{ij} = \prod_{k=i}^{j-1}\exp([(\boldsymbol{\omega}_k - \mathbf{b}_g)\Delta t]_\times)

이는 자세, 속도, 위치의 변화로 구성된다.

11.3 다양체 위의 처리

미리 적분은 다양체 위에서 정확하게 정의되며, 자세 변화가 SO(3)의 원소로 표현된다.

11.4 이점

미리 적분은 IMU 측정값의 효율적인 처리를 제공하며, 키프레임 사이의 변화만 저장하면 된다. 이는 VIO와 시각적 SLAM의 효율성을 크게 향상시킨다.

12. 분포 추정

12.1 다양체 위의 분포

자세 분포를 다양체 위에서 정의하는 다양한 방법이 있다.

12.1.1 가우시안 분포 (접선 공간)

명목 자세 주위의 접선 공간에서 정의된 가우시안 분포이다.

p(\mathbf{R}) \propto \exp\!\left(-\frac{1}{2}\lVert\log(\hat{\mathbf{R}}^T\mathbf{R})\rVert^2_\Sigma\right)

12.1.2 Bingham 분포

쿼터니언 위의 분포로, 회전의 양면적 특성을 자연스럽게 다룬다.

12.1.3 von Mises-Fisher 분포

단위 구 S^n 위의 분포이다.

12.1.4 행렬 Fisher 분포

SO(n) 위의 분포이다.

12.2 응용

이러한 분포는 자세 추정, 자세 분류, 자세 학습 등에 활용된다.

13. 학습 기반 방법

13.1 신경망 출력의 처리

신경망이 자세를 출력할 때 다양체 구조를 고려해야 한다. 단위 쿼터니언, 6D 표현, 로그 사상 등이 사용된다.

13.2 손실 함수

학습의 손실 함수는 자세 사이의 측지 거리이다.

L = \lVert\log(\mathbf{R}_{\text{pred}}^T\mathbf{R}_{\text{true}})\rVert^2

13.3 자동 미분

리 군 위의 자동 미분이 지원되어야 학습이 가능하다. 일부 라이브러리는 이를 직접 지원한다.

14. 성능 비교

14.1 표준 EKF vs 리 군 기반 EKF

14.2 적용 권장 사항

• 작은 자세 변화에서 단순함이 필요하면 표준 EKF
• 큰 자세 변화나 정확도가 중요하면 MEKF/ESKF
• 실시간 시스템에서 효율성과 정확도의 균형이 필요하면 ESKF

15. 라이브러리와 도구

15.1 Sophus

Sophus 라이브러리는 리 군 변수의 추정에 활용된다. EKF, MEKF 등의 구현에 사용된다.

15.2 GTSAM

GTSAM은 비주얼-관성 항법, 그래프 SLAM 등을 위한 라이브러리이다. 리 군 변수의 자동 미분을 내장한다.

15.3 Ceres Solver

Ceres Solver는 비선형 최소제곱 최적화 라이브러리이며, 리 군 변수를 매개변수화 클래스를 통해 지원한다.

15.4 manif

manif는 가벼운 리 군 라이브러리이며, 자코비안과 자동 미분을 지원한다.

15.5 ROS Filter

ROS의 robot_localization 패키지는 EKF/UKF 기반 상태 추정을 제공하며, 다양한 자세 표현을 지원한다.

16. 학습의 가치

리 군 기반 상태 추정 알고리즘을 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

• 자세 추정의 핵심 알고리즘을 이해할 수 있다.
• 비주얼-관성 항법, SLAM 등의 알고리즘을 설계할 수 있다.
• 다양체 구조를 활용한 정확한 추정이 가능하다.
• 학술 문헌의 표준 알고리즘을 이해할 수 있다.
• 실시간 시스템의 성능을 향상시킬 수 있다.

17. 응용 예시: 우주선 자세 결정

위성과 우주선의 자세 결정은 MEKF의 주요 응용 분야이다. 별 추적기, 자이로, 태양 센서 등의 측정을 결합하여 자세를 추정한다. 자세는 단위 쿼터니언으로 유지되며, 측정 갱신이 곱셈으로 이루어진다.

18. 응용 예시: 드론의 비주얼-관성 항법

소형 드론의 비주얼-관성 항법에서 카메라와 IMU의 측정이 결합되어 자세와 위치가 추정된다. ESKF나 최적화 기반 VIO가 사용되며, 모두 리 군 구조를 활용한다.

19. 응용 예시: 자율 주행차의 위치 추정

자율 주행차의 위치 추정은 GPS, IMU, 라이다, 카메라 등의 다중 센서 융합 문제이다. 자세는 SE(3)의 원소이며, 리 군 기반 알고리즘이 표준이다.

20. 응용 예시: 매니퓰레이터의 캘리브레이션

매니퓰레이터의 손-눈 캘리브레이션은 비선형 추정 문제이며, 미지의 변환이 SE(3)의 원소이다. 리 군 위의 최적화로 풀린다.

21. 응용 예시: 인간 동작 추적

인간의 동작을 IMU나 카메라로 추적하는 응용에서 인체 부위의 자세가 SO(3)의 원소이다. 다중 부위의 자세를 동시에 추정하는 문제는 리 군 기반 알고리즘으로 효율적으로 풀린다.

22. 알고리즘 선택 기준

22.1 문제의 특성

• 작은 자세 변화: 표준 EKF 또는 단순 알고리즘
• 큰 자세 변화: 리 군 기반 알고리즘(MEKF, ESKF)
• 비선형성: UKF 또는 입자 필터
• 다중 모드 분포: 입자 필터

22.2 계산 자원

• 고성능 시스템: 정확한 알고리즘(UKF, 최적화 기반)
• 저전력 시스템: 효율적 알고리즘(상보 필터, ESKF)

22.3 정확도 요구

• 고정밀: 그래프 SLAM, 번들 조정
• 실시간: 칼만 필터 계열

23. 미래의 발전 방향

23.1 학습 기반 추정

신경망과 칼만 필터의 결합이 활발히 연구되고 있다. 측정 모형이나 잡음 모형을 학습하는 방법이 있다.

23.2 분포 표현

복잡한 분포를 표현하는 방법이 발전하고 있다. Bingham 분포, 행렬 Fisher 분포 등이 더 널리 활용될 것이다.

23.3 미분 가능 알고리즘

자세 추정 알고리즘 자체를 미분 가능하게 만들어 학습 시스템과 통합하는 연구가 진행 중이다.

24. 참고 문헌

• 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.
• Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2017). “On-manifold preintegration for real-time visual-inertial odometry.” IEEE Transactions on Robotics, 33(1), 1–21.
• Mahony, R., Hamel, T., & Pflimlin, J. M. (2008). “Nonlinear Complementary Filters on the Special Orthogonal Group.” IEEE Transactions on Automatic Control, 53(5), 1203–1218.
• Trawny, N., & Roumeliotis, S. I. (2005). “Indirect Kalman Filter for 3D Attitude Estimation.” Technical Report, University of Minnesota.
• Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.

version: 1.0