Distributed SLAM Using Improved Particle Filter for Mobile Robot Localization

Algorithm 1

Improved distributed particle filter (Improved DPF).

Step  1. Initialization, initialize the position of the

robot 𝐗_𝑣, and the particles 𝐗^𝑗_𝑘 in local filters.

 for 1∶𝑛

 {

  Step  2. Reconstruct the local filters

  {

  Delete the local filters without input observation

Added local filters for new observation

  }

  Step  3. Generate the probabilities of the particles

  The probabilities of the particles in local filters

are calculated by (*). The particles closer

to the observation have larger probabilities.

  𝑤_𝑘^𝑖∗=1(2𝜋)^𝑛𝑣/2det𝐑_𝑘𝑒^{−(1/2)(𝐳𝑘−𝐡𝑘(𝐱𝑘𝑖∗))𝑇𝐑𝑘−1(𝐳𝑘−𝐡𝑘(𝐱𝑘𝑖∗))}    (*)

   (a) Update the weights of the particles in time k by  (*).

   (b) Calculate the variance of the particles and

    use the variance as parameter  𝐑^′_𝑘 to

    recalculate the  𝐰_𝑘^𝑖∗ by  (*).

   (c) Update  𝝎^𝑖_𝑘 by (**)

     𝝎^𝑖_𝑘=𝝎^𝑖_𝑘−1𝑝(𝐳_𝑘∣𝐱^𝑖_𝑘)=𝝎^𝑖_𝑘−1𝐰_𝑘^𝑖∗    (**)

  Step  4. Evaluate the state in every local filter by (***).

       𝑥_𝑘=∑^𝑛_𝑖=1𝑥^𝑖_𝑘∗𝑤^𝑖_𝑘   (***)

  Step  5. Calculate the evaluation result of the master filter

  The result of evaluation calculated by each local

filter is transformed to the master filter. And the

weight of each local filter is calculated by  (17).

  Step  6. Save the distribution of the particles in every local filter

  The probabilities of the particles in every local filter

are saved to generate the distribution of the particles in next iteration.

  Step  7. Resample

  Where 𝑁_threshold is the threshold of the number of

peffective particle, if the 𝑁_eff𝑖 of a local filter lower

than 𝑁_threshold, this local filter should be resample.

  if (𝑁_eff𝑖<𝑁_threshold)

  {

   Resample this local filter;

  }

 }