제1부 수학적 기초

1.8 리군·리대수 II: 섭동·리 야코비안·불확실성 전파 — manif·Sophus 구현

1.8.1 이 장에서 배우는 것 / 선수 지식 / 특히 중요한 독자

이 장은 리군 위의 함수를 접공간에서 미분하는 방법 — 섭동 연산자 $\oplus$·$\ominus$, 수반, 리 야코비안 — 과, 이를 이용해 자세의 불확실성을 접공간의 공분산으로 전파하는 방법을 다룬다. 이 도구들이 자세 그래프 최적화, 오차상태 칼만 필터, 추측 항법의 불확실성 추정의 공통 수학이다. 앞 장(1.7)의 지수·로그 사상 위에서, 작은 섭동에 대한 합성·역·벡터 작용의 야코비안과 공분산 전파를 유도하고 수치로 확인한다. 라이브러리(manif·Sophus)의 규약 확인 사항을 함께 정리한다. 다룰 질문은 네 가지다. 오른쪽 섭동과 왼쪽 섭동은 어떻게 대응하는가, $SO(3)$의 야코비안과 연산의 야코비안은 무엇이며 어떻게 검증하는가, 접공간의 공분산은 합성에서 어떻게 전파되고 어디서 틀리는가, 라이브러리를 선택할 때 확인할 규약은 무엇인가.

배우는 것:

• 섭동 연산자와 수반 — $X\oplus\tau=X\,\mathrm{Exp}(\tau)$, $Y\ominus X=\mathrm{Log}(X^{-1}Y)$, $\mathrm{Ad}_X$.
• 리 야코비안 — $J_r$, $J_l$, $J_r^{-1}$의 식과 의미.
• 연산의 야코비안 — 합성·역·벡터 작용, 오른쪽 섭동 기준.
• 불확실성 전파 — 접공간의 가우시안, $\Sigma_Z=J_X\Sigma_XJ_X^\top+J_Y\Sigma_YJ_Y^\top$, 추측 항법의 불확실성 증가, 일차 근사의 한계.
• 라이브러리 규약 — manif·Sophus의 공개 정보와 확인할 항목.
• 풀이 예제 — 유한 차분 검증, 몬테카를로 공분산, 추측 항법.

선수 지식: 1.7(지수·로그 사상), 1.5(쿼터니언), 1.19(확률·가우시안; 공분산). 표기는 BK-05와 A.2을 따른다.

특히 중요한 독자: 추정·최적화 연구자·엔지니어, 소프트웨어 개발자(리군 라이브러리의 사용), 불확실성의 일관성을 검증하는 독자.

출처 방침. 정리는 원 논문과 교과서에 근거하며 서지는 arXiv 응답으로 확인했다(목록은 이 장 끝). 직접 읽은 자료: ① Solà, Deray, Atchuthan, 『A micro Lie theory for state estimation in robotics』(arXiv:1812.01537)의 $SE(3)$ 부록(수반 $\mathrm{Ad}_M\tau=(R\rho+[t]_\times R\theta,\ R\theta)$, 식 (179)·(180)의 $J_l$과 결합 항 $Q$). ② GitHub API의 artivis/manif·strasdat/Sophus 저장소 정보와 manif의 README(조회 2026-11-03). 읽지 못한 것: 두 라이브러리의 소스와 문서(성분 순서·규약은 (확인 필요)), 교과서 본문. 수치는 모두 이 장의 순수 파이썬 구현으로 계산했으며 1.7의 지수·로그, 1.5의 쿼터니언 곱, 1.1의 행렬 곱을 불러와 사용했다.

1.8.2 개념 — 오차를 접공간에 두기

회전은 덧셈이 정의되지 않아 오차 $e=R_{\mathrm{est}}-R_{\mathrm{true}}$를 직접 다룰 수 없다. 오차를 접공간의 3개 수 $\epsilon$로 두고 $R=\bar R\,\mathrm{Exp}(\epsilon)$로 적으면 오차가 최소 표현이고 가우시안 $\epsilon\sim\mathcal N(0,\Sigma)$로 모델링된다. 자세를 갱신하는 단계도 $R\leftarrow R\,\mathrm{Exp}(\delta)$로 하면 항상 회전 행렬이 유지된다. 이 구조에서 필요한 것은 (1) 접공간의 섭동이 함수의 값에 주는 영향, 곧 야코비안, (2) 섭동을 기준 프레임에서 정의하는 방식(오른쪽·왼쪽)의 차이이다.

1.8.3 원리

1.8.3.1 섭동 연산자와 수반

리군 원소 $X$와 접공간 원소 $\tau$에 대해

$$X\oplus\tau=X\,\mathrm{Exp}(\tau)\ (\text{오른쪽}),\qquad \tau\oplus X=\mathrm{Exp}(\tau)\,X\ (\text{왼쪽}),\qquad Y\ominus X=\mathrm{Log}(X^{-1}Y)$$

이다. 오른쪽 섭동은 $X$의 지역 좌표에서, 왼쪽 섭동은 세계 좌표에서의 오차이다. 두 섭동은 수반 $\mathrm{Ad}_X$로 이어진다.

$$X\,\mathrm{Exp}(\tau)=\mathrm{Exp}(\mathrm{Ad}_X\tau)\,X$$

$SO(3)$에서 $\mathrm{Ad}_R=R$이다($R\,\mathrm{Exp}(\tau)R^\top=\mathrm{Exp}(R\tau)$). $SE(3)$에서는 이 책의 트위스트 순서 $(\omega,v)$로 $\mathrm{Ad}_T=\begin{bmatrix}R&0\\[p]_\times R&R\end{bmatrix}$이고, Solà 외의 순서 $(\rho,\theta)$로는 $\mathrm{Ad}_T=\begin{bmatrix}R&[p]_\times R\\0&R\end{bmatrix}$이다(이 장이 읽은 식). 두 행렬은 블록 순서만 다르다(1.7.3.1).

1.8.3.2 $SO(3)$의 리 야코비안

오른쪽 야코비안 $J_r$은 접공간의 작은 변화가 오른쪽 섭동으로 나타나는 정도이다.

$$\mathrm{Exp}(\omega+\delta)\approx\mathrm{Exp}(\omega)\,\mathrm{Exp}\bigl(J_r(\omega)\delta\bigr),\qquad \mathrm{Log}\bigl(\mathrm{Exp}(\omega)\mathrm{Exp}(\delta)\bigr)\approx\omega+J_r^{-1}(\omega)\delta$$

식은 다음과 같다($\theta=\lVert\omega\rVert$).

$$J_r(\omega)=I-\frac{1-\cos\theta}{\theta^2}[\omega]_\times+\frac{\theta-\sin\theta}{\theta^3}[\omega]_\times^2,\qquad J_r^{-1}(\omega)=I+\frac12[\omega]_\times+\Bigl(\frac1{\theta^2}-\frac{1+\cos\theta}{2\theta\sin\theta}\Bigr)[\omega]_\times^2$$

왼쪽 야코비안은 $J_l(\omega)=J_r(-\omega)=J_r(\omega)^\top$이며 1.7의 $V(\theta)$와 같다. $SE(3)$의 야코비안은 이동 블록에 결합 항 $Q(\rho,\theta)$가 더해진다(Solà 외, 식 (179)·(180)). 이 장은 $SE(3)$ 야코비안을 구현하지 않았다.

1.8.3.3 연산의 야코비안

접공간의 오른쪽 섭동에 대한 함수의 야코비안은 연쇄 법칙으로 이어진다. 이 장이 수치로 확인한 연산은 다음과 같다.

이 야코비안으로 비선형 최소제곱의 자세 갱신(1.12)과 오차상태 칼만 필터(9.4)의 선형화가 이루어진다.

1.8.3.4 불확실성의 전파

접공간의 가우시안 $X=\bar X\,\mathrm{Exp}(\epsilon_X)$, $\epsilon_X\sim\mathcal N(0,\Sigma_X)$와 독립인 $Y=\bar Y\,\mathrm{Exp}(\epsilon_Y)$에서 합성 $Z=XY$의 접공간 공분산은 일차 근사로

$$\Sigma_Z=J^Z_X\Sigma_X(J^Z_X)^\top+J^Z_Y\Sigma_Y(J^Z_Y)^\top=\mathrm{Ad}_{Y^{-1}}\Sigma_X\mathrm{Ad}_{Y^{-1}}^\top+\Sigma_Y$$

이다. 추측 항법에서 평균이 항등인 등방 증분 $\Sigma=\sigma^2I$를 $n$번 합성하면 $\mathrm{Ad}$가 직교이므로 $\Sigma_n=n\sigma^2I$이며 회전 벡터의 표준편차가 $\sqrt n\,\sigma$로 늘어난다. 이 근사는 섭동이 작을 때에만 유효하다. 크기가 커지면 2차 효과(분포의 휨)가 나타난다(1.8.5에서 확인). ROS의 6차원 공분산은 회전 성분을 고정축 표현으로 정의하며(REP-103, 1.3), 이는 세계 좌표의 섭동(왼쪽 섭동)에 해당하는 것으로 읽히므로 오른쪽 섭동 공분산을 내보낼 때 $\mathrm{Ad}_X$로 변환해야 할 수 있다 (확인 필요).

1.8.3.5 라이브러리와 확인할 규약

라이브러리를 선택하거나 문헌의 식을 옮길 때 확인할 규약은 네 가지다. ① 트위스트 성분 순서(이동 먼저 대 각속도 먼저), ② 섭동의 쪽(오른쪽·왼쪽)과 야코비안의 정의, ③ 공분산이 정의된 접공간의 쪽, ④ 쿼터니언 성분 순서(1.5). 두 라이브러리의 소스와 문서는 이 장이 읽지 않아 각 규약은 (확인 필요)이다.

1.8.4 알고리즘

오른쪽 섭동 갱신과 공분산 전파

갱신:      R ← R ⊕ δ = R Exp(δ)                   (R은 항상 회전 행렬)
상대 오차: e = R1 ⊖ R0 = Log(R0ᵀ R1)
합성의 공분산(독립, 평균 X̄, Ȳ):
    Σ_Z ← Ȳᵀ Σ_X Ȳ + Σ_Y                         (SO(3): Ad_{Y⁻¹} = Yᵀ)
선형화(오차상태 필터·최소제곱):
    잔차 r(X ⊕ δ) ≈ r(X) + J δ,   J는 연쇄 법칙(Ad, J_r)으로 구함

구현은 1.8.5의 코드에 있다.

1.8.5 풀이 예제 — 야코비안 검증·공분산 전파·추측 항법

$SO(3)$의 야코비안을 유한 차분으로 검증하고, 접공간 공분산의 합성 전파를 몬테카를로와 비교하며, 추측 항법의 불확실성 증가를 계산한다. 결론부터 적으면 다음과 같다. ① $J_r$과 $J_r^{-1}$의 일차 근사 오차는 섭동 크기 $\delta$에 대해 $\delta^2$으로 줄고 $J_rJ_r^{-1}=I$는 $10^{-16}$ 안에서 성립한다. ② 합성·역·벡터 작용의 해석적 야코비안은 수치 야코비안과 $10^{-10}$ 안에서 일치한다. ③ 접공간 공분산의 합성 전파는 $\sigma=0.1$ rad까지 상대 오차 1%(표본 잡음 수준)이고 $\sigma=0.6$ rad에서 19%이다. ④ 추측 항법의 불확실성은 $\sqrt n\,\sigma$로 늘며 $n=320$(표준편차 0.36 rad)에서 예측과 2.5% 안에서 일치한다.

구현. jr·jl·jr_inv·box_plus·box_minus·cov_compose. 1.7의 exp_so3·log_so3, 1.1의 행렬 곱, 몬테카를로의 증분 합성은 1.5의 쿼터니언 곱

import math

def _jac(w, s):
    """I + s b W + c W^2 (s=1 왼쪽, s=-1 오른쪽)"""
    _, b, c = coef(math.sqrt(sum(x * x for x in w)))
    W = hat(w)
    W2 = matmul(W, W)
    return [[(i == j) + s * b * W[i][j] + c * W2[i][j]
             for j in range(3)] for i in range(3)]

def jr(w):
    return _jac(w, -1)

def jl(w):
    return _jac(w, 1)

def jr_inv(w):
    """J_r^-1 = I + W/2 + (1/θ² − (1+cosθ)/(2θ sinθ)) W^2"""
    th = math.sqrt(sum(x * x for x in w))
    k = 1 / 12 if th < 1e-2 else (1 / th ** 2
        - (1 + math.cos(th)) / (2 * th * math.sin(th)))
    W = hat(w)
    W2 = matmul(W, W)
    return [[(i == j) + 0.5 * W[i][j] + k * W2[i][j]
             for j in range(3)] for i in range(3)]

def box_plus(R, w):
    """R ⊕ w = R Exp(w) (오른쪽 섭동)"""
    return matmul(R, exp_so3(w))

def box_minus(R1, R0):
    """R1 ⊖ R0 = Log(R0^T R1)"""
    return log_so3(matmul(transpose(R0), R1))

def cov_compose(Sx, Sy, Ry):
    """Z = X Y의 접공간 공분산(독립): Ry^T Sx Ry + Sy"""
    A = matmul(matmul(transpose(Ry), Sx), Ry)
    return [[A[i][j] + Sy[i][j] for j in range(3)]
            for i in range(3)]

① 리 야코비안의 검증. 일차 근사의 오차를 섭동 크기로 나눈 값은 다음과 같다.

오차를 $\delta$로 나눈 값이 $\delta$에 비례해 줄어(기울기 1) 근사의 오차가 $\delta^2$이다. 대수적 항등식은 다음과 같다.

② 연산의 야코비안. 오른쪽 섭동에 대한 수치 야코비안(중심차분)과 해석식의 차이는 다음과 같다.

③ 접공간 공분산의 합성 전파. 평균 자세 $\bar X$, $\bar Y$를 고정하고 오차 $\epsilon_X$, $\epsilon_Y$를 가우시안으로 뽑아 $Z=XY$를 만든 뒤 $\mathrm{Log}(\bar Z^{-1}Z)$의 표본 공분산을 예측과 비교한다.

$\sigma\le0.1$ rad에서 상대 오차는 표본 크기(20,000개)에 의한 통계 잡음 수준인 약 1%이고, $\sigma=0.3$ rad에서 3%, $\sigma=0.6$ rad에서 19%로 커진다. 일차 근사가 큰 불확실성에서 분포의 휨을 놓치기 때문이다. 이 오차의 비례(2차)는 이 장의 표본에서 관측한 것이다.

④ 추측 항법의 불확실성 증가.

⑤ 라이브러리(조회 2026-11-03).

manif의 마지막 푸시는 2026-08-05이고 Sophus의 마지막 푸시는 2024-07-06이다. 푸시 날짜는 유지의 대리 지표일 뿐이다(0.18).

한계. 구현은 $SO(3)$이며 $SE(3)$의 야코비안·공분산 전파(이동 블록의 결합)는 다루지 않았다. 가우시안은 접공간에서 가정했고 실제 센서의 오차 분포는 아니다. 일관성(필터의 공분산이 실제 오차를 반영하는가)은 9.14이 다룬다. 몬테카를로 표본 수(20,000개·3,000개)에 따른 통계 잡음이 있다.

견해: 이 책은 자세의 오차와 갱신을 오른쪽 섭동(지역 좌표)으로 통일하고, 다른 좌표(왼쪽·세계)로 공분산을 내보낼 때만 수반으로 변환하는 것을 기본으로 둔다고 본다. 근거는 위 예제에서 오른쪽 섭동의 야코비안이 $\mathrm{Ad}_{Y^{-1}}$와 $I$로 단순하고 공분산이 항등 평균의 등방 증분에서 $n\sigma^2$로 정확히 늘어난다는 점이다. 큰 불확실성에서는 일차 전파가 틀리므로(19%) 시그마점 등 다른 방법을 검토한다(9.4).

1.8.6 설계 트레이드오프 — 섭동의 쪽과 공분산의 정의

자료에서 확인되는 선택지는 세 가지다.

• ① 오른쪽 섭동(지역 좌표). 야코비안이 단순하다($J^{XY}_Y=I$). 오차가 물체 좌표에 정의된다. 센서 모델의 선형화가 쉽다.
• ② 왼쪽 섭동(세계 좌표). 오차가 세계 좌표에 정의되어 지도·전역 제약과 가깝다. 합성의 야코비안이 수반으로 바뀐다.
• ③ 로그 좌표에서의 직접 가우시안. 회전 벡터를 직접 가우시안으로 두는 단순화. 큰 각에서 부정확하다.

견해: 한 프로젝트 안에서 섭동의 쪽과 공분산의 정의를 하나로 정하고 입출력 경계에서만 변환하는 것이 선택 자체보다 중요하다고 본다. 근거는 이 장이 확인한 오류 원인(성분 순서·섭동의 쪽·쿼터니언 순서)이 모두 규약의 암묵성에서 왔다는 점이다.

1.8.7 응용 축 비교

이 표는 이 책의 정리이며 장별 요구는 다르다.

1.8.8 관련 규격·프로토콜

1.8.9 관련 규제·인증 요구와의 연결

직접 적용되는 규제는 없다. 불확실성 추정의 일관성은 안전 기능의 입력(위치 오차 범위)의 신뢰도로 이어지며 안전 관련 소프트웨어의 검증 항목이 될 수 있다. 산업·서비스 로봇의 안전 기능은 27.10·26.20이 다룬다.

1.8.10 한계·유의점·미해결

• $SE(3)$. 이동 블록의 결합 항 $Q$와 $SE(3)$ 야코비안은 구현하지 않았다.
• 일차 근사. 큰 불확실성(예: $\sigma=0.6$ rad)에서 19%의 공분산 오차가 있다.
• 라이브러리 규약. manif·Sophus의 성분 순서와 정의는 소스·문서를 읽지 않아 확인하지 않았다 (확인 필요).
• ROS 공분산의 쪽. REP-103의 고정축 표현이 왼쪽 섭동에 해당한다는 읽기는 확인하지 않았다 (확인 필요).
• 정리의 출처. 교과서 정리 번호는 확인하지 않았다 (확인 필요).

1.8.11 체크포인트·연구 질문

체크포인트:

1. $\oplus$·$\ominus$의 오른쪽·왼쪽 정의와 수반 $\mathrm{Ad}_X$의 관계를 설명할 수 있는가.
2. $J_r$·$J_r^{-1}$의 식과 $J_l=J_r^\top$를 쓸 수 있는가.
3. 합성·역·벡터 작용의 야코비안을 오른쪽 섭동으로 유도하고 부호를 점검할 수 있는가.
4. 접공간 공분산의 합성 전파식과 추측 항법의 $\sqrt n\,\sigma$ 증가를 설명할 수 있는가.
5. 일차 전파가 틀리는 조건(큰 불확실성)을 예제의 표에서 읽을 수 있는가.
6. 리군 라이브러리를 선택할 때 확인할 네 가지 규약을 말할 수 있는가.
7. ROS 6차원 공분산의 고정축 정의를 오른쪽 섭동 공분산과 연결할 때의 주의를 설명할 수 있는가.

연구 질문(47.12에 수록한다):

• manif·Sophus 등의 $\mathfrak{se}(3)$ 성분 순서와 야코비안 정의는 공개 문서에서 어떻게 표기되며 변환 오류는 얼마나 보고되는가.
• 큰 불확실성에서 시그마점·비선형 전파와 일차 전파의 공분산 차이는 로봇 응용에서 어느 크기부터 실제 오차 예측에 영향을 주는가.
• ROS의 자세 공분산을 내보내는 노드는 오른쪽·왼쪽 섭동 중 어느 쪽을 사용하는가 (확인 필요).

1차 출처

• J. Solà, J. Deray, D. Atchuthan, 「A micro Lie theory for state estimation in robotics」, arXiv:1812.01537, 2018(부록 $SE(3)$ 읽음).
• GitHub API: artivis/manif(MIT, 최신 릴리스 0.0.5·2024-08-11, 마지막 푸시 2026-08-05), strasdat/Sophus(NOASSERTION, 최신 릴리스 1.24.6·2024-06-29, 마지막 푸시 2024-07-06), 조회 2026-11-03; manif의 README.
• T. D. Barfoot, 『State Estimation for Robotics』, Cambridge University Press, 2017 (확인 필요: 장 번호).
• B. C. Hall, 『Lie Groups, Lie Algebras, and Representations』, 2판, Springer, 2015 (확인 필요: 장 번호).
• REP-103 「Standard Units of Measure and Coordinate Conventions」(1.3).

다음 장. 1.9은 리만 기하 기초로, 다양체·접공간·측지선·평행 이동·SPD 다양체를 다룬다. 이 장까지의 리군을 일반 다양체로 넓혀 공분산 행렬 같은 대칭 양정부호 행렬의 공간에서 거리와 평균을 정의하는 방법을 정리한다.