Beam-based Sensor Models

Beam-based distance sensors (sonar, lidar, depth IR) measure distances to the nearest surface along discrete directions (beams). We develop a probabilistic forward (generative) model $p(\mathbf z_t \mid \mathbf x_t, m)$ describing the likelihood of receiving range scan $\mathbf z_t$ from pose $\mathbf x_t$ in map $m$, and an inverse model for occupancy mapping. Notation:

• Robot pose (planar): $\mathbf x_t = (x_t, y_t, \theta_t)$.
• Map $m$: occupancy grid (binary / probabilistic) or geometric (polygonal, mesh).
• Scan: $\mathbf z_t = { z_t^k }_{k=1}^K$ with beam angles $\phi_k$ relative to sensor frame.
• Maximum sensor range: $z_{\max}$.
• Expected (ideal) range along beam $k$: $\hat z_t^k = r(\mathbf x_t, \phi_k, m)$ found via ray casting.

Assumed conditional independence (approximation): $$p(\mathbf z_t \mid \mathbf x_t, m) = \prod_{k=1}^K p(z_t^k \mid \mathbf x_t, m).$$

​

Beam-Based Forward Model (Mixture)

Real measurements exhibit multiple phenomena: precise hits, unexpected short returns, max-range (no return), and random noise. Model each beam as a weighted mixture: $$p(z \mid \hat z) = \alpha_{\text{hit}} p_{\text{hit}}(z \mid \hat z) + \alpha_{\text{short}} p_{\text{short}}(z \mid \hat z) + \alpha_{\text{max}} p_{\text{max}}(z) + \alpha_{\text{rand}} p_{\text{rand}}(z),$$ with $\sum_i \alpha_i = 1$.

​

Precise Hit Component

$$p_{\text{hit}}(z \mid \hat z) = \begin{cases} \eta \exp\!\left(-\dfrac{(z - \hat z)^2}{2\sigma_{\text{hit}}^2}\right), & 0 \le z \le z_{\max}\\ 0, & \text{otherwise} \end{cases}$$ $\eta$ normalizes over $[0,z_{\max}]$.

​

Unexpected Short Return

Captures unmodelled obstacles between sensor and predicted surface: $$p_{\text{short}}(z \mid \hat z) = \begin{cases} \eta \lambda e^{-\lambda z}, & 0 \le z \le \hat z\\ 0, & z > \hat z \end{cases}$$

​

Max Range

$$p_{\text{max}}(z) = \begin{cases} 1, & z = z_{\max}\\ 0, & \text{otherwise} \end{cases}$$

​

Random Noise

$$p_{\text{rand}}(z) = \begin{cases} \dfrac{1}{z_{\max}}, & 0 \le z < z_{\max}\\ 0, & \text{otherwise} \end{cases}$$

​

Final Per-Beam Likelihood

$$p(z_t^k \mid \mathbf x_t, m) = p(z \mid \hat z_t^k).$$ Log form for numerical stability: $$\log p(\mathbf z_t \mid \mathbf x_t, m) = \sum_k \log\left( \sum_j \alpha_j p_j(z_t^k) \right).$$

​

Parameter Estimation

Given training set ${ (z_n, \hat z_n)}$:

• Closed-form for $\sigma_{\text{hit}}^2$ using weighted residual variance if component assignments known.
• Use EM:
  • E-step: responsibilities $r_{ni} = \dfrac{\alpha_i p_i(z_n)}{\sum_j \alpha_j p_j(z_n)}$.
  • M-step: $\alpha_i = \frac{1}{N}\sum_n r_{ni}$; update $\sigma_{\text{hit}}, \lambda$ via weighted MLE.

Constrain $\alpha_i \ge 0$, $\sum\alpha_i=1$.

​

Dynamic Obstacles

Augment forward model with dynamic layer $m_D$: $$p(z \mid \hat z, m, m_D) = (1-\beta) p(z \mid \hat z, m) + \beta p_{\text{dyn}}(z),$$ where $p_{\text{dyn}}$ could emphasize short / random returns; $\beta$ from motion segmentation.

---