Nonlinear Bayesian Tracking Loops for Multipath Mitigation

Algorithm 1

Standard particle filtering (SPF).

Require:  {𝐱^(𝑖)_𝑛−1,𝑤^(𝑖)_𝑛−1}^𝑁_𝑖=1 and 𝐲_𝑛

Ensure:  {𝐱_𝑛^(𝑖),𝑤_𝑛^(𝑖)}^𝑁_𝑖=1 and 𝐱_𝑛

  1:   for  𝑖=1 to 𝑁  do

  2:    Generate 𝐱_𝑛^(𝑖)∼𝜋(𝐱_𝑛|𝐱^(𝑖)_0∶𝑛−1,𝐲_1∶𝑛)

  3:    Calculate 𝑤_𝑛^(𝑖)=𝑤^(𝑖)_𝑛−1𝑝(𝐲_𝑛|𝐱^(𝑖)_0∶𝑛,𝐲_1∶𝑛−1)𝑝(𝐱_𝑛^(𝑖)|𝐱^(𝑖)_𝑛−1)𝜋(𝐱_𝑛^(𝑖)|𝐱^(𝑖)_0∶𝑛−1,𝐲_1∶𝑛)

  4:    end  for

  5:    for  𝑖=1 to 𝑁  do

  6:      Normalize weights: 𝑤_𝑛^(𝑖)=𝑤_𝑛^(𝑖)∑^𝑁_𝑗=1𝑤_𝑛^(𝑗)

  7:    end  for

  8:    MMSE state estimation:  𝐱_𝑛=∑^𝑁_𝑖=1𝑤_𝑛^(𝑖)𝐱_𝑛^(𝑖)

  9:   {𝐱_𝑛^(𝑖),1/𝑁}^𝑁_𝑖=1 = Resample({𝐱_𝑛^(𝑖),𝑤_𝑛^(𝑖)}^𝑁_𝑖=1)