6.143 다중 센서 융합에서의 가중 행렬 결합

로봇 시스템은 관성 측정 장치(IMU), LiDAR, 카메라, GPS, 인코더 등 다양한 센서를 탑재하며, 각 센서의 측정치를 통합하여 보다 정확하고 강건한 상태 추정을 수행한다. 다중 센서 융합(multi-sensor fusion)에서 각 센서 측정치의 신뢰도를 반영하는 핵심 수학적 도구가 가중 행렬(weighting matrix)이다. 본 절에서는 가중 행렬을 이용한 다중 센서 측정치의 최적 결합 원리를 선형대수학적 관점에서 다룬다.

1. 센서 측정 모델의 행렬 표현

N개의 센서가 동일한 상태 벡터 \mathbf{x} \in \mathbb{R}^n를 관측한다고 하자. k번째 센서의 측정 모델은 다음과 같다.

\mathbf{z}_k = H_k \mathbf{x} + \mathbf{v}_k, \quad k = 1, 2, \dots, N

여기서 \mathbf{z}_k \in \mathbb{R}^{m_k}는 측정 벡터, H_k \in \mathbb{R}^{m_k \times n}는 관측 행렬, \mathbf{v}_k는 평균이 영이고 공분산이 R_k \in \mathbb{R}^{m_k \times m_k}인 측정 잡음 벡터이다.

\mathbb{E}[\mathbf{v}_k] = \mathbf{0}, \quad \mathbb{E}[\mathbf{v}_k \mathbf{v}_k^\top] = R_k

서로 다른 센서의 잡음은 상호 독립이라고 가정한다.

\mathbb{E}[\mathbf{v}_j \mathbf{v}_k^\top] = \mathbf{0}, \quad j \neq k

2. 결합 측정 모델

N개의 센서 측정치를 하나의 결합 벡터로 적층(stacking)하면 다음과 같다.

\mathbf{z} = H \mathbf{x} + \mathbf{v}

여기서

\mathbf{z} = \begin{bmatrix} \mathbf{z}_1 \\ \mathbf{z}_2 \\ \vdots \\ \mathbf{z}_N \end{bmatrix}, \quad H = \begin{bmatrix} H_1 \\ H_2 \\ \vdots \\ H_N \end{bmatrix}, \quad \mathbf{v} = \begin{bmatrix} \mathbf{v}_1 \\ \mathbf{v}_2 \\ \vdots \\ \mathbf{v}_N \end{bmatrix}

결합 잡음 벡터의 공분산 행렬은 블록 대각 구조를 가진다.

R = \mathbb{E}[\mathbf{v}\mathbf{v}^\top] = \text{diag}(R_1, R_2, \dots, R_N) = \begin{bmatrix} R_1 & & \\ & R_2 & \\ & & \ddots & \\ & & & R_N \end{bmatrix}

3. 가중 최소 제곱 추정

결합 측정 모델에 대한 최적 추정치는 가중 최소 제곱(Weighted Least Squares, WLS) 기준으로 다음과 같이 구한다.

\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} (\mathbf{z} - H\mathbf{x})^\top R^{-1} (\mathbf{z} - H\mathbf{x})

여기서 W = R^{-1}은 가중 행렬이다. 정규 방정식을 풀면 다음을 얻는다.

\hat{\mathbf{x}} = (H^\top R^{-1} H)^{-1} H^\top R^{-1} \mathbf{z}

R의 블록 대각 구조를 이용하면 이를 각 센서별 기여의 합으로 분해할 수 있다.

\hat{\mathbf{x}} = \left(\sum_{k=1}^{N} H_k^\top R_k^{-1} H_k\right)^{-1} \left(\sum_{k=1}^{N} H_k^\top R_k^{-1} \mathbf{z}_k\right)

이 표현에서 R_k^{-1}은 k번째 센서 측정치에 부여되는 가중치 역할을 한다. 잡음 공분산 R_k가 작은(정밀도가 높은) 센서일수록 큰 가중치가 부여된다.

4. 추정치의 공분산

WLS 추정치의 공분산 행렬은 다음과 같다.

P = \text{Cov}(\hat{\mathbf{x}}) = (H^\top R^{-1} H)^{-1} = \left(\sum_{k=1}^{N} H_k^\top R_k^{-1} H_k\right)^{-1}

이 공분산 행렬은 융합된 추정치의 불확실성을 나타내며, 개별 센서만을 사용한 경우의 공분산보다 항상 작거나 같다. 이는 다중 센서 융합의 근본적 이점이다.

5. 정보 행렬 형식

정보 행렬(information matrix) \Omega = P^{-1}을 이용하면 센서 융합이 더욱 간명해진다.

\Omega = \sum_{k=1}^{N} H_k^\top R_k^{-1} H_k = \sum_{k=1}^{N} \Omega_k

여기서 \Omega_k = H_k^\top R_k^{-1} H_k는 k번째 센서의 정보 기여(information contribution)이다. 정보 벡터(information vector)를 \boldsymbol{\eta} = \Omega \hat{\mathbf{x}}로 정의하면 다음이 성립한다.

\boldsymbol{\eta} = \sum_{k=1}^{N} H_k^\top R_k^{-1} \mathbf{z}_k = \sum_{k=1}^{N} \boldsymbol{\eta}_k

정보 형식에서 센서 융합은 단순한 가법적(additive) 연산이 된다. 이는 분산 시스템에서 각 센서 노드의 정보 기여를 독립적으로 계산한 후 합산할 수 있어, 분산 센서 융합 아키텍처에 적합하다.

6. 동일 상태의 직접 측정 결합

모든 센서가 동일한 물리량을 직접 측정하는 경우(H_k = I, m_k = n), 융합 공식이 크게 단순화된다.

P^{-1} = \sum_{k=1}^{N} R_k^{-1}

\hat{\mathbf{x}} = P \sum_{k=1}^{N} R_k^{-1} \mathbf{z}_k

스칼라의 경우(n = 1), N개 센서의 측정치 z_k와 분산 \sigma_k^2에 대해 융합 결과는 다음과 같다.

\hat{x} = \frac{\sum_{k=1}^{N} \sigma_k^{-2} z_k}{\sum_{k=1}^{N} \sigma_k^{-2}}, \quad \sigma_{\text{fused}}^2 = \frac{1}{\sum_{k=1}^{N} \sigma_k^{-2}}

이는 정밀도(분산의 역수)의 가중 평균에 해당하며, 융합된 분산은 항상 개별 센서의 최소 분산보다 작다.

7. 상관된 센서 잡음의 처리

센서 잡음이 상호 상관(cross-correlated)된 경우, 결합 공분산 행렬은 비대각 성분을 포함한다.

R = \begin{bmatrix} R_{11} & R_{12} & \cdots \\ R_{21} & R_{22} & \cdots \\ \vdots & & \ddots \end{bmatrix}

이 경우에도 WLS 공식은 동일하게 적용되나, R^{-1}의 계산이 복잡해진다. 상관 구조를 무시하고 대각 블록만을 사용하면(나이브 융합) 추정치의 최적성이 보장되지 않으며, 추정 공분산이 과소 추정될 위험이 있다.

8. 순차적 센서 갱신

센서 측정치가 순차적으로 도착하는 경우, 배치(batch) 처리 대신 순차적 갱신(sequential update)이 효율적이다. k번째 센서의 측정치로 갱신하는 과정은 다음과 같다.

K_k = P_{k-1} H_k^\top (H_k P_{k-1} H_k^\top + R_k)^{-1}

\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_{k-1} + K_k (\mathbf{z}_k - H_k \hat{\mathbf{x}}_{k-1})

P_k = (I - K_k H_k) P_{k-1}

여기서 K_k는 칼만 이득(Kalman gain)이다. 순차적 갱신은 배치 WLS와 수학적으로 동등한 결과를 산출하며, 칼만 필터의 갱신 단계와 동일한 구조를 가진다.

9. 로봇 공학에서의 응용

10. 참고 문헌

• Bar-Shalom, Y., Li, X. R., & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley.
• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
• Simon, D. (2006). Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches. Wiley.
• Maybeck, P. S. (1979). Stochastic Models, Estimation, and Control, Volume 1. Academic Press.
• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.

v 0.1