Likelihood Field Model

The classic Beam Model needs ray casting into the map for every beam to find the predicted hit; that’s accurate but slow. The likelihood field replaces ray casting with a precomputed distance-to-obstacle field.

Essentially, from the occupancy grid $m$, compute for each grid the Euclidean distance to the nearest occupied cell:

$$\text{dist}(x,y)=\underset{(x^{\prime },y^{\prime }):\text{occ}(x^{\prime },y^{\prime })}{\min }\sqrt{(x-x^{\prime }{)}^2+(y-y^{\prime }{)}^2}.$$

• This is the distance transform; it can be computed once with fast algorithms (e.g., Felzenszwalb & Huttenlocher).

Convert that into a likelihood via a Gaussian centered at zero distance:

$$p_{\text{hit}}(z_k^i\mid {\xi }_k,m)\propto \exp (-\frac{\text{dist}(x_k^i,y_k^i{)}^2}{2{\sigma }_{\text{hit}}^2}),$$

where $(x_k^i,y_k^i)$ is the endpoint of the measured beam when the robot is at pose ${\xi }_k$.