아래의 내용은 KALMAN FILTERING Theory and Practice Using MATLAB 이란 책의 내용을 인용하여 내용을 한글로 정리한 글이다.
1. Estimator in Linear Form
선형 continuous time에서의 plant 방정식과 출력은 다음과 같다.
$$ \dot{x} = F(t)x(t)+w(t) $$
$$ z(t)=H(t)x(t)+v(t)$$
Discrete time 에서는
$$ x_k = \Phi_{k-1}x_{x-1}+w_{k-1} $$
$$ z_k=H_k x_k+v_k $$
Optimal linear estimate는 \(x\), \(z\)가 jointly Gaussian이면 general(nonlinear) optimal estimator와 동일하다. 그러므로 \( z_k \)를 관찰(observation)하여 \( \hat{x} _{k(+)} \)를 추정(estimate)한다. 즉 priori 추정과 \(z\)의 선형 함수가 \( \hat{x} _{k(+)} \) 이다.
$$ \hat{x} _{k(+)} =K^1_k\hat{x} _{k(-)} + \bar{K}_k z_k $$
여기 \( \hat{x} _{k(-)} \) 는 \( x_k \)의 priori estimate 이고 \( \hat{x} _{k(+)} \) 는 posteriori estimate이다.
2. Kalman Gain 풀기
\(K^1_k \)와 \( \bar{K}_k \)는 아직 알려져 알려져 있지 않지만, 적당한 \(K^1_k \)와 \( \bar{K}_k \) 를 찾아서 새로운 estimate \( \hat{x} _{k(+)} \) 가 orthogonality principle을 만족 하도록 한다.
$$ \begin{split} & E\langle [ x_k - \hat{x}_{k(+)}]z^T_i \rangle = 0, i=1,2,...,k-1, \\ & E\langle [ x_k - \hat{x}_{k(+)}]z^T_k \rangle = 0. \end{split} $$
여기서 \( z_1, ... , z_k\)는 노이즈 텀인 \( w_k\) 을 포함하고 있지 않다. 그러므로 \( w_k\) 와 \( v_k\)는 상관관계가 아니고 \( E \langle w_k z_i^T\rangle = 0 \quad for \quad 1\le i \le k \)
$$ E \langle [ \Phi_{k-1} x_{k-1} + w_{k-1} - K_{k}^1 \hat{x}_{k(-)} - \bar{K}_k z_k ] z_i^T\rangle = 0 \quad for \quad i=1, ..., k-1 $$
그런데, \( z_k=H_k x_k+v_k \) 이므로 위식은 아래와 같이 된다.
$$ E \langle [ \Phi_{k-1} x_{k-1} - K_{k}^1 \hat{x}_{k(-)} - \bar{K}_k H_k x_k - \bar{K}_k v_k ] z_i^T\rangle = 0 \quad for \quad i=1, ..., k-1 $$
또한 아래 식도 성립한다.
$$ E \langle [ x_{k-1} - \hat{x}_{(k-1)(+)} ]z^T_k \rangle = 0 \quad i=1, ..., k-1 $$
그리고
$$ E \langle v_{k}z^T_k \rangle = 0 \quad i=1, ..., k-1$$
\( E \langle [ \Phi_{k-1} x_{k-1} - K_{k}^1 \hat{x}_{k(-)} - \bar{K}_k H_k x_k - \bar{K}_k v_k ] z_i^T\rangle = 0 \quad for \quad i=1, ..., k-1 \)은 아래와 같이 정리 된다.
$$ \Phi_{k-1} E \langle x_{k-1}z_i^T \rangle - K_{k}^1 E \langle \hat{x}_{k(-)}z_i ^T \rangle - \bar {K}_{k} H_k \Phi_{k-1} E \langle x_{k-1}z_i^T \rangle - \bar{K}_{k} E \langle v_{k}z_i^T \rangle = 0, \\ \Phi_{k-1} E \langle x_{k-1}z_i^T \rangle - K_{k}^1 E \langle \hat{x}_{k(-)}z_i ^T \rangle - \bar {K}_{k} H_k \Phi_{k-1} E \langle x_{k-1}z_i^T \rangle = 0, \\ E \langle [x_{k} - \bar {K}_{k} H_k x_k - K_k^1 x_k] z_i^T - K_{k}^1 ( \hat{x}_{k(-)} -x_k)z_i^T \rangle = 0, \\ [I-K_k^1 - \bar {K}_{k} H_k ]E \langle x_k z_i^T \rangle = 0. $$
만약
$$ K_k^1 = I - \bar {K}_{k} H_k $$
이면 \( x_k \)에 상관 없이 위 식이 성립한다.
여기서 선택된 \( K_k^1 \)은 \( \hat{x} _{k(+)} =K^1_k\hat{x} _{k(-)} + \bar{K}_k z_k \) 을 만족하고, 위에 언급된 orthogonality principle도 \( E\langle [ x_k - \hat{x}_{k(+)}]z^T_i \rangle = 0 \) 만족한다. 또한 \( \bar{K}_k \) 도 \( E\langle [ x_k - \hat{x}_{k(+)}]z^T_k \rangle = 0 \)을 만족한다.
여기서 에러를 다음과 같이 정의한다.
$$ \begin{split} & \tilde{x}_{k(+)} = \hat{x_{k(+)}} - x_k, \\ & \tilde{x}_{k(-)} =\hat{x}_{k(-)} - x_k, \\ & \tilde{z}_{k(+)} = \hat{z}_{k(-)} - z_k \\ &=H_k \hat{x}_{k(-)} - z_k \end{split} $$
업데이트 전후의 \( \tilde{x}_{k(+)}\; , \tilde{x}_{k(-)}\) 는 각각 estimation error 이고 이 때 파라미터 \( \hat{x}_{k(+)} \)는 \( x_{k}\)에 선형적인 합이며 \( x_{k}\) 또한 \( z_{k}\)의 선형적인 합이다. 그래서 아래 식이 성립한다.
$$ E\langle [ x_k - \hat{x}_{k(+)}] \hat{z}^T_{k(-)} \rangle = 0 $$
또한
$$ E\langle [ x_k - \hat{x}_{k(+)}] \tilde{z}^T_{k(-)} \rangle = 0 $$
가 성립한다.
\( x_k \; ,\hat{x}_{k(+)}\; , \tilde{z}_{k}\)를 위식에서 해당하는 식으로 바꾸면
$$ E\langle [ \Phi_{k-1} x_{k-1}+w_{k-1} - K_{k}^1 \hat{x}_{k(-)} - K_{k}^1 z_k] [H_k \hat{x}_{k(-)} -z_{k}]^T \rangle = 0 $$
하지만 시스템에서 \( E\langle w_k z^T_{k} \rangle = E\langle w_k \hat{x}^T_{k(+)} \rangle = 0 \) 이므로
$$ E\langle [ \Phi_{k-1} x_{k-1} - K_{k}^1 \hat{x}_{k(-)} - K_{k}^1 z_k] [H_k \hat{x}_{k(-)} -z_{k}]^T \rangle = 0 $$
이다.
\( K_{k}^1 \;, z_{k}\;, \tilde{x}_{k(-)} \)를 \( E\langle \tilde{x}_k v^T_{k(-)} \rangle = 0 \) 을 사용하여 바꾸면
$$ \begin{split} 0 &= E\langle [ \Phi_{k-1} x_{k-1} - \hat{x}_{k(-)} + \bar{K}_k H_k \hat{x}_{k(-)} - \bar{K}_k H_k x_{k} -\bar{K}_k v_k] [H_k \hat{x}_{k(-)} - H_k x_{k} - v_k]^T \rangle \\ &= E\langle [ (x_k - \hat{x}_{k(-)}] - \bar{K}_k H_k (x_k - \hat{x}_{k(-)}) - \bar{K}_k v_k] [H_k \tilde{x}_{k(-)} - v_k]^T \rangle \\ &= E\langle [ -\tilde{x}_{k(-)} \bar{K}_k H_k \tilde{x}_{k(-)} - \bar{K}_k v_k] [H_k \tilde{x}_{k(-)} - v_k]^T \rangle \end{split} $$
priori covariance의 정의에 의하여
$$ P_{k(-)} = E\langle \tilde{x}_{k(-)} \tilde{x}_{k(-)}^T \rangle $$
이것은 다음 식을 만족한다.
$$ [I -\bar{K}_k H_k ]P_{k(-)} H_k^T - \bar{K}_k R_k = 0 $$
그러므로 kalman gain은 다음과 같이 표현 된다.
$$ \bar{K}_k = P_{k(-)} H_k^T [H_k P_{k(-)} H_k^T +R_k]^{-1} $$