System Model and | System model : $ x_{k+1}=f(x_k,u_k,k)+Gw_k $ |
Measurement Model | Measurement model : $ z_k=h(x_k,k)+v_k $ |
Assumption : $ x(0)\sim X({\tilde x}_0,P_0);\; \; w(k)\sim N(0,Q_k); $ | |
Assumption : $ v_k\sim N(0,R) $ | |
Initialization | $ {\tilde x}(0)={\tilde x}_0;\; \; P(0)=P_0 $ |
Time Predict | Estimation : $ \hat{x}_{k+1}^-=f(\hat{x}_{k}^-,u_k) $ |
Covariance : $ P_{k+1}^-=AP_kA^T+G_kQ_kG_k^T $ | |
Measurement Update | Kalman gain : |
$ K_{k+1}=P_{k+1}^-H^T(H_{k+1}P_{k+1}^-H^T+R_{k+1})^{-1} $ | |
Estimation : $ \hat{x}_{k+1}=\hat{x}_{k+1}^-+K_{k+1}(z_{k+1}-H\hat{x}_{k+1}^-) $ | |
Error covariance : $ P_{k+1}=(I-K_{k+1}H)P_{k+1}^- $ |