Investigate beam sensor model

[nahueespinosa]

Description

Probabilistic Robotics, Chapter 6.3.1 proposes a sensor model closely linked to the geometry and physics of range finders.
It's also implemented in nav2_amcl.

Definition of Done

• A short presentation talking about the algorithm and/or a comprehensive description posted in this ticket.
• Review and understand nav2_amcl implementation.

[glpuga]

Off topic, but exploring correlation based approaches (Chapter 6.5) can link to a previous attempt we did to implement a very efficient multiscale scan matcher (M3RSM) for use in SLAM. I don't think we got very far into it, though. See https://github.com/ekumenlabs/RnD/issues/25

The same M3RSM algorithm is probably more performing than what #52 is using to propose new hypothesis (just based on the description, since the code is not available yet).

[frneer]

Beam Sensor Model Investigation

This sensor model is deeply linked to the geometry and the physics of range finders.

Our goal is to implement this model and the usage consists in applying this sensor model to the current belief updater of the robot, basically getting the currently estimated pose and updating it accordingly. For that, we need to get the probability of making a particular measurement at a given time given the current pose and our map: $p\left(z_t \mid x_t, m\right)$. For that, we may consider independence between beam measurements $z_t^k$, therefore:

$$p\left(z_t \mid x_t, m\right)=\prod_{k=1}^K p\left(z_t^k \mid x_t, m\right)$$

The model consists in four probabilistic distributions, which are glued together resulting in a weighted Mixture Distribution that represents the resulting sensor model.

$$p\left(z_t^k \mid x_t, m\right)=\left(\begin{array}{c} z_{\text {hit }} \\ z_{\text {short }} \\ z_{\text {max'}} \\ z_{\text {rand }} \end{array}\right)^T \cdot\left(\begin{array}{c}. p_{\text {hit }}\left(z_t^k \mid x_t, m\right) \\ p_{\text {short }}\left(z_t^k \mid x_t, m\right) \\ p_{\text {max}}\left(z_t^k \mid x_t, m\right) \\ p_{\text {rand }}\left(z_t^k \mid x_t, m\right) \end{array}\right)$$

Where:

• $z_t^k$: k beam measurement for time t of the sensor.
• $x_t$: current pose of the robot.
• $m$: the map information.
• $z_{hit} + z_{short} + z_{max'} + z_{rand} = 1$

It's possible to identify a source of error/noise in each of these densities:

1. $p_{hit}$: Represents the measurement of the nearest object plus measurement noise.
2. $p_{short}$: Represents errors due to unexpected objects
3. $p_{max}$: Represents errors due to failures to detect objects.
4. $p_{rand}$: Represents random unexplained noise.

Distribution descriptions

$p_{hit}$: Correct range with local measurement noise

This probability is represented by a gaussian distribution: $\mathcal{N} \left(z_t^{k*}, \sigma^2_{hit} \right)$ but limited to $z \in \left[0, z_{max}\right]$, being $z_{max}$ the maximum range of the sensor. Since it's limited we need a scaling factor for it to be a real distribution:

$$p_{\text {hit }}\left(z_t^k \mid x_t, m\right)= \begin{cases}\eta ~\mathcal{N}\left(z_t^k ; z_t^{k *}, \sigma_{\text {hit }}^2\right) & \text { if } 0 \leq z_t^k \leq z_{\max } \\ 0 & \text { otherwise }\end{cases}$$

Where:

$$\eta=\left(\int_0^{z_{\max }} \mathcal{N}\left(z_t^k ; z_t^{k *}, \sigma_{\mathrm{hit}}^2\right) d z_t^k\right)^{-1}$$

Intrinsic parameters: $\sigma_{\mathrm{hit}}^2$

$p_{short}$: Unexpected objects

This probability represents objects not taken into account in the map that may be present in the measurements. And it can be modeled as an exponential distribution: $exp\left(\lambda_{short}\right)$ limited to $z \in \left[0, z_t^{k *}\right]$, being $z_t^{k *}$ the true range in that particular direction.

$$p_{\text {short }}\left(z_t^k \mid x_t, m\right)= \begin{cases}\eta \lambda_{\text {short }} e^{-\lambda_{\text {short }} z_t^k} & \text { if } 0 \leq z_t^k \leq z_t^{k *} \\ 0 & \text { otherwise }\end{cases}$$

Where:

$$\eta=\frac{1}{1-e^{-\lambda_{\text {short }} z_t^{k *}}}$$

Intrinsic parameters: $\lambda_{\mathrm{short}}$

$p_{max}$: Failures

This probability represents the case where the sensor doesn't get a measurement at all, for example, this may happen with low-reflective surfaces. Therefore, it saturates to the max range, it's modeled as a point-mass distribution centered at $z_{max}$.

$$p_{\max }\left(z_t^k \mid x_t, m\right)=\mathbb{1}\left(z=z_{\max }\right)= \begin{cases}1 & \text { if } z=z_{\max } \\ 0 & \text { otherwise }\end{cases}$$

As I spoiled before since the total distribution is a mixture of the four distributions, this being a discrete distribution may seem odd, but it's very easy to represent this algorithmically.

$p_{rand}$: Random measurements

This probability represents unexplained measurements, for example, cross-talk generated. And this case is modeled as a uniform distribution across the range of the sensor: $\mathcal{U}\left(0, z_{max}\right)$:

$$p_{\text {rand }}\left(z_t^k \mid x_t, m\right)= \begin{cases}\frac{1}{z_{max }} & \text { if } 0 \leq z_{t^k} < z_{max} \\ 0 & \text { otherwise }\end{cases}$$

Mixture Distribution

So the final distribution looks something like this:

How to code it?

So for every set of measurements, the probability of it is calculated given the map and the current pose by applying this model.

Input variables for the model

All the intrinsic parameters should be input parameters: $z_{hit}$, $z_{short}$, $z_{max}$, $z_{rand}$, $\sigma_{\mathrm{hit}}^2$ and $\lambda_{\mathrm{short}}$. Also, the map $m$, the current pose $x_t$, and the measurements $z_t$. As well as the maximum range of the given sensor: $z_{max_range}$.

Distributions

It's a requirement to be able to generate each of these probabilistic distributions:

1. Gaussian distribution
2. Uniform distribution
3. Exponential distribution
4. Delta distribution

The first 3 can be defined via their density functions, the last one is as simple as an if statement.

Ray casting

A method that from a given sensor pose $x_{sensor_t}$ and the map $m$ computes the expected measured range $z_t^{k*}$ for every direction of the input laser measurements $z_t^k$, this direction can be calculated from the point cloud format of the measurements.

Bibliography

• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.