6.144 배치 추정과 최소 제곱 최적화

로봇 시스템의 상태 추정에서 배치 추정(batch estimation)은 일정 시간 구간에 걸쳐 수집된 모든 측정치를 한꺼번에 처리하여 최적의 상태 궤적을 구하는 기법이다. 이 접근법은 재귀적(recursive) 필터와 대조적으로, 전체 시간 윈도 내의 측정 데이터를 동시에 활용하므로 보다 정밀한 추정 결과를 얻을 수 있다. 배치 추정의 핵심 수학적 도구는 최소 제곱 최적화(least squares optimization)이며, 본 절에서는 이를 선형대수학적 관점에서 체계적으로 다룬다.

1. 배치 추정 문제의 정식화

시간 단계 t = 0, 1, \dots, T에서 로봇의 상태 벡터를 \mathbf{x}_t \in \mathbb{R}^n이라 하고, 전체 상태 궤적을 하나의 벡터로 적층(stacking)하면 다음과 같다.

\mathbf{x} = \begin{bmatrix} \mathbf{x}_0 \\ \mathbf{x}_1 \\ \vdots \\ \mathbf{x}_T \end{bmatrix} \in \mathbb{R}^{n(T+1)}

각 시간 단계에서의 동역학 모델(프로세스 모델)과 측정 모델은 다음과 같이 주어진다.

\mathbf{x}_{t+1} = F_t \mathbf{x}_t + \mathbf{w}_t, \quad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)

\mathbf{z}_t = H_t \mathbf{x}_t + \mathbf{v}_t, \quad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, R_t)

여기서 F_t \in \mathbb{R}^{n \times n}는 상태 전이 행렬, H_t \in \mathbb{R}^{m_t \times n}는 관측 행렬, Q_t와 R_t는 각각 프로세스 잡음과 측정 잡음의 공분산 행렬이다.

2. 결합 잔차 벡터의 구성

배치 추정에서는 프로세스 모델 잔차와 측정 모델 잔차를 하나의 결합 잔차 벡터로 구성한다. 프로세스 모델 잔차를 다음과 같이 정의한다.

\mathbf{e}_{p,t} = \mathbf{x}_{t+1} - F_t \mathbf{x}_t, \quad t = 0, 1, \dots, T-1

측정 모델 잔차는 다음과 같다.

\mathbf{e}_{m,t} = \mathbf{z}_t - H_t \mathbf{x}_t, \quad t = 0, 1, \dots, T

초기 상태에 대한 사전 정보가 \mathbf{x}_0 \sim \mathcal{N}(\hat{\mathbf{x}}_0, P_0)로 주어지면, 사전 잔차는 다음과 같다.

\mathbf{e}_0 = \mathbf{x}_0 - \hat{\mathbf{x}}_0

3. 가중 최소 제곱 비용 함수

최대 사후 확률(MAP) 추정 원리에 의하면, 배치 추정 문제는 다음의 가중 최소 제곱 비용 함수를 최소화하는 문제로 귀결된다.

J(\mathbf{x}) = \lVert \mathbf{e}_0 \rVert_{P_0^{-1}}^2 + \sum_{t=0}^{T-1} \lVert \mathbf{e}_{p,t} \rVert_{Q_t^{-1}}^2 + \sum_{t=0}^{T} \lVert \mathbf{e}_{m,t} \rVert_{R_t^{-1}}^2

여기서 가중 노름(weighted norm)은 다음과 같이 정의된다.

\lVert \mathbf{a} \rVert_{W}^2 = \mathbf{a}^\top W \mathbf{a}

이를 전개하면 비용 함수는 다음과 같다.

J(\mathbf{x}) = (\mathbf{x}_0 - \hat{\mathbf{x}}_0)^\top P_0^{-1} (\mathbf{x}_0 - \hat{\mathbf{x}}_0) + \sum_{t=0}^{T-1} (\mathbf{x}_{t+1} - F_t \mathbf{x}_t)^\top Q_t^{-1} (\mathbf{x}_{t+1} - F_t \mathbf{x}_t) + \sum_{t=0}^{T} (\mathbf{z}_t - H_t \mathbf{x}_t)^\top R_t^{-1} (\mathbf{z}_t - H_t \mathbf{x}_t)

4. 행렬 형태의 정식화

결합 잔차 벡터를 \mathbf{e}(\mathbf{x}) = \mathbf{b} - A\mathbf{x}로 표현하면, 행렬 A와 벡터 \mathbf{b}, 가중 행렬 W는 다음과 같이 구성된다.

A = \begin{bmatrix} I & & & & \\ -F_0 & I & & & \\ & -F_1 & I & & \\ & & \ddots & \ddots & \\ & & & -F_{T-1} & I \\ H_0 & & & & \\ & H_1 & & & \\ & & \ddots & & \\ & & & & H_T \end{bmatrix}

\mathbf{b} = \begin{bmatrix} \hat{\mathbf{x}}_0 \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \\ \mathbf{z}_0 \\ \mathbf{z}_1 \\ \vdots \\ \mathbf{z}_T \end{bmatrix}

가중 행렬은 블록 대각 행렬로 주어진다.

W = \text{diag}(P_0^{-1}, Q_0^{-1}, Q_1^{-1}, \dots, Q_{T-1}^{-1}, R_0^{-1}, R_1^{-1}, \dots, R_T^{-1})

비용 함수는 다음과 같이 간결하게 표현된다.

J(\mathbf{x}) = (\mathbf{b} - A\mathbf{x})^\top W (\mathbf{b} - A\mathbf{x})

5. 정규 방정식과 최적 해

비용 함수를 \mathbf{x}에 대하여 미분하고 영벡터로 놓으면 정규 방정식(normal equation)이 얻어진다.

\frac{\partial J}{\partial \mathbf{x}} = -2 A^\top W (\mathbf{b} - A\mathbf{x}) = \mathbf{0}

이를 정리하면 다음의 정규 방정식이 된다.

A^\top W A \, \hat{\mathbf{x}} = A^\top W \mathbf{b}

정보 행렬(information matrix)을 \Lambda = A^\top W A로, 정보 벡터(information vector)를 \boldsymbol{\eta} = A^\top W \mathbf{b}로 정의하면, 최적 해는 다음과 같다.

\hat{\mathbf{x}} = \Lambda^{-1} \boldsymbol{\eta} = (A^\top W A)^{-1} A^\top W \mathbf{b}

추정치의 공분산 행렬은 정보 행렬의 역행렬로 주어진다.

P = \Lambda^{-1} = (A^\top W A)^{-1}

6. 정보 행렬의 희소 구조

배치 추정 문제에서 정보 행렬 \Lambda = A^\top W A는 블록 삼대각(block tridiagonal) 구조를 가진다. 이는 마르코프 성질(Markov property)에 의해 시간적으로 인접한 상태 변수만 직접 연결되기 때문이다.

\Lambda = \begin{bmatrix} \Lambda_{00} & \Lambda_{01} & & & \\ \Lambda_{10} & \Lambda_{11} & \Lambda_{12} & & \\ & \ddots & \ddots & \ddots & \\ & & \Lambda_{T-1,T-2} & \Lambda_{T-1,T-1} & \Lambda_{T-1,T} \\ & & & \Lambda_{T,T-1} & \Lambda_{TT} \end{bmatrix}

대각 블록과 비대각 블록은 다음과 같이 계산된다.

\Lambda_{tt} = F_t^\top Q_t^{-1} F_t + Q_{t-1}^{-1} + H_t^\top R_t^{-1} H_t

\Lambda_{t,t+1} = -Q_t^{-1} F_t, \quad \Lambda_{t+1,t} = -F_t^\top Q_t^{-1}

단, t = 0일 때는 \Lambda_{00} = P_0^{-1} + F_0^\top Q_0^{-1} F_0 + H_0^\top R_0^{-1} H_0이다.

7. 촐레스키 분해를 이용한 효율적 풀이

정보 행렬 \Lambda가 대칭 양정치(symmetric positive definite)이므로 촐레스키 분해(Cholesky decomposition)를 적용할 수 있다.

\Lambda = L L^\top

여기서 L은 하삼각 행렬이다. \Lambda의 블록 삼대각 구조 덕분에 L도 블록 하이삼각(block lower bidiagonal) 구조를 가지며, 전진 대입(forward substitution)과 후진 대입(backward substitution)을 통해 O(Tn^3)의 계산 복잡도로 해를 구할 수 있다.

L \mathbf{y} = \boldsymbol{\eta} \quad \text{(전진 대입)}

L^\top \hat{\mathbf{x}} = \mathbf{y} \quad \text{(후진 대입)}

이 과정은 전체 정보 행렬을 직접 역행렬 계산하는 O(T^3 n^3) 복잡도에 비해 매우 효율적이다.

8. QR 분해를 이용한 풀이

가중 최소 제곱 문제를 직접 풀기 위해 QR 분해를 사용할 수도 있다. 가중 잔차 벡터를 다음과 같이 정의한다.

W^{1/2}(\mathbf{b} - A\mathbf{x}) = W^{1/2}\mathbf{b} - W^{1/2}A\mathbf{x}

\tilde{A} = W^{1/2}A, \tilde{\mathbf{b}} = W^{1/2}\mathbf{b}로 놓으면 비용 함수는 다음과 같다.

J(\mathbf{x}) = \lVert \tilde{\mathbf{b}} - \tilde{A}\mathbf{x} \rVert^2

\tilde{A}의 QR 분해를 수행한다.

\tilde{A} = Q \begin{bmatrix} R \\ \mathbf{0} \end{bmatrix}

여기서 Q는 직교 행렬, R은 상삼각 행렬이다. 최적 해는 다음과 같다.

R \hat{\mathbf{x}} = Q_1^\top \tilde{\mathbf{b}}

QR 분해는 정규 방정식의 조건 수(condition number)가 제곱되는 문제를 피할 수 있으므로, 수치적으로 더 안정적이다.

9. 비선형 배치 추정과 가우스-뉴턴 방법

실제 로봇 시스템에서는 동역학 모델과 측정 모델이 비선형인 경우가 대부분이다.

\mathbf{x}_{t+1} = f(\mathbf{x}_t) + \mathbf{w}_t

\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_t

비선형 비용 함수를 최소화하기 위해 가우스-뉴턴(Gauss-Newton) 반복법을 적용한다. 현재 추정치 \mathbf{x}^{(k)} 주위에서 선형화하여 잔차의 야코비 행렬을 구한다.

J_t^f = \left. \frac{\partial f}{\partial \mathbf{x}} \right\vert_{\mathbf{x}_t = \mathbf{x}_t^{(k)}}, \quad J_t^h = \left. \frac{\partial h}{\partial \mathbf{x}} \right\vert_{\mathbf{x}_t = \mathbf{x}_t^{(k)}}

증분 벡터 \delta \mathbf{x} = \mathbf{x} - \mathbf{x}^{(k)}에 대하여 선형화된 정규 방정식을 풀고, 상태를 갱신한다.

(A^{(k)\top} W A^{(k)}) \delta \mathbf{x}^{(k)} = A^{(k)\top} W \mathbf{e}^{(k)}

\mathbf{x}^{(k+1)} = \mathbf{x}^{(k)} + \delta \mathbf{x}^{(k)}

수렴할 때까지 이 과정을 반복한다. 레벤버그-마쿼트(Levenberg-Marquardt) 방법은 가우스-뉴턴에 감쇠 항을 추가하여 수렴 안정성을 높인다.

(A^{(k)\top} W A^{(k)} + \lambda I) \delta \mathbf{x}^{(k)} = A^{(k)\top} W \mathbf{e}^{(k)}

10. 배치 추정과 재귀적 추정의 관계

배치 추정은 선형 가우시안 시스템에서 칼만 스무더(Kalman smoother)와 동일한 결과를 산출한다. 배치 추정의 정보 행렬에 대한 블록 촐레스키 분해의 전진 대입 과정은 칼만 필터의 예측-갱신 단계와 동치이며, 후진 대입 과정은 레이치-퉁-스트리벨(Rauch-Tung-Striebel) 스무더와 동치이다.

11. 그래프 기반 배치 추정

배치 추정 문제는 인수 그래프(factor graph)로 자연스럽게 표현된다. 인수 그래프에서 변수 노드는 각 시간 단계의 상태 \mathbf{x}_t를 나타내고, 인수 노드는 사전 분포, 프로세스 모델, 측정 모델에 해당하는 확률적 제약 조건을 나타낸다.

결합 확률 밀도 함수는 인수들의 곱으로 분해된다.

p(\mathbf{x} \vert \mathbf{z}) \propto p(\mathbf{x}_0) \prod_{t=0}^{T-1} p(\mathbf{x}_{t+1} \vert \mathbf{x}_t) \prod_{t=0}^{T} p(\mathbf{z}_t \vert \mathbf{x}_t)

MAP 추정은 음의 로그 가능도를 최소화하는 것으로, 이는 앞서 도출한 가중 최소 제곱 비용 함수와 동일하다.

\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \left[ -\ln p(\mathbf{x}_0) - \sum_{t=0}^{T-1} \ln p(\mathbf{x}_{t+1} \vert \mathbf{x}_t) - \sum_{t=0}^{T} \ln p(\mathbf{z}_t \vert \mathbf{x}_t) \right]

그래프의 희소 연결 구조가 정보 행렬의 희소 구조에 직접 반영되므로, 변수 소거(variable elimination) 순서를 최적화하여 계산 효율을 극대화할 수 있다.

12. 마르지널화와 슈어 보해

정보 행렬을 특정 상태 변수에 대하여 분할하면 슈어 보해(Schur complement)를 이용한 마르지널화(marginalization)가 가능하다. 상태 벡터를 \mathbf{x} = [\mathbf{x}_a^\top, \mathbf{x}_b^\top]^\top으로 분할하고, 정보 행렬을 다음과 같이 분할한다.

\Lambda = \begin{bmatrix} \Lambda_{aa} & \Lambda_{ab} \\ \Lambda_{ba} & \Lambda_{bb} \end{bmatrix}

\mathbf{x}_b를 마르지널화하면, \mathbf{x}_a에 대한 마르지널 정보 행렬은 다음과 같다.

\Lambda_a^{\text{marg}} = \Lambda_{aa} - \Lambda_{ab} \Lambda_{bb}^{-1} \Lambda_{ba}

이 연산은 슬라이딩 윈도 필터(sliding window filter)에서 오래된 상태를 제거할 때 핵심적으로 사용된다. 마르지널화 후에도 잔여 상태 변수 간의 상관 관계가 정확하게 보존된다.

참고 문헌

• Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
• Dellaert, F., & Kaess, M. (2006). Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. International Journal of Robotics Research, 25(12), 1181-1203.
• Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J., & Dellaert, F. (2012). iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree. International Journal of Robotics Research, 31(2), 216-235.
• Nocedal, J., & Wright, S. J. (2006). Numerical Optimization (2nd ed.). Springer.

v 0.1