11.21 리 군의 불확실성 표현과 공분산 전파

1. 불확실성 표현의 개요

리 군 위에서 정의되는 자세나 강체 변환의 불확실성은 일반적인 유클리드 공간의 가우시안 분포로 직접 표현할 수 없다. 다양체 구조를 고려한 적절한 불확실성 표현이 필요하며, 이는 자세 추정, SLAM, 비주얼 항법 등에서 핵심적이다. 가장 일반적인 방법은 명목 자세 주위의 접선 공간에서 가우시안 분포를 정의하는 것이다. 이를 통해 불확실성의 표현, 전파, 갱신이 일관되게 다루어진다.

2. 다양체 위의 분포

2.1 일반 유클리드 분포의 한계

일반적인 가우시안 분포는 평균 벡터와 공분산 행렬로 정의된다.

$p(\mathbf{x}) = \frac{1}{\sqrt{(2\pi)^n\det(\Sigma)}}\exp\!\left(-\frac{1}{2}(\mathbf{x} - \boldsymbol{\mu})^T\Sigma^{-1}(\mathbf{x} - \boldsymbol{\mu})\right)$

이는 $\mathbb{R}^n$ 위에서 정의되며, 다양체 위에서 직접 사용할 수 없다.

2.2 다양체 위의 가우시안

다양체 위에서 가우시안 분포를 정의하는 한 가지 자연스러운 방법은 명목 점 주위의 접선 공간을 사용하는 것이다.

$\mathbf{R} = \hat{\mathbf{R}}\exp([\boldsymbol{\xi}]_\times), \quad \boldsymbol{\xi} \sim \mathcal{N}(\mathbf{0}, \Sigma)$

여기서 $\hat{\mathbf{R}}$ 이 명목 자세이고 $\boldsymbol{\xi}$ 는 접선 공간(리 대수)의 가우시안 변수이다.

2.3 표현의 의미

이러한 표현은 자세의 작은 변화를 가우시안으로 모형화한다. 큰 변화에서는 가우시안 가정이 정확하지 않을 수 있지만, 추정 알고리즘에서는 일반적으로 작은 변화를 다루므로 적합하다.

3. 우측 곱과 좌측 곱 표현

3.1 우측 곱 표현

위에서 사용된 표현은 우측 곱이며, 본체 좌표계에서의 작은 회전을 표현한다.

$\mathbf{R} = \hat{\mathbf{R}}\exp([\boldsymbol{\xi}_b]_\times)$

3.2 좌측 곱 표현

좌측 곱 표현은 공간 좌표계에서의 작은 회전을 표현한다.

$\mathbf{R} = \exp([\boldsymbol{\xi}_s]_\times)\hat{\mathbf{R}}$

3.3 두 표현의 변환

두 표현은 수반 표현으로 변환된다.

$\boldsymbol{\xi}_s = \hat{\mathbf{R}}\boldsymbol{\xi}_b = \mathrm{Ad}_{\hat{\mathbf{R}}}(\boldsymbol{\xi}_b)$

3.4 공분산의 변환

따라서 두 표현의 공분산도 수반 표현으로 변환된다.

$\Sigma_s = \mathbf{R}\Sigma_b\mathbf{R}^T = [\mathrm{Ad}_{\hat{\mathbf{R}}}]\Sigma_b[\mathrm{Ad}_{\hat{\mathbf{R}}}]^T$

4. $SE(3)$ 위의 불확실성

4.1 표현

$SE(3)$ 위의 자세 $\mathbf{T} = (\mathbf{R}, \mathbf{p})$ 의 불확실성은 다음과 같이 표현된다.

$\mathbf{T} = \hat{\mathbf{T}}\exp(\hat{\boldsymbol{\xi}}), \quad \boldsymbol{\xi} \sim \mathcal{N}(\mathbf{0}, \Sigma)$

여기서 $\boldsymbol{\xi} \in \mathbb{R}^6$ 는 트위스트(회전 부분과 병진 부분의 결합)이고 $\Sigma$ 는 $6 \times 6$ 공분산 행렬이다.

4.2 회전과 병진의 결합

$SE(3)$ 의 공분산은 회전과 병진 사이의 결합을 표현할 수 있다. 일반적으로 두 부분 사이에 상관관계가 존재한다.

$\Sigma = \begin{bmatrix}\Sigma_{\boldsymbol{\omega}\boldsymbol{\omega}} & \Sigma_{\boldsymbol{\omega}\mathbf{v}} \\ \Sigma_{\boldsymbol{\omega}\mathbf{v}}^T & \Sigma_{\mathbf{v}\mathbf{v}}\end{bmatrix}$

비대각 블록이 회전과 병진 사이의 상관관계를 표현한다.

5. 공분산 전파

5.1 결정론적 함수의 변환

함수 $f : G \to G'$ 에 대해 입력의 불확실성으로부터 출력의 불확실성을 계산하는 것이 공분산 전파이다.

$\mathbf{y} = f(\mathbf{x}), \quad \Sigma_{\mathbf{y}} = \mathbf{J}\Sigma_{\mathbf{x}}\mathbf{J}^T$

여기서 $\mathbf{J}$ 는 $f$ 의 자코비안이며, 리 군 위의 미분이다.

5.2 자세 합성

두 자세의 합성은 다음과 같다.

$\mathbf{T}_3 = \mathbf{T}_1\mathbf{T}_2$

각 자세가 불확실성을 가지면 결과의 불확실성도 계산해야 한다.

5.3 차 근사

작은 불확실성에 대해 1차 근사가 사용된다.

$\Sigma_3 \approx [\mathrm{Ad}_{\mathbf{T}_2^{-1}}]\Sigma_1[\mathrm{Ad}_{\mathbf{T}_2^{-1}}]^T + \Sigma_2$

이는 두 불확실성이 어떻게 결합되는지를 보여준다.

5.4 자세 역전

자세의 역원의 불확실성은 다음과 같다.

$\mathbf{T}^{-1} = \hat{\mathbf{T}}^{-1}\exp(-[\mathrm{Ad}_{\hat{\mathbf{T}}^{-1}}]\boldsymbol{\xi})$

따라서 공분산은

$\Sigma_{\mathbf{T}^{-1}} = [\mathrm{Ad}_{\hat{\mathbf{T}}^{-1}}]\Sigma_{\mathbf{T}}[\mathrm{Ad}_{\hat{\mathbf{T}}^{-1}}]^T$

6. 측정 모형의 야코비안

6.1 측정 함수

측정 함수 $h : G \to \mathbb{R}^k$ 에 대해 측정 잔차의 자세에 대한 자코비안이 필요하다.

$\mathbf{H} = \frac{\partial h}{\partial\boldsymbol{\xi}}\bigg|_{\boldsymbol{\xi} = \mathbf{0}}$

이 자코비안이 칼만 필터의 측정 자코비안에 해당한다.

6.2 연쇄 법칙

복잡한 측정 함수의 자코비안은 연쇄 법칙을 사용하여 계산된다. 각 단계에서 리 군 위의 미분이 등장한다.

7. 자세 추정의 공분산 갱신

7.1 칼만 갱신

칼만 필터의 측정 갱신 단계에서 공분산이 갱신된다.

$\Sigma_+ = (\mathbf{I} - \mathbf{K}\mathbf{H})\Sigma_-$

여기서 $\mathbf{K}$ 는 칼만 게인이다. 리 군 변수의 경우 $\Sigma$ 는 접선 공간의 공분산이다.

7.2 명목 상태의 갱신

오차 상태의 평균이 칼만 게인을 통해 계산되고, 이것이 명목 상태에 곱해진다.

$\hat{\mathbf{T}}_+ = \hat{\mathbf{T}}_-\exp(\hat{\boldsymbol{\delta}})$

오차 상태는 갱신 후 0으로 재설정된다.

8. 불확실성의 시각화

8.1 공분산 타원체

공분산 행렬은 타원체로 시각화될 수 있다. 6자유도 자세의 경우 6차원 타원체이지만, 위치와 회전을 분리하여 3차원 타원체로 시각화할 수 있다.

8.2 표시

자세 추정 알고리즘의 결과를 시각화할 때 공분산 타원체가 자주 사용된다. 이는 추정의 신뢰도를 직관적으로 보여준다.

8.3 제약

선형화 기반 표현은 큰 불확실성에서 부정확할 수 있다. 큰 불확실성에서는 다른 시각화 방법(예: 입자 표시)이 필요할 수 있다.

9. 비가우시안 분포

9.1 다양체 위의 다른 분포

가우시안 외에도 다양체 위에서 정의되는 분포들이 있다.

9.1.1 Bingham 분포

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

$p(\mathbf{q}) \propto \exp(\mathbf{q}^T\mathbf{M}\mathbf{q})$

여기서 $\mathbf{M}$ 은 대칭 행렬이며, 분포의 형태를 결정한다.

9.1.2 von Mises-Fisher 분포

$n$ 차원 단위 구 위의 분포이다.

$p(\mathbf{x}) \propto \exp(\kappa\boldsymbol{\mu}^T\mathbf{x})$

여기서 $\boldsymbol{\mu}$ 는 평균 방향, $\kappa$ 는 집중도이다.

9.1.3 행렬 Fisher 분포

$SO(n)$ 위의 분포이다.

$p(\mathbf{R}) \propto \exp(\mathrm{tr}(\mathbf{F}^T\mathbf{R}))$

여기서 $\mathbf{F}$ 는 분포 매개변수이다.

9.2 응용

이러한 분포들은 큰 불확실성이나 다중 모드 분포가 필요한 응용에서 활용된다.

10. 입자 기반 표현

10.1 입자 필터

입자 필터는 분포를 입자(샘플)들의 집합으로 표현한다. 다양체 위에서도 입자 필터가 적용된다.

10.2 다양체 위의 입자

다양체 위의 입자는 그 다양체의 원소이다. 자세의 경우 각 입자가 $SE(3)$ 의 원소이다.

10.3 가중치와 재샘플링

입자 필터의 표준 알고리즘이 다양체 위에서도 동작한다. 가중치 갱신과 재샘플링이 이루어진다.

10.4 응용

입자 필터는 비가우시안 분포나 다중 모드 분포를 다루는 데 효과적이다. 모바일 로봇의 위치 추정 등에서 활용된다.

11. 자코비안 계산의 예

11.1 자세 합성의 자코비안

$\mathbf{T}_3 = \mathbf{T}_1\mathbf{T}_2$ 의 자코비안은 다음과 같다.

$\frac{\partial\mathbf{T}_3}{\partial\boldsymbol{\xi}_1} = [\mathrm{Ad}_{\mathbf{T}_2^{-1}}], \quad \frac{\partial\mathbf{T}_3}{\partial\boldsymbol{\xi}_2} = \mathbf{I}_6$

여기서 $\boldsymbol{\xi}_i$ 는 $\mathbf{T}_i$ 의 우측 곱 매개변수화이다.

11.2 자세 역전의 자코비안

$\mathbf{T}^{-1}$ 의 자코비안은 다음과 같다.

$\frac{\partial\mathbf{T}^{-1}}{\partial\boldsymbol{\xi}} = -[\mathrm{Ad}_{\mathbf{T}^{-1}}]$

11.3 자세 비교의 자코비안

자세 비교 $\Delta\mathbf{T} = \mathbf{T}_1^{-1}\mathbf{T}_2$ 의 자코비안은 두 자세 모두에 대해 계산된다. 리 군의 미분 공식을 사용하여 유도된다.

12. 불확실성의 통계적 검증

12.1 카이 제곱 검정

추정 결과의 신뢰성을 평가하기 위해 카이 제곱 검정이 사용된다.

$\chi^2 = \boldsymbol{\xi}^T\Sigma^{-1}\boldsymbol{\xi}$

이 값이 임계값을 초과하면 추정에 문제가 있을 가능성이 있다.

12.2 마할라노비스 거리

마할라노비스 거리(Mahalanobis distance)는 두 분포 사이의 거리를 측정한다. 측정값과 예측값의 차이를 검사하는 데 사용된다.

12.3 일관성 검증

추정 알고리즘의 일관성은 정규화된 추정 오차 제곱(NEES)이나 유사한 지표로 검증된다.

13. 응용 분야

13.1 자세 추정

자세 추정 알고리즘에서 불확실성 표현은 추정의 신뢰도를 정량화한다. 칼만 필터, MEKF, ESKF 등이 모두 공분산을 활용한다.

13.2 시각적 SLAM

번들 조정에서 공분산 행렬이 추정의 정확도를 표현한다. 회로 폐쇄 검출의 강건성에도 활용된다.

13.3 운동 계획

운동 계획에서 자세의 불확실성을 고려한 강건한 경로 계획이 가능하다. 불확실성이 큰 영역을 회피하거나 정보가 많은 영역으로 이동한다.

13.4 상호작용 안전

인간-로봇 상호작용에서 로봇의 자세 추정 불확실성이 안전 한계의 결정에 사용된다.

13.5 다중 센서 융합

여러 센서의 측정을 결합할 때 각 센서의 불확실성이 가중치로 사용된다. 정확도가 높은 측정이 더 큰 가중치를 받는다.

14. 학습의 가치

리 군의 불확실성 표현과 공분산 전파를 이해하는 것은 다음과 같은 이점을 제공한다.

자세 추정 알고리즘의 통계적 기반을 이해할 수 있다.
공분산 행렬의 의미와 활용을 알 수 있다.
다양체 구조를 고려한 정확한 추정이 가능하다.
학술 문헌의 표준 표기를 이해할 수 있다.
실제 시스템의 성능 평가가 가능하다.

15. 응용 예시: 자세 추정의 불확실성

자세 추정 알고리즘이 매 갱신마다 자세의 평균과 공분산을 출력한다. 공분산 행렬의 크기가 추정의 불확실성을 나타내며, 작을수록 정확한 추정이다.

16. 응용 예시: 시각적 SLAM의 회로 폐쇄

회로 폐쇄 검출에서 후보 자세와 현재 자세의 차이가 측정된다. 이 차이가 추정 불확실성 내에 있으면 회로 폐쇄로 받아들여진다. 이는 마할라노비스 거리 검정으로 수행된다.

17. 응용 예시: 매니퓰레이션의 자세 정확도

매니퓰레이터가 부품을 조립할 때 도달한 자세의 불확실성이 작아야 한다. 자세 추정의 공분산이 작업 허용 한계 내에 있어야 정확한 작업이 가능하다.

18. 응용 예시: 자율 주행의 위치 신뢰도

자율 주행 차량의 위치 추정 불확실성이 차선 변경 등 결정에 사용된다. 불확실성이 크면 안전한 동작을 우선하고, 작으면 더 적극적인 동작이 가능하다.

19. 분포의 결합

19.1 곱셈

두 가우시안 분포의 곱은 다시 가우시안이며, 평균과 공분산이 다음과 같이 결합된다.

$\Sigma_{\text{combined}}^{-1} = \Sigma_1^{-1} + \Sigma_2^{-1}$

$\boldsymbol{\mu}_{\text{combined}} = \Sigma_{\text{combined}}(\Sigma_1^{-1}\boldsymbol{\mu}_1 + \Sigma_2^{-1}\boldsymbol{\mu}_2)$

이는 두 추정의 융합에 사용된다.

19.2 다양체 위의 융합

다양체 위에서의 분포 결합은 명목 점 주위의 선형화를 통해 이루어진다. 공통 명목 점을 선택하고 각 분포를 그 점 주위로 변환한 후 결합한다.

20. 미래의 발전

20.1 비선형 분포

가우시안 가정의 한계를 넘는 비선형 분포의 활용이 발전하고 있다. 학습 기반 분포 모형이 그 한 예이다.

20.2 미분 가능 추정

미분 가능한 추정 알고리즘이 학습 시스템과의 통합을 가능하게 한다.

20.3 분산 추정

여러 로봇이나 센서가 협력하는 분산 추정에서 불확실성의 공유와 결합이 핵심이다.

21. 참고 문헌

Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
Chirikjian, G. S. (2011). Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications. Birkhäuser.
Sola, J., Deray, J., & Atchuthan, D. (2018). “A Micro Lie Theory for State Estimation in Robotics.” arXiv:1812.01537.
Markley, F. L., & Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. Springer.
Wang, Y., & Chirikjian, G. S. (2008). “Nonparametric second-order theory of error propagation on motion groups.” International Journal of Robotics Research, 27(11–12), 1258–1273.
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.

version: 1.0