STEP

Description

Mathematical Representation

Equation

1

Initialization (k = 0) – initialize particle weights.

e.g., $w_k^i~1/N_s$ , $i=1,\cdots ,N_s$ .

(8)

2

Prediction—predict the weights..

$w_{k\backslash k-1}^i={\displaystyle \sum _{j=1}^{N_s}{w}_{k-1\backslash k-1}^{j}p\left(x_k^i|x_{k-1}^j\right)}$

(9)

3

Update—update the weights.

$w_{k\backslash k}^i=\frac{w_{k\backslash k-1}^{i}p\left(z_k|x_k^i\right)}{{\displaystyle {\sum }_{j=1}^{N_s}{w}_{k\backslash k-1}^{j}p\left(z_k|x_k^j\right)}}$

(10)

4

Obtain optimal minimum variance estimate of the state vector and corresponding error covariance.

${\widehat{x}}_k\approx {\displaystyle \sum _{i=1}^{N_s}{w}_{k|k}^i{x}_k^i}$

$P_{{\widehat{x}}_k}\approx {\displaystyle \sum _{i=1}^{N_s}{w}_{k|k}^i\left(x_k^i-{\widehat{x}}_k\right){\left(x_k^i-{\widehat{x}}_k\right)}^{\text{T}}}$

(11)

5

Let k = k + 1 & iterate to step 2.

In the above equations it is required that the likelihood pdf $p\left(z_k|x_k^i\right)$ and the transitional probabilities $p\left(x_k^i|x_{k-1}^j\right)$ be known and specified.