Monte-Carlo-Localization.md

MONTE-CARLO-LOCALIZATION

AIMA3e

function MONTE-CARLO-LOCALIZATION(a, z, N, P(X'|X, v, w), P(z|z*), m) returns a set of samples for the next time step
 inputs: a, robot velocities v and w
     z, range scan z₁,..., z_M
     P(X'|X,v,w), motion model
     P(z|z*), range sensor noise model
     m, 2D map of the environment
 persistent: S, a vector of samples of size N
 local variables: W, a vector of weights of size N
          S', a temporary vector of particles of size N
          W', a vector of weights of size N

 if S is empty then   /* initialization phase */
   for i = 1 to N do
     S[i] ← sample from P(X₀)
 for i = 1 to N do  /* update cycle */
   S'[i] ← sample from P(X'|X = S[i], v, w)
   W'[i] ← 1
   for j = 1 to M do
      z* ← RAYCAST(j, X = S'[i], m)
      W'[i] ← W'[i] . P(z_j|z*)
 S ← WEIGHTED-SAMPLE-WITH-REPLACEMENT(N, S', W')
 return S

---

FIGURE ?? A Monte Carlo localization algorithm using a range-scan sensor model with independent noise.