이산 선형 칼만 필터 중간 유도 정리

확장 칼만 필터에 대해서부터 전개해보자.

 

System model and Measurement model

$$\matrix{x_{k+1}  = F_k  x_k + w_k & w_k \sim \mathcal{N} (0, \sigma_x) \\ z_k = H_k x_k + v_k & v_k \sim \mathcal{N}(0, \sigma_z) } $$

 

상태 방정식으로 표현된 위 시스템에 대해서 다음과 같이 칼만 필터 과정이 전개된다.

 

0. Parameter Initialization

Initial Posteriori estimate vector and its covariance matrix

$$P_0, \hat{x}_0$$

Set System noise covariance matrix and Measurement noise covariance matrix

$$Q, R$$

 

1. Time Update (Prediction Step)

Priori estimate $\hat{x}_k^-$ and Priori covariance matrix $P_k^-$

$$\cases{\hat{x}_{k}^- = F_k \hat{x}_{k-1} \\ P_k^- = E \left[e_k^- \left(e_k^- \right) ^T \right] = F_k P_{k-1} F_k^T + Q_k }$$

 

Innovation vector $d_k^-$ and Innovation covariance matrix $S_k^-$

$$\matrix{d_k^- &=& z_k - H_k \hat{x}_k^- \\   &=& v_k + H_k \left( x_k - \hat{x}_k^- \right) \\ S_k^- &=& R_k + H_k P_k^- H_k^T } $$

 

Get Kalman Gain

$$K_k = P_k^- H_k^T \left(S_k^- \right)^{-1}$$

 

2. Measurement Update (Estimation Step)

여기서 계측 값 $z_k$에 대해 연산을 수행하고, 추정 상태 $\hat{x}_k$을 얻을 수 있다.

Posteriori estimate vector $\hat{x}_k$ and Posteriori estimate covariance matrix $P_k$

$$\cases{\hat{x}_k = \hat{x}_k^- + K_k d_k^- \\ P_k = P_k^- - K_k S_k^{-1} K_k^T = \left(I -K_k H_k \right) P_K^- }$$

 

Residual vector $d_k$ and Residual covariance matrix $S_k$

$$\cases{d_k = z_k - H_k \hat{x}_k \\ S_k = R_k + H_k P_k H_k^T} $$

 

유도 1. [4]

Priori error covariance matrix 의 유도

$$P_k^- = E \left[e_k^- \left(e_k^- \right) ^T \right] = F_k P_{k-1} F_k^T + Q_k $$

$$\matrix{ e_k^-  &=& x_k - \hat{x}_k \\ &=& F_{k-1} x_{k-1} - \left(F_{k-1} \hat{x}_{k-1} + w_k  \right)  \\ &=& F_{k-1} \left(x_{k-1} - \hat{x}_{k-1} \right) - w_k \\ &=& F_{k-1} e_{k-1} - w_k}$$

Posteriori error covariance matrix 와 Kalman Gain의 유도

priori estimate $\hat{x}_k^-$ 를 계측 $z_k$으로 개선하기 위해서, 다음 식과 같이 잡음이 있는 계측과 priori estimate 의 선형 결합을 선택해보자.

$$\hat{x}_k = \hat{x}_k^- + K_k \left( z_k - H_k \hat{x}_k^- \right)$$

$$z_k =H_k x_k + v_k$$

$K_k$를 결합 계수라고 하자. 여기서의 문제는 갱신한 추정이 최적인 결합 계수 $K_k$를 찾아내는 것이다. 그래서 Wiener solution 과 같이, 성능 지표로써 평균 제곱 오차가 최소가 되도록 한다. 갱신된 추정인 Posteriori estimate와 연관된 오차 공분산 행렬 $P_k$를 구성해보면 다음과 같다.

$$P_k = E \left[ e_k e_k^T \right] = E \left[ \left(x_k - \hat{x}_k \right) \left(x_k - \hat{x}_k \right)^T \right] $$

$$\matrix{ \left(x_k - \hat{x}_k \right) & = & \left(x_k - \hat{x}_k^- \right) - K_k \left( H_k x_K + v_k - H_k \hat{x}_k^- \right) \\ & =&  \left[I-K_k H_k \right] \left[ x_k \hat{x}_k^- \right] - K_k v_k } $$

위 식으로부터 $P_k$를 구하면 다음과 같은 quadratic form 의 $P_k$를 구하게 된다. 이는 $P_k^-$와 $R_k$가 양의 정정행렬이라면 $P_k$가 양의 정정행렬이 된다. 이를 Joseph Stabilized version이라고 한다.[3]

$$\matrix{ P_k & = & \left( I - K_k H_k \right) P_k^- \left( I - K_k H_k \right)^T + K_k R_k K_k^T \\ & = & P_k^- - K_k H_k P_k^- - P_k^- H_k^T K_k^T + K_k \left( H_k P_k H_k^T + R_k \right)\ K_k^T }  $$

위 식은 $K_k$ 에 대해 linear 및 quadratic 한 항으로 구성되어있다. 상태 벡터 요소의 추정 값에서 평균 제곱 오차의 합을 최소화하기 위해서 $P_k$를 최소화하도록 하는 $K_k$를 구해보자.

여기서 행렬 미분 공식을 활용해보면..

$$\matrix{ \frac{d \left[ \text{tr}\left( AB \right) \right]} {dA} & = & B^T & AB \in \mathbb{R}^{n \times n} \\ \frac{d \left[ \text{tr} \left( ACA^T \right) \right]}{dA} &=&2AC & C=C^T}$$

$$\frac{d \left[ \text{tr} \left( P_k \right) \right] }{dK_k } = -2 \left( H_k P_k^- \right)^T + 2 K_k \left( H_k P_k H_K^T + R_k \right)$$

$$\therefore)  K_k = P_k^- H_k^T \left( H_k P_k^- H_k^T + R_k \right)^{-1} $$

위의 결합 계수를 칼만 이득이라고 한다.

Joseph Stabilized version 식을 전개하고, 위의 칼만 이득을 넣으면 다시한번 정리할 수 있다.

$$P_k = \left(I - K_k H_k \right) P_k^- $$

 

정리하면..

$$\matrix{P_k & =& P_k^- - K_k H_k P_k^- - P_k^- H_k^T K_k^T + K_k \left( H_k P_k H_k^T + R_k \right)\ K_k^T \\ & = & \left(I - K_k H_k \right) P_k^- \\ & = & P_k^- - P_k^- H_k^T \left( H_k P_k^- H_k^T + R_k \right)^{-1} H_k P_k^- \\ & = & P_k^- - K_k \left( H_k P_k^- H_k^T + R_k \right) K_k^T } $$

위와 같이 posteriori error covariance matrix $P_k$를 계산하였으니, 이를 기반으로 priori error covariance matrix $P_k^-$를 계산해보자.

$$\matrix{ e_{k+1}^- &=& x_{k+1} - \hat{x}_{k+1}^- \\ &=& F_k x_k  - F_k \hat{x}_k - w_k \\ &=& F_k e_k - w_k }$$

$$ \matrix{ P_k^- &=& E \left[ e_k^- \left( e_k^- \right)^T \right] \\ \therefore) P_k^-&=&F_k P_k F_k^T + Q_k  }$$

 

[1] Akhlaghi, S., Zhou, N., & Huang, Z. (2017). Adaptive adjustment of noise covariance in Kalman filter for dynamic state estimation. 2017 IEEE Power & Energy Society General Meeting. doi:10.1109/pesgm.2017.8273755

[2] WANG, J. (1999). Stochastic Modeling for Real-Time Kinematic GPS/GLONASS Positioning. Navigation, 46(4), 297–305. doi:10.1002/j.2161-4296.1999.tb02416.x

[3] Lewis, F. L, Xie, L, and Popa D., Optimal and Robust Estimation - with an Introduction ot Stochastic Control Theory, 2nd, pp.73-75.

[4] Brown R. G. and Hwang P. Y. C. Introduction to Random Signals and Applied Kalman Filtering, 3rd Ed., pp.214-247.

 

** EOF **