Table 2.

EKF estimation steps.

Step	EKF	Real System
Evolution	${\tilde{\mathbf{X}}}_{k+1}=f({\widehat{\mathbf{X}}}_k,{\mathbf{U}}_k,0)$	${\mathbf{X}}_{k+1}=f({\mathbf{X}}_k,{\mathbf{U}}_k,{\mathbf{V}}_k)$
	$[{\tilde{\mathbf{P}}}_{\mathbf{k}+\mathbf{1}}]=\left[{\mathbf{F}}_{X,k+1}\right][{\widehat{\mathbf{P}}}_k]{\left[{\mathbf{F}}_{X,k+1}\right]}^T$
	$+\left[{\mathbf{F}}_{V,k+1}\right]\left[\mathbf{Q}\right]{\left[{\mathbf{F}}_{V,k+1}\right]}^T$
Prediction/measurement	${\tilde{\mathbf{Y}}}_k=h({\tilde{\mathbf{X}}}_k,{\mathbf{U}}_k,0)$	${\mathbf{Y}}_k=h({\mathbf{X}}_k,{\mathbf{U}}_k,{\mathbf{W}}_k)$
Correction	${\mathbf{E}}_k={\mathbf{Y}}_k-{\tilde{\mathbf{Y}}}_k$
	$\left[{\mathbf{K}}_k\right]=[{\tilde{\mathbf{P}}}_{\mathbf{k}}]{\left[{\mathbf{H}}_{X,k}\right]}^T{(\left[{\mathbf{H}}_{X,k}\right][{\tilde{\mathbf{P}}}_k]{\left[{\mathbf{H}}_{X,k}\right]}^T+\left[\mathbf{R}\right])}^{-1}$
	${\widehat{\mathbf{X}}}_k={\tilde{\mathbf{X}}}_k+\left[{\mathbf{K}}_k\right]{\mathbf{E}}_k$
	$[{\widehat{\mathbf{P}}}_k]=(\mathbf{I}-\left[{\mathbf{K}}_k\right]\left[{\mathbf{H}}_{X,k}\right])[{\tilde{\mathbf{P}}}_k]$