6.139 예측 단계와 갱신 단계의 행렬 연산

1. 개요

칼만 필터의 핵심은 예측(prediction)과 갱신(update)이라는 두 단계의 반복이다. 각 단계는 구체적인 행렬 연산으로 구성되며, 이들 연산의 수학적 의미와 수치적 특성을 정확히 이해하는 것이 효율적이고 안정적인 구현의 핵심이다. 본 절에서는 예측 단계와 갱신 단계의 행렬 연산을 상세히 분석한다.

2. 예측 단계의 행렬 연산

2.1 상태 예측

시각 $k$ 에서의 사후 추정값 $\hat{\mathbf{x}}_{k\vert k}$ 로부터 시각 $k+1$ 에서의 사전 추정값을 계산한다.

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

이 연산은 행렬-벡터 곱 $\mathbf{F}_k \hat{\mathbf{x}}_{k\vert k} \in \mathbb{R}^n$ 으로, 계산 복잡도는 $O(n^2)$ 이다. $\mathbf{F}_k$ 가 희소(sparse)하면 복잡도는 비영 원소의 수에 비례하여 감소한다.

로봇공학에서 자주 등장하는 등속도 운동 모델(constant velocity model)의 경우, 상태 벡터 $\mathbf{x} = [p, v]^T$ (위치, 속도)에 대하여

$\mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}$

이 행렬은 상삼각 행렬이며, 행렬-벡터 곱은 $O(n)$ 의 복잡도를 가진다.

2.2 공분산 예측

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

이 연산은 칼만 필터에서 가장 계산 비용이 높은 부분 중 하나이다. 단계별 분석은 다음과 같다.

단계 1: 임시 행렬 $\mathbf{M} = \mathbf{F}_k \mathbf{P}_{k\vert k}$ 계산

$\mathbf{F}_k \in \mathbb{R}^{n \times n}$ , $\mathbf{P}_{k\vert k} \in \mathbb{R}^{n \times n}$
복잡도: $O(n^3)$

단계 2: $\mathbf{M}\mathbf{F}_k^T$ 계산

복잡도: $O(n^3)$

단계 3: $\mathbf{Q}_k$ 덧셈

복잡도: $O(n^2)$

총 복잡도는 $O(n^3)$ 이며, $\mathbf{P}_{k\vert k}$ 가 대칭이므로 대칭 행렬의 성질을 이용하면 연산량을 약 절반으로 줄일 수 있다. $\mathbf{P}_{k\vert k}$ 의 대칭성은 합동 변환 $\mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T$ 에 의하여 보존되고, 대칭 행렬인 $\mathbf{Q}_k$ 를 더하므로 $\mathbf{P}_{k+1\vert k}$ 도 대칭이다.

2.3 양의 반정치성 보존

$\mathbf{P}_{k\vert k} \succeq 0$ 이면 임의의 벡터 $\mathbf{v}$ 에 대하여

$\mathbf{v}^T \mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T \mathbf{v} = (\mathbf{F}_k^T \mathbf{v})^T \mathbf{P}_{k\vert k} (\mathbf{F}_k^T \mathbf{v}) \geq 0$

이므로 $\mathbf{F}_k \mathbf{P}_{k\vert k} \mathbf{F}_k^T \succeq 0$ 이다. $\mathbf{Q}_k \succeq 0$ 이므로

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

이다. 이론적으로 양의 반정치성이 보존되지만, 유한 정밀도 부동소수점 연산에서는 반올림 오차가 누적되어 대칭성이나 양의 반정치성이 훼손될 수 있다.

3. 갱신 단계의 행렬 연산

3.1 혁신 계산

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

혁신(innovation) $\tilde{\mathbf{z}}_k \in \mathbb{R}^m$ 은 실제 관측과 예측 관측의 차이이다. 이 연산은 행렬-벡터 곱 $\mathbf{H}_k \hat{\mathbf{x}}_{k\vert k-1}$ 으로 $O(mn)$ 의 복잡도를 가진다.

3.2 혁신 공분산 계산

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

단계별 분석은 다음과 같다.

단계 1: $\mathbf{L} = \mathbf{H}_k \mathbf{P}_{k\vert k-1}$ 계산, 복잡도 $O(mn^2)$

단계 2: $\mathbf{L}\mathbf{H}_k^T$ 계산, 복잡도 $O(m^2 n)$

단계 3: $\mathbf{R}_k$ 덧셈, 복잡도 $O(m^2)$

$\mathbf{S}_k \in \mathbb{R}^{m \times m}$ 은 대칭 양의 정치(positive definite) 행렬이다. $\mathbf{R}_k \succ 0$ 이므로 $\mathbf{S}_k \succ 0$ 이 보장된다.

3.3 칼만 이득 계산

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

$\mathbf{S}_k^{-1}$ 을 명시적으로 계산하는 대신, 선형 시스템 $\mathbf{S}_k \mathbf{X} = \mathbf{H}_k \mathbf{P}_{k\vert k-1}^T$ 를 풀어 $\mathbf{K}_k = \mathbf{X}^T$ 를 구하는 것이 수치적으로 더 안정적이다. 구체적으로는 다음과 같이 구현한다.

촐레스키 분해: $\mathbf{S}_k = \mathbf{L}_S \mathbf{L}_S^T$ (복잡도 $O(m^3/3)$ )

전방 대입: $\mathbf{L}_S \mathbf{Y} = (\mathbf{P}_{k\vert k-1} \mathbf{H}_k^T)^T$ (복잡도 $O(m^2 n)$ )

후방 대입: $\mathbf{L}_S^T \mathbf{X} = \mathbf{Y}$ (복잡도 $O(m^2 n)$ )

전체 복잡도는 $O(m^3 + m^2 n)$ 이다.

3.4 상태 갱신

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

행렬-벡터 곱 $\mathbf{K}_k \tilde{\mathbf{z}}_k$ 의 복잡도는 $O(nm)$ 이다.

3.5 공분산 갱신

표준 형태:

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

이 연산에서 $\mathbf{K}_k \mathbf{H}_k \in \mathbb{R}^{n \times n}$ 의 계산은 $O(n^2 m)$ 이고, $(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)\mathbf{P}_{k\vert k-1}$ 의 계산은 $O(n^3)$ 이다.

수치 안정성을 위한 요셉 형태:

$\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$

계산 비용은 약 2배 증가하지만, 대칭 양의 반정치성이 구조적으로 보장된다.

4. 연산 복잡도 요약

각 단계의 행렬 연산별 복잡도를 정리하면 다음과 같다.

연산	행렬 크기	복잡도	비고
상태 예측 $\mathbf{F}\hat{\mathbf{x}}$	$n \times 1$	$O(n^2)$	행렬-벡터 곱
공분산 예측 $\mathbf{F}\mathbf{P}\mathbf{F}^T + \mathbf{Q}$	$n \times n$	$O(n^3)$	합동 변환
혁신 $\mathbf{z} - \mathbf{H}\hat{\mathbf{x}}$	$m \times 1$	$O(mn)$	행렬-벡터 곱
혁신 공분산 $\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R}$	$m \times m$	$O(m n^2)$	합동 변환
촐레스키 분해 $\mathbf{S} = \mathbf{L}_S \mathbf{L}_S^T$	$m \times m$	$O(m^3/3)$	삼각 분해
칼만 이득 $\mathbf{K}$	$n \times m$	$O(m^2 n)$	삼각 대입
상태 갱신 $\hat{\mathbf{x}} + \mathbf{K}\tilde{\mathbf{z}}$	$n \times 1$	$O(nm)$	행렬-벡터 곱
공분산 갱신 (표준)	$n \times n$	$O(n^2 m + n^3)$	행렬 곱

전체 반복당 복잡도에서 지배적인 항은 $O(n^3)$ (공분산 예측)과 $O(mn^2)$ (혁신 공분산)이다. 관측 차원 $m$ 이 상태 차원 $n$ 보다 작은 경우( $m \ll n$ ), 공분산 예측이 병목이 된다.

5. 효율적 구현 기법

5.1 대칭 행렬의 활용

$\mathbf{P}$ 가 대칭이므로, 상삼각(또는 하삼각) 부분만 저장하고 갱신하면 메모리와 계산량을 줄일 수 있다. 대칭 행렬-행렬 곱(SYMM) 루틴을 사용하면 일반 행렬 곱(GEMM)에 비하여 연산량이 약 절반이다.

5.2 희소 행렬 활용

대규모 SLAM 문제에서 상태 벡터의 차원 $n$ 이 수백에서 수천에 이를 수 있다. 이 경우 $\mathbf{F}_k$ 와 $\mathbf{H}_k$ 가 매우 희소한 경우가 많다. 희소 행렬 연산을 이용하면 공분산 예측과 갱신의 복잡도를 크게 줄일 수 있다.

예를 들어, 랜드마크 $j$ 에 대한 관측만 존재하는 경우, $\mathbf{H}_k$ 의 비영 블록은 로봇 상태와 랜드마크 $j$ 에 해당하는 열에만 존재한다. 이 경우 혁신 공분산 계산에서 $\mathbf{P}$ 의 해당 부분 행렬만 추출하여 사용하면 된다.

$\mathbf{S}_k = \mathbf{H}_{k,r} \mathbf{P}_{rr} \mathbf{H}_{k,r}^T + \mathbf{H}_{k,j} \mathbf{P}_{jj} \mathbf{H}_{k,j}^T + \mathbf{H}_{k,r} \mathbf{P}_{rj} \mathbf{H}_{k,j}^T + \mathbf{H}_{k,j} \mathbf{P}_{jr} \mathbf{H}_{k,r}^T + \mathbf{R}_k$

여기서 하첨자 $r$ 은 로봇 상태, $j$ 는 랜드마크를 나타낸다.

5.3 순차 갱신(Sequential Update)

다수의 관측을 동시에 처리하는 대신, 각 관측을 하나씩 순차적으로 갱신할 수 있다. 스칼라 관측( $m = 1$ )에 대한 갱신에서는 $\mathbf{S}_k$ 가 스칼라가 되므로 역행렬 계산이 단순한 나눗셈이 된다.

$\mathbf{K}_k = \frac{\mathbf{P}_{k\vert k-1} \mathbf{h}_k}{s_k}, \quad s_k = \mathbf{h}_k^T \mathbf{P}_{k\vert k-1} \mathbf{h}_k + r_k$

여기서 $\mathbf{h}_k$ 는 $\mathbf{H}_k$ 의 행 벡터이고, $r_k$ 는 해당 관측의 잡음 분산이다. 이 방법의 복잡도는 관측당 $O(n^2)$ 이므로, $m$ 개의 관측에 대하여 총 $O(mn^2)$ 이 된다. 이는 $m < n$ 일 때 일괄 갱신( $O(n^3)$ )보다 효율적이다.

5.4 제곱근 필터의 예측 및 갱신

$\mathbf{P} = \mathbf{L}\mathbf{L}^T$ 로 분해하여 전파하는 제곱근 필터에서, 갱신 단계는 다음과 같이 수행한다.

결합 행렬 구성:

$\begin{bmatrix} \mathbf{L}_{\mathbf{R}_k} & \mathbf{H}_k \mathbf{L}_{k\vert k-1} \\ \mathbf{0} & \mathbf{L}_{k\vert k-1} \end{bmatrix}$

직교 삼각화(QR 분해 또는 기븐스 회전) 적용:

$\mathbf{T} \begin{bmatrix} \mathbf{L}_{\mathbf{R}_k} & \mathbf{H}_k \mathbf{L}_{k\vert k-1} \\ \mathbf{0} & \mathbf{L}_{k\vert k-1} \end{bmatrix} = \begin{bmatrix} \mathbf{L}_{\mathbf{S}_k} & \bar{\mathbf{K}}_k \\ \mathbf{0} & \mathbf{L}_{k\vert k} \end{bmatrix}$

여기서 $\mathbf{T}$ 는 직교 행렬, $\mathbf{L}_{\mathbf{S}_k}$ 는 $\mathbf{S}_k$ 의 촐레스키 인수, $\mathbf{L}_{k\vert k}$ 는 갱신된 공분산의 촐레스키 인수이다.

이 방법의 장점은 직교 변환이 수치적으로 안정적이고, 양의 정치성이 구조적으로 보장된다는 것이다.

6. 수치 예제: 2차원 위치 추정

상태 벡터 $\mathbf{x} = [p_x, p_y, v_x, v_y]^T \in \mathbb{R}^4$ 인 2차원 등속도 모델에서의 행렬 연산을 구체적으로 살펴본다.

$\mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}$

$\mathbf{F}$ 는 상삼각 행렬이므로 $\mathbf{F}\hat{\mathbf{x}}$ 의 계산에서 곱셈 횟수가 6회( $4^2 = 16$ 회 대비)로 감소한다. $\mathbf{H}$ 는 $\mathbf{P}$ 의 좌상단 $2 \times 2$ 블록만 추출하는 선택 행렬이므로, $\mathbf{H}\mathbf{P}\mathbf{H}^T$ 는 $\mathbf{P}$ 의 $(1:2, 1:2)$ 부분 행렬에 해당한다.

$\mathbf{S} = \begin{bmatrix} P_{11} & P_{12} \\ P_{21} & P_{22} \end{bmatrix} + \mathbf{R}$

이 경우 $2 \times 2$ 행렬의 역행렬은 해석적으로 계산할 수 있다.

$\mathbf{S}^{-1} = \frac{1}{S_{11}S_{22} - S_{12}S_{21}} \begin{bmatrix} S_{22} & -S_{12} \\ -S_{21} & S_{11} \end{bmatrix}$

7. 참고 문헌

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.
Bierman, G. J. (1977). Factorization Methods for Discrete Sequential Estimation. Academic Press.
Golub, G. H., & Van Loan, C. F. (2013). Matrix Computations (4th ed.). Johns Hopkins University Press.
Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.

v 0.1