Linear-Kalman-Filter-for-2D-object-Tracker

the System Model:

$x_k=F_{k-1}x_{k-1}+w_{k-1}$

the Observation Model:

$z_k=H_{k}x_{k}+v_{k}$

the Assumptions:

1)Gaussian error with 0 mean and covariance matrix

$w_k \thicksim N(0,Q_k)$
$v_k \thicksim N(0,R_k)$

2)Time independent error

$E(w_kw^T_j)=Q_k\delta_{k-j} $
$E(v_kv^T_j)=R_k\delta_{k-j} $

3)Uncorrelation of measurement noise with dynamical noise

$E(w_kv^T_j)=0 $

the state vector:

$x_k={[p_x,p_y,v_x,v_y]}^T_k$

F matrix:

$[1 \quad 0 \quad \Delta t \quad 0]$
$[0 \quad 1 \quad 0 \quad \Delta t]$
$[0 \quad 0 \quad 1 \quad 0]$
$[0 \quad 0 \quad 0 \quad 1]$


noise input :

$w_k=[a_x,a_y,a_x,a_y]^T_k$
$\sigma_{a}=\sigma_{a_x}=\sigma_{a_y}$


Q matrix :

$[\frac{1}{2}\Delta t^2 \quad 0 \quad 0 \quad 0 ]$
$[ 0 \quad \frac{1}{2}\Delta t^2 \quad 0 \quad 0 ]$
$[ 0 \quad 0\quad \quad \Delta t \quad 0 ]$
$[ 0 \quad 0\quad \quad 0\quad \Delta t ]$


we denote the a prori estimate or the forward estimate by f and the posteriori estimate the analysis estimate by a

prediction :

$x_k=Fx_{k-1}$
$P_{f,k}=FP_{a,k-1}F^T+Q$

Innovation :

$z_k$ is our measurement vector
$\tilde{y_k}=z_k - H_k \hat{x}_{f,k}$

Innovation Covariance :

$S_k=H_k P_{f,k} H_k^T + R_k$

Update

$\hat{x_{a,k}} = \hat{x}_{f,k} + K_k \tilde{y_k}$

$P_{a,k}=(I-K_kH_k)P_{f,k}$

$K_k=P_{f,k}H^T_kS_k^{-1}$

$S_k=H_kP_{f,k}H^T_k+R_k$