Kalman gain 구하기
아래의 내용은 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} $$