6.138 칼만 필터의 선형대수학적 구조

1. 칼만 필터의 개요

칼만 필터(Kalman filter)는 잡음이 포함된 관측으로부터 선형 동적 시스템의 상태를 최적으로 추정하는 재귀적 알고리즘이다. 1960년 루돌프 칼만(Rudolf E. Kalman)이 제안한 이 필터는 로봇공학에서 위치 추정, 센서 융합, SLAM 등에 광범위하게 사용된다. 칼만 필터의 모든 연산은 행렬의 곱셈, 덧셈, 역행렬 계산으로 구성되며, 그 구조는 선형대수학적으로 명확하게 해석할 수 있다.

2. 시스템 모델

칼만 필터가 다루는 선형 가우시안 시스템 모델은 다음과 같다.

상태 전이 모델(process model):

\mathbf{x}_{k+1} = \mathbf{F}_k \mathbf{x}_k + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k

관측 모델(measurement model):

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

여기서 각 기호의 의미는 다음과 같다.

잡음 \mathbf{w}_k와 \mathbf{v}_k는 서로 독립이고 백색(white) 가우시안 과정이다. \mathbf{Q}_k \succeq 0은 과정 잡음 공분산 행렬이고, \mathbf{R}_k \succ 0은 관측 잡음 공분산 행렬이다.

3. 칼만 필터 알고리즘의 행렬 구조

칼만 필터는 예측(prediction) 단계와 갱신(update) 단계로 구성된다.

3.1 예측 단계

\hat{\mathbf{x}}_{k+1\vert k} = \mathbf{F}_k \hat{\mathbf{x}}_{k\vert k} + \mathbf{B}_k \mathbf{u}_k

\mathbf{P}_{k+1\vert k} = \mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T + \mathbf{Q}_k

상태 추정값 \hat{\mathbf{x}}_{k+1\vert k}은 상태 전이 행렬 \mathbf{F}_k에 의한 선형 변환이다. 공분산 행렬 \mathbf{P}_{k+1\vert k}의 갱신에서 \mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T는 합동 변환(congruence transformation)으로, \mathbf{P}_{k\vert k}가 양의 반정치(positive semidefinite)이면 결과도 양의 반정치이다. \mathbf{Q}_k를 더하므로 불확실성이 증가한다.

3.2 갱신 단계

혁신(innovation):

\tilde{\mathbf{z}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k\vert k-1}

혁신 공분산(innovation covariance):

\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k\vert k-1} \mathbf{H}_k^T + \mathbf{R}_k

칼만 이득(Kalman gain):

\mathbf{K}_k = \mathbf{P}_{k\vert k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}

상태 갱신:

\hat{\mathbf{x}}_{k\vert k} = \hat{\mathbf{x}}_{k\vert k-1} + \mathbf{K}_k \tilde{\mathbf{z}}_k

공분산 갱신:

\mathbf{P}_{k\vert k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\vert k-1}

4. 칼만 이득의 선형대수학적 해석

4.1 최적 가중 평균으로서의 해석

칼만 필터의 갱신은 사전 추정값(prior estimate) \hat{\mathbf{x}}_{k\vert k-1}과 관측 기반 추정값 \mathbf{H}_k^{\dagger}\mathbf{z}_k의 최적 가중 평균으로 해석할 수 있다. 칼만 이득 \mathbf{K}_k는 두 정보원의 불확실성을 고려하여 최적의 가중치를 결정한다.

\mathbf{P}_{k\vert k-1}이 작으면(사전 추정의 불확실성이 작으면) \mathbf{K}_k가 작아져 관측 정보를 적게 반영하고, \mathbf{R}_k가 작으면(관측이 정확하면) \mathbf{K}_k가 커져 관측 정보를 많이 반영한다.

4.2 사영 행렬로서의 해석

\mathbf{K}_k \mathbf{H}_k는 칼만 갱신에서 보정되는 방향을 결정하는 행렬이다. 공분산 갱신 공식에서

\mathbf{P}_{k\vert k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\vert k-1}

\mathbf{I} - \mathbf{K}_k \mathbf{H}_k는 관측에 의하여 정보가 제공되지 않는 부분 공간에 대한 사영(projection)의 역할을 한다. 관측이 완전하여 \mathbf{H}_k가 정방 가역 행렬이면 \mathbf{K}_k \mathbf{H}_k \to \mathbf{I}이고, \mathbf{P}_{k\vert k} \to \mathbf{0}이 된다.

5. 수치 안정성과 대칭 양의 정치성 보존

5.1 요셉 형태(Joseph Form)

표준 공분산 갱신 공식 \mathbf{P}_{k\vert k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)\mathbf{P}_{k\vert k-1}은 수치 오차로 인하여 대칭성이나 양의 반정치성을 잃을 수 있다. 요셉 형태(Joseph form)는 이를 개선한다.

\mathbf{P}_{k\vert k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\vert k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T

이 형태는 \mathbf{K}_k의 값과 무관하게(최적이 아닌 이득을 사용하더라도) \mathbf{P}_{k\vert k}의 대칭 양의 반정치성을 보장한다. 이는 \mathbf{M}\mathbf{P}\mathbf{M}^T + \mathbf{N}\mathbf{R}\mathbf{N}^T 형태가 항상 양의 반정치임을 이용한 것이다.

5.2 제곱근 필터(Square-Root Filter)

공분산 행렬 \mathbf{P}를 직접 다루는 대신, 그 촐레스키 인수(Cholesky factor) \mathbf{L}을 전파하는 방법이다. \mathbf{P} = \mathbf{L}\mathbf{L}^T로 분해하면, 양의 정치성이 구조적으로 보장되고 조건수가 절반으로 줄어든다.

예측 단계에서 제곱근 인수의 갱신은 다음과 같이 수행한다. \mathbf{F}_k \mathbf{L}_{k\vert k}와 \mathbf{L}_{\mathbf{Q}_k} (\mathbf{Q}_k = \mathbf{L}_{\mathbf{Q}_k}\mathbf{L}_{\mathbf{Q}_k}^T)를 열 방향으로 결합한 행렬

\begin{bmatrix} \mathbf{F}_k \mathbf{L}_{k\vert k} & \mathbf{L}_{\mathbf{Q}_k} \end{bmatrix}

에 QR 분해를 적용하면 \mathbf{L}_{k+1\vert k}를 얻는다.

5.3 UD 분해 필터

공분산 행렬을 \mathbf{P} = \mathbf{U}\mathbf{D}\mathbf{U}^T로 분해하여 전파하는 방법도 있다. 여기서 \mathbf{U}는 단위 상삼각 행렬이고, \mathbf{D}는 대각 행렬이다. 제곱근을 계산할 필요가 없어 계산 효율이 높다.

6. 칼만 필터와 최소 제곱법의 관계

6.1 배치 최소 제곱(Batch Least Squares)과의 동치

시각 0부터 k까지의 모든 관측 \mathbf{z}_0, \mathbf{z}_1, \dots, \mathbf{z}_k를 사용한 가중 최소 제곱(weighted least squares) 추정은 다음과 같다.

\hat{\mathbf{x}}_k = \arg\min_{\mathbf{x}} \left[\lVert \mathbf{x}_0 - \hat{\mathbf{x}}_0 \rVert_{\mathbf{P}_0^{-1}}^2 + \sum_{j=0}^{k} \lVert \mathbf{z}_j - \mathbf{H}_j \mathbf{x}_j \rVert_{\mathbf{R}_j^{-1}}^2 + \sum_{j=0}^{k-1} \lVert \mathbf{x}_{j+1} - \mathbf{F}_j \mathbf{x}_j \rVert_{\mathbf{Q}_j^{-1}}^2 \right]

칼만 필터는 이 배치 문제를 재귀적으로 푸는 것과 정확히 동일한 결과를 산출한다. 이는 셔먼-모리슨-우드베리(Sherman-Morrison-Woodbury) 항등식을 통하여 증명할 수 있다.

6.2 정보 행렬과의 관계

공분산 행렬의 역행렬 \boldsymbol{\Omega}_k = \mathbf{P}_k^{-1}을 정보 행렬(information matrix)이라 한다. 정보 형태(information form)에서 갱신은 다음과 같이 덧셈으로 간결하게 표현된다.

\boldsymbol{\Omega}_{k\vert k} = \boldsymbol{\Omega}_{k\vert k-1} + \mathbf{H}_k^T \mathbf{R}_k^{-1} \mathbf{H}_k

이는 새로운 관측이 기존 정보에 덧셈적으로 기여함을 보여주며, 다중 센서 융합에서 특히 직관적이다.

7. 관측 가능성과 칼만 필터 수렴

칼만 필터의 오차 공분산 \mathbf{P}_{k\vert k}가 유계(bounded)이고 수렴하기 위한 조건은 시스템의 관측 가능성(observability)과 관련된다. 시간 불변 시스템 (\mathbf{F}, \mathbf{H})에 대하여 관측 가능성 행렬

\mathcal{O} = \begin{bmatrix} \mathbf{H} \\ \mathbf{H}\mathbf{F} \\ \vdots \\ \mathbf{H}\mathbf{F}^{n-1} \end{bmatrix}

이 전열 계수(full column rank), 즉 \text{rank}(\mathcal{O}) = n이면, 시스템이 완전 관측 가능(completely observable)하다고 한다. 이 조건 하에서 \mathbf{P}_{k\vert k}는 유일한 정상 상태 해로 수렴하며, 이 해는 이산 시간 대수 리카티 방정식(DARE)을 만족한다.

\mathbf{P} = \mathbf{F}\mathbf{P}\mathbf{F}^T + \mathbf{Q} - \mathbf{F}\mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R})^{-1}\mathbf{H}\mathbf{P}\mathbf{F}^T

8. 참고 문헌

• Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 82(1), 35–45.
• Simon, D. (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Wiley.
• Bar-Shalom, Y., Li, X. R., & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley.
• Strang, G. (2019). Linear Algebra and Learning from Data. Wellesley-Cambridge Press.
• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.

v 0.1