Probabilistic Motion and Measurement Models

Probabilistic filters need two models. A motion model says where the robot might be after a command, as a spread of possibilities. A measurement model says how likely each sensor reading is, given where the robot really is, including the ways real sensors fail.

Rotation noise from rotation Translation noise from translation True distance to the wall Chance of a short reading (obstacle)

The odometry motion model

An odometry step is split into a turn $\delta_1$, a straight move $\delta_t$, and a second turn $\delta_2$. Each is sampled with noise that grows with the motion:

$$\hat\delta_t = \delta_t + \varepsilon\!\left(\alpha_3\,\delta_t + \alpha_4(\lvert\delta_1\rvert + \lvert\delta_2\rvert)\right), \qquad \hat\delta_1 = \delta_1 + \varepsilon\!\left(\alpha_1\lvert\delta_1\rvert + \alpha_2\,\delta_t\right)$$

The beam measurement model

A laser reading $z$ given the true distance $z^*$ is a mix of four cases:

$$p(z \mid z^*) = w_{\text{hit}}\,\mathcal{N}(z; z^*, \sigma^2) + w_{\text{short}}\,\lambda e^{-\lambda z} + w_{\max}\,\mathbb{1}[z = z_{\max}] + w_{\text{rand}}\,\tfrac{1}{z_{\max}}$$

What to notice

After a turn and a drive, the possible poses form a curved banana, since heading noise swings the end point sideways. In the sensor model, the tall peak is a good hit near the true distance, the decaying curve on the left is readings cut short by people or objects in the way, and the spike at the right is the sensor returning its maximum when nothing reflects. Modeling these failure cases keeps a particle filter from trusting a single bad reading too much. Both models come from Thrun, Burgard, and Fox's Probabilistic Robotics.