I have been using GMapping in simulation and on a physical robot recently and wanted to write a note to serve as a reminder how Bayesian filtering with particles, Rao-Blackwellization, SLAM etc. all come together in Grisetti, Stachniss, and Burgard's TRO 2007 paper. My goal is to summarize some of the required background and the important results without going into all derivations in the paper. An excellent background read is the Probabilistic Robotics book on which the background material here is based.


Note: You may have to refresh this page (F5) if the math below is not displayed correctly.


The Bayes filter:
The Bayes filter is a recursive state estimator of the posterior of state $x$ at times $1\ldots t$. Adapted to the robotics domain, we define $bel(x_t):=p(x_t|z_{1:t},u_{1:t})$ to be the posterior of the state at time $t$, given sensory observations (such as laser scans) up to time $t$, $z_{1:t}$, and control signals (such as odometry readings) $u_{1:t}$. The filter requires initialization with $bel(x_0):=p(x_0)$ which can be set to a uniform distribution in case no specific knowledge about the initial state is available.
Under the Markov assumption, we can derive a recursive relation from $bel(x_{t-1})$ to $bel(x_t)$ as a two-step process: \[\overline{bel}(x_t) = \int p(x_t|u_t,x_{t-1}) bel(x_{t-1})\,\mathrm{d} x_{t-1}\] \[bel(x_t) = \eta p(z_t|x_t) \overline{bel}(x_t)\] These are known as prediction and measurement update (or correction) steps, respectively.

The Particle filter:
The previous statement of the Bayes filter did not make any claims about the form of the distributions $bel(x_t)$, observation model $p(z_t|x_t)$ and transition model $p(x_t|u_t,x_{t-1})$. Under linear-Gaussian assumptions, the Kalman filter gives analytic update equations for the mean and covariance of the posterior Gaussian $bel(x_t)$. If we don't want to commit to such a parametric form of the posterior, we can use a particle filter which represents $bel(x_t)$ by a set of random state samples $x_t^{[m]}$ for $m=1\ldots M$ drawn from this posterior.
The actual particle filter algorithm is quite straight-forward (see, e.g. Table 4.3 on page 98 of the Probabilistic Robotics book). Because we cannot sample from the target distribution $bel(x_t)$ directly, the particle filter uses a trick and draws samples from a proposal distribution $\pi(x_t)$ instead. Each such sampled particle $x_t^{[m]}$ is then associated with an importance weight given by $w_t^{[m]} = \frac{bel(x_t^{[m]})}{\pi(x_t^{[m]})}$, denoting the "difference" between target and proposal distributions. Lastly, particles are redrawn with replacement with a likelihood proportional to their associated weights, a process known as importance sampling. This last resampling step ensures that the resulting particles are distributed according to the posterior distribution of interest, $bel(x_t)$.

Proposal distributions:
In order to ensure a quality representation of $bel(x_t)$ with a limited set of $M$ particles, we do not want to waste our particle set with samples generated in regions that are associated with a low likelihood under $bel(x_t)$. In that sense, it is beneficial for $\pi(x_t)$ to be close to $bel(x_t)$. As discussed in the paper, the frequent choice of the odometry motion model $p(x_t|u_t,x_{t-1})$ for proposal $\pi(x_t)$ is not ideal because weights $w_t^{[m]}$ are then computed according to the observation model $p(z_t|x_t)$, which, for precise laser scanners, likely results in a small set of particles associated with large weights while the remaining particles only experience negligible weights. Grisetti, Stachniss, and Burgard therefore suggest to include the observations $z_t$ in the proposal function when generating the next generation of samples, more on this below.
Another issue of the particle filter is the variance inherent in random sampling. In general, we would like the particle set drawn from the proposal distribution to repeatably be a good representation of the true density, i.e. to display low variance. Doucet derived an optimal proposal $\pi(x_t)$ with respect to the variance of the particle weights which has a convenient recursive form \[\pi(x_{1:t}|z_{1:t}, u_{1:t}) = \pi(x_t|x_{1:t-1},z_{1:t},u_{1:t}) \pi(x_{1:t-1}|z_{1:t-1},u_{1:t-1}) \] allowing us to compute the particle weights at time $t$, $w_t$, recursively as well. The exact form of the optimal proposal $\pi(x_t)$ together with the recursive weight computation is given in the paper in Equations 9 and 13, respectively.

SLAM and Rao-Blackwellization:
The SLAM (simultaneous localization and mapping) problem can be stated in two forms, an online and a full form. The former is to obtain the posterior joint distribution over location of the robot at time $t$ together with the map, $p(x_t, m|z_{1:t},u_{1:t})$ while the latter is concerned with the entire robot path $x_{1:t}$ and can be stated as $p(x_{1:t}, m|z_{1:t},u_{1:t})$. Rao-Blackwellization uses the following factorization \[p(x_{1:t}, m|z_{1:t},u_{1:t})=p(m|x_{1:t},z_{1:t})p(x_{1:t}|z_{1:t},u_{1:t})\] which makes this problem a trajectory estimation problem (second factor) and a map computation (first factor). The mapping part given known trajectory $x_{1:t}$ is easily solved and a particle filter can be used to estimate the path distribution (the second factor). Note that here each particle corresponds to a path instantiation $x_{1:t}^{[k]}$ and has an associated map $m$ based on that path.

We are now ready to state the contributions of the paper. First, the authors present a proposal distribution that incorporates the observations $z_t$ (as briefly mentioned above) and show how to compute it with gridmaps. Second, the authors describe an adaptive resampling technique which reduces the amount of resampling to maintain a diverse particle set and to avoid particle depletion, a common problem of the particle filter.
More details on the first contribution: under the assumption that the optimal proposal is peaked (due to peaked observation model $p(z_t|x_t)$ with accurate sensors, as mentioned above), we should focus sampling around the maximum of the proposal. For gridmaps, the authors employ a scanmatching procedure to narrow down the position estimate given by the odometry. Concretely, the authors use hillclimbing to correct the odometry estimate extrapolated from $x_t^{[k]}$ until it best fits the laser observation $z_t$ and estimate the optimal proposal with a Gaussian around that maximum. The next $x_{t+1}^{[k]}$ is drawn from that Gaussian and the weight of this particle is updated according to Equation 19 in the paper.
Second, in order to avoid particle depletion, resampling is limited and only carried out if the dispersion of particles becomes too large. The criterion used to determine dispersion is the effective sample size, $N_{eff}$ which is defined as $$\frac{1}{\sum_{i=1}^M(\~w^{[i]})^2}$$ for normalized particle weights $\~w^{[i]}$. Experimental results and a discussion of the behavior of the algorithm with different datasets are provided in the paper. Good stuff, and it actually works! It's at the core of WillowGarage's navigation stack for the PR2!