베이즈 필터로부터 칼만 필터 유도하기
칼만 필터(Kalman Filter)는 베이즈 필터(Bayesian Filter)의 특수한 구현이다. 칼만 필터는 가우시안 노이즈를 갖는 선형 시스템에서 최적성과 계산 효율성 덕분에 다양한 공학 분야에서 널리 사용된다. 아래서는 칼만 필터의 단계별 업데이트 식을 수학적으로 유도해본다.
위 인터랙티브 데모는 칼만 필터가 마우스의 움직임을 어떻게 추정하는지 실시간으로 시각화한다. 커서를 움직이거나 화면을 터치하면, 세 가지 핵심 요소 간의 상호작용을 관찰할 수 있다. 파란 선은 포인터의 실제 경로(ground truth)를 나타내고, 초록 점은 노이즈가 섞인 불확실한 측정값을 보여준다. 마젠타 원은 칼만 필터의 출력을 나타내며, 추정 위치(중심)와 현재 불확실성의 정도(원의 크기)를 모두 표현한다. 우측 상단 제어창의 값을 조정하면, 이 변수들의 효과를 실시간으로 확인할 수 있다. 측정 분산(Q)을 키우면 추정된 궤적이 불규칙한 측정값을 무시하고 예측값에 더 의존하기 시작한다. 반대로 프로세스 분산(R)을 조정하면 추정값이 실제 경로를 얼마나 빠르게 따라가는지에 영향을 준다.
가정
전통적인 칼만 필터를 유도하기 위해, 세 가지 핵심 가정에 의존한다.
-
선형성: 상태 전이(운동) 모델과 측정 모델 모두 선형이다. 따라서 운동 모델은
\[x_t = A_t x_{t-1} + B_t u_t + w_t\]로 주어지고, 측정 모델은
\[z_t = C_t x_t + v_t\]로 주어진다.
- 가우시안 초기 상태: 초기 belief \(bel(x_0)\)는 가우시안 분포를 따른다고 가정한다.
-
가우시안 노이즈: 프로세스 노이즈
\[w_t \sim \mathcal{N}(0, R_t)\]와 측정 노이즈
\[v_t \sim \mathcal{N}(0, Q_t)\]는 각각 상태 모델과 측정 모델에 더해지는 평균이 0인 가우시안 분포다.
유도 과정
베이즈 필터의 두 주요 단계는 아래와 같다.
- 예측(Prediction)
- 보정(Correction)
예측
이 단계에서는 운동 모델을 이용해 이전 belief \(bel(x_{t-1})\)를 시간상 한 스텝 앞으로 업데이트한다. 초기 상태와 노이즈가 가우시안이고 운동/측정 모델이 선형이므로, 임의의 시각 \(t\)에서의 belief \(bel(x_t)\)는 가우시안으로 유지된다. \(bel(x_{t-1})\)의 평균과 공분산을 각각 \(\mu_{t-1}\), \(\Sigma_{t-1}\)이라 하자. 즉,
\[bel(x_{t-1}) \sim \mathcal{N}(\mu_{t-1}, \Sigma_{t-1})\]\(bel(x_{t-1})\)를 다음과 같이 표현할 수도 있다.
\[\frac{1}{(2\pi)^{n/2} |\Sigma_{t-1}|^{1/2}} \exp\left({-\frac{1}{2}(x_{t-1} - \mu_{t-1})^T \Sigma_{t-1}^{-1} (x_{t-1} - \mu_{t-1})}\right)\]여기서 \(n\)은 상태 벡터 \(x_{t-1}\)의 차원이다. 다음부터 앞에 곱해진 값을 \(\eta\)로 표현한다. 즉,
\[bel(x_{t-1}) = \eta \exp\left({-\frac{1}{2}(x_{t-1} - \mu_{t-1})^T \Sigma_{t-1}^{-1} (x_{t-1} - \mu_{t-1})}\right).\]운동 모델을 베이즈 예측 방정식에 대입하면 다음을 얻는다.
\[\begin{align} \overline{bel}(x_t) &= \int p(x_t | x_{t-1}, u_t) bel(x_{t-1}) dx_{t-1} \\ &= \int \mathcal{N}(A_t x_{t-1} + B_t u_t, R_t) \mathcal{N}(\mu_{t-1}, \Sigma_{t-1}) dx_{t-1} \\ &= \eta \int \exp \left( {-\frac{1}{2}(x_t - A_t x_{t-1} - B_t u_t)^T R_t^{-1} (x_t - A_t x_{t-1} - B_t u_t)} {-\frac{1}{2}(x_{t-1} - \mu_{t-1})^T \Sigma_{t-1}^{-1} (x_{t-1} - \mu_{t-1})} \right) dx_{t-1}. \end{align}\]식을 간단히 하기 위해, 지수를 \(L_t = L_t(x_{t-1}, x_t) + L_t(x_t)\)로 정의하여 다음과 같이 둔다.
\[\overline{bel}(x_t) = \eta \int \exp \left({-L_t} \right) dx_{t-1}\]우리의 목표는 피적분 함수에서 변수 \(x_{t-1}\)을 적분으로 소거하는 것이다. 가우시안을 전체 정의역에 대해 적분하면 상수가 되므로, \(x_{t-1}\)이 주변화(marginalize)된 후 남는 \(x_t\) 항에만 집중하면 된다.
\[\begin{align} \overline{bel}(x_t) &= \eta \int \exp \left({-L_t} \right) dx_{t-1} \\ &= \eta \int \exp \left({-L_t(x_{t-1}, x_t)} \right) \exp \left({-L_t(x_t)} \right) dx_{t-1} \\ &= \eta \exp \left({-L_t(x_t)} \right) \int \exp \left({-L_t(x_{t-1}, x_t)} \right) dx_{t-1} \\ &= \eta \prime \exp \left({-L_t(x_t)} \right) \end{align}\]\(x_{t-1}\)에 대한 이차 방정식의 계수를 찾기 위해, 간단한 관계를 사용한다. 이차 방정식 \(f(x; m, S) = \frac{1}{2}(x-m)^T S (x-m)\)에 대해 다음이 만족된다.
\[\begin{align} f'(m) &= (m-m)^T S = 0, \\ f''(x) &= S \end{align}\]따라서 \(x_{t-1}\)에 대한 \(L_t\)의 1차 도함수는
\[\frac{\partial L_t}{\partial x_{t-1}} = -(x_t - A_t x_{t-1} - B_t u_t)^T R_t^{-1} A_t + (x_{t-1} - \mu_{t-1})^T \Sigma_{t-1}^{-1}\]이고, \(x_{t-1}=m\)에서 아래가 만족된다.
\[-(x_t - A_t m - B_t u_t)^T R_t^{-1} A_t + (m - \mu_{t-1})^T \Sigma_{t-1}^{-1} =0\]따라서
\[m = (A_t^T R_t^{-1} A_t + \Sigma_{t-1}^{-1})^{-1} (A_t^T R_t^{-1} x_t - A_t^T R_t^{-1} B_t u_t + \Sigma_{t-1}^{-1} \mu_{t-1})\]이다. 또한 \(x_{t-1}\)에 대한 \(L_t\)의 2차 도함수는 아래 식을 만족한다.
\[S = A_t^T R_t^{-1} A_t + \Sigma_{t-1}^{-1}\]이제 \(x_{t-1}\)의 완전 제곱식 \(L_t(x_{t-1}, x_t)\)를 얻었다. 따라서 남은 항 \(L_t - L_t(x_{t-1}, x_t)\)는 \(x_t\)의 2차 다항식이 된다.
\[\begin{align} L_t(x_t) =& L_t - L_t(x_{t-1}, x_t) \\ =& \frac{1}{2}(x_t - A_t x_{t-1} - B_t u_t)^T R_t^{-1} (x_t - A_t x_{t-1} - B_t u_t) \\ &+ \frac{1}{2}(x_{t-1} - \mu_{t-1})^T \Sigma_{t-1}^{-1} (x_{t-1} - \mu_{t-1}) \\ &- \frac{1}{2} (x_{t-1} - m)^T S (x_{t-1} - m) \\ =& \frac{1}{2} (x_t^T R_t^{-1} x_t + u_t^T B_t^T R_t^{-1} B_t u_t - 2 x_t^T R_t^{-1} B_t u_t) \\ &+ \frac{1}{2} \mu_{t-1}^T \Sigma_{t-1}^{-1} \mu_{t-1} \\ &- \frac{1}{2} (A_t^T R_t^{-1} x_t - A_t^T R_t^{-1} B_t u_t + \Sigma_{t-1}^{-1} \mu_{t-1})^T S^{-1} (A_t^T R_t^{-1} x_t - A_t^T R_t^{-1} B_t u_t + \Sigma_{t-1}^{-1} \mu_{t-1}) \\ \end{align}\]모든 항을 전개하고 \(S\)와 \(\Sigma_{t-1}\)의 관계를 사용한다.
\[\begin{align} S &= A_t^T R_t^{-1} A_t + \Sigma_{t-1}^{-1} \\ S^{-1} \Sigma_{t-1}^{-1} &= I - S^{-1} A_t^T R_t^{-1} A_t \\ \Sigma_{t-1}^{-1} S^{-1} &= I - A_t^T R_t^{-1} A_t S^{-1} \end{align}\]그러면 다음을 얻는다.
\[L_t(x_t) = \frac{1}{2} (x_t - A_t \mu_{t-1} - B_t u_t)^T (R_t^{-1} - R_t^{-1}A_t S^{-1} A_t^T R_t^{-1}) (x_t - A_t \mu_{t-1} - B_t u_t)\]\(x_t\)의 belief
\[\overline{bel} (x_t) = \eta’ \exp \left({-L_t(x_t)} \right)\]으로부터, \(x_t\)의 belief도 가우시안으로 분포함을 알 수 있다.
\[\overline{bel} (x_t) \sim \mathcal{N}(A_t \mu_{t-1} + B_t u_t, (R_t^{-1} - R_t^{-1}A_t S^{-1} A_t^T R_t^{-1})^{-1})\]나아가 우드버리 항등식(Woodbury identity)을 이용하면 공분산을 다음과 같이 단순화할 수 있다.
\[\begin{align} &R_t^{-1} - R_t^{-1}A_t S^{-1} A_t^T R_t^{-1} \\ &= R_t^{-1} - R_t^{-1}A_t (A_t^T R_t^{-1} A_t + \Sigma_{t-1}^{-1})^{-1} A_t^T R_t^{-1} \\ &= (R_t + A_t \Sigma_{t-1} A_t^T)^{-1} \end{align}\]따라서
\[\overline{bel} (x_t) \sim \mathcal{N}(A_t \mu_{t-1} + B_t u_t, A_t \Sigma_{t-1} A_t^T + R_t) := \mathcal{N}(\bar{\mu}_t, \bar{\Sigma}_t)\]이다. 최종적으로 예측 단계는 아래와 같다.
\[\begin{align} \bar{\mu}_t &= A_t \mu_{t-1} + B_t u_t \\ \bar{\Sigma}_t &= A_t \Sigma_{t-1} A_t^T + R_t \end{align}\]보정
측정 모델과 위 베이즈 필터의 보정 단계를 이용하면 다음을 얻는다.
\[\begin{align} bel(x_t) &= \eta p(z_t|x_t)\overline{bel}(x_t) \\ &= \eta \mathcal{N}(C_t x_t, Q_t)\mathcal{N}(\bar{\mu}_t, \bar{\Sigma}_t) \\ \end{align}\]예측 단계와 마찬가지로, 지수는
\[M_t := \frac{1}{2} (z_t - C_t x_t)^T Q_t^{-1} (z_t - C_t x_t) + \frac{1}{2} (x_t - \bar{\mu}_t)^T \bar{\Sigma}_t^{-1} (x_t - \bar{\mu}_t)\]이며, \(bel(x_t) = \eta\prime \exp \left(-M_t \right)\)을 만족한다.
\(M_t\)의 도함수를 계산하면,
\[\begin{align} &\frac{\partial M_t}{\partial x_t}\bigg\rvert_{x_t=\mu_t} = - (z_t - C_t x_t)^T Q_t^{-1} C_t + (x_t - \bar{\mu}_t)^T \bar{\Sigma}_t^{-1} = 0 \\ &\frac{\partial^2 M_t}{\partial x_t^2} = C_t^T Q_t^{-1} C_t + \bar{\Sigma}_t^{-1} \\ \end{align}\]\(bel(x_t)\)의 공분산과 평균을 얻는다.
\[\begin{align} \Sigma_t &= (C_t^T Q_t^{-1} C_t + \bar{\Sigma}_t^{-1})^{-1}\\ \mu_t &= (C_t^T Q_t^{-1} C_t + \bar{\Sigma}_t^{-1})^{-1} (C_t^T Q_t^{-1} z_t + \bar{\Sigma}_t^{-1}\bar{\mu}_t)\\ &= \Sigma_t (C_t^T Q_t^{-1} z_t + (\Sigma_t^{-1} - C_t^T Q_t^{-1} C_t)\bar{\mu}_t) \\ &= \bar{\mu}_t + \Sigma_t C_t^T Q_t^{-1} (z_t - C_t\bar{\mu}_t) \\ &= \bar{\mu}_t + K_t (z_t - C_t\bar{\mu}_t) \\ \end{align}\]여기서 \(K_t := \Sigma_t C_t^T Q_t^{-1}\)는 칼만 이득(Kalman gain)이다. 따라서 보정 단계는 아래와 같다.
\[\begin{align} \mu_t &= \bar{\mu}_t + \Sigma_t C_t^T Q_t^{-1} (z_t - C_t\bar{\mu}_t) \\ \Sigma_t &= (C_t^T Q_t^{-1} C_t + \bar{\Sigma}_t^{-1})^{-1} \end{align}\]계산 효율성
대부분의 실용적 응용에서, 상태 벡터의 차원(\(n\))은 측정 벡터의 차원(\(l\))보다 훨씬 크다. 즉, \(A_t \in \mathbb{R}^{m \times n}\), \(B_t \in \mathbb{R}^{m\times k}\), \(R_t \in \mathbb{R}^{m\times m}\), \(C_t \in \mathbb{R}^{l\times m}\), \(Q_t \in \mathbb{R}^{l\times l}\)에 대해 \(l\)은 \(n\)보다 작다. 이 경우 \(n \times n\) 행렬 \(\Sigma_t\)의 역행렬을 직접 계산하는 것은 비용이 크다.
행렬 역변환 보조정리(Matrix Inversion Lemma)를 적용하면 칼만 이득을 다시 쓸 수 있다. \(K_t\)의 \(\Sigma_t\)를 \(\bar{\Sigma}_t\)로 치환하면 다음을 얻는다.
\[\begin{align} K_t &= (C_t^T Q_t^{-1} C_t + \bar{\Sigma}_t^{-1})^{-1} C_t^T Q_t^{-1} \\ &= (\bar{\Sigma}_t - \bar{\Sigma}_t C_t^T (Q_t + C_t \bar{\Sigma}_t C_t^T)^{-1} C_t \bar{\Sigma}_t) C_t^T Q_t^{-1} ~\because {\rm Woodbury ~identity}\\ &= \bar{\Sigma}_t C_t^T (I - (Q_t + C_t \bar{\Sigma}_t C_t^T)^{-1} C_t \bar{\Sigma}_t C_t^T) Q_t^{-1} \\ &= \bar{\Sigma}_t C_t^T (Q_t + C_t \bar{\Sigma}_t C_t^T)^{-1}(Q_t + C_t \bar{\Sigma}_t C_t^T - C_t \bar{\Sigma}_t C_t^T) Q_t^{-1}\\ &= \bar{\Sigma}_t C_t^T (Q_t + C_t \bar{\Sigma}_t C_t^T)^{-1} \\ \end{align}\]\(Q_t\)와 \(C_t \bar{\Sigma}_t C_t^T\)의 차원은 \(n\)보다 작은 \(l\)이므로, \(K_t\)를 더 적은 계산 부하로 계산할 수 있다. 이 형태는 더 효율적일 뿐 아니라, 명시적인 대형 행렬 역변환 없이 공분산을 갱신할 수 있게 한다.
\[\begin{align} K_t C_t &= \Sigma_t C_t^T Q_t^{-1} C_t \\&= \Sigma_t (\Sigma_t^{-1} - \bar{\Sigma}_t^{-1}) \\ &= I - \Sigma_t \bar{\Sigma}_t^{-1} \end{align}\]그러면 \(\Sigma_t = (I - K_t C_t) \bar{\Sigma}_t\)가 유도된다.
정리하며
마침내, 칼만 필터의 업데이트 방정식은 다음과 같이 정리할 수 있다.
- 예측
- 보정
칼만 필터는 시간에 따라 가우시안 belief를 유지하는 재귀 알고리즘이다. 위 유도로부터, 두 가우시안의 곱이 또 다른 가우시안이며, 가우시안 노이즈 하의 선형 시스템에 대해 최적 추정을 제공함을 관찰할 수 있다. 평균값이 분산을 최소화한다는 성질을 활용하여, 오차 분산 최소화를 통해서도 위 방정식들을 유도할 수도 있다.
칼만 필터는 선형 시스템에서 효과적으로 적용되나, 유도에 사용한 가정들, 즉 선형성과 가우시안에 의해 종종 한계점이 발생한다. 이 어려움을 극복하기 위해, 비선형 시스템을 위한 Extended KF, Unscented KF나, 비가우시안 분포를 위한 파티클 필터 같은 변형을 고려할 수 있다.