In Chapter 8, Lessons 1–2, we established the MCL update: the particle set represents a belief \( bel(\mathbf{x}_t) \), and importance weights are assigned by the observation likelihood: \( w_t^{(n)} \propto p(\mathbf{z}_t \mid \mathbf{x}_t^{(n)}, \mathbf{m}) \). The core design question is how to specify \( p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) \) so that it is (i) physically meaningful, (ii) robust to outliers and dynamics, and (iii) computationally tractable.

Throughout this lesson, we use the conditional-independence factorization (an approximation that must be validated empirically):

\[ p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) \;\approx\; \prod_{i=1}^K p(z_t^i \mid \mathbf{x}_t, \mathbf{m}), \quad K \text{ beams/features}. \]

flowchart TD
  A["Particle pose x_n"] --> B["Predict measurement from map"]
  B --> C["LiDAR: endpoints -> distance-to-obstacle d"]
  B --> D["Vision: matched landmarks -> bearing residual e"]
  C --> E["Compute log p_lidar = sum log p_i"]
  D --> F["Compute log p_vision = sum log p_i"]
  E --> G["Fuse: logw = log p_lidar + log p_vision"]
  F --> G
  G --> H["Normalize weights (log-sum-exp)"]
  H --> I["Resample (Lesson 2)"]
        

The remainder of the lesson develops two families of sensor likelihoods: (A) LiDAR range-scan likelihoods (beam model and likelihood-field model), and (B) vision likelihoods based on landmark-bearing residuals with explicit outlier modeling.

Consider a 2D LiDAR providing ranges \( z^i \in [0, z_{\max}] \) at relative beam angles \( \phi_i \) (in the robot frame). Given a particle pose \( \mathbf{x}=(x,y,\theta) \) and a known map \( \mathbf{m} \), let \( z^{\ast i}=z^{\ast}(\mathbf{x},\phi_i,\mathbf{m}) \) denote the expected range from ray casting (intersection with the nearest mapped obstacle along that direction).

The classical beam model decomposes measurement causes into a mixture of components: (i) a correct hit near \( z^{\ast i} \), (ii) an unexpected short reading due to an unmodeled obstacle, (iii) a max-range failure reading, and (iv) a random measurement due to spurious returns. For one beam:

\[ p(z^i \mid \mathbf{x},\mathbf{m}) \;=\; w_{\text{hit} }\,p_{\text{hit} }(z^i \mid z^{\ast i}) + w_{\text{short} }\,p_{\text{short} }(z^i \mid z^{\ast i}) + w_{\text{max} }\,p_{\text{max} }(z^i) + \\ w_{\text{rand} }\,p_{\text{rand} }(z^i), \quad \sum_k w_k = 1,\; w_k \ge 0. \]

A standard instantiation is:

\[ p_{\text{hit} }(z \mid z^{\ast}) = \eta_{\text{hit} } \exp\!\left(-\frac{(z-z^{\ast})^2}{2\sigma_{\text{hit} }^2}\right) \mathbf{1}_{0 \le z \le z_{\max} }, \qquad \eta_{\text{hit} }^{-1} = \int_0^{z_{\max} } \exp\!\left(-\frac{(z-z^{\ast})^2}{2\sigma_{\text{hit} }^2}\right)\,dz. \]

\[ p_{\text{short} }(z \mid z^{\ast}) = \eta_{\text{short} }\, \lambda\,e^{-\lambda z} \mathbf{1}_{0 \le z \le z^{\ast} }, \qquad \eta_{\text{short} }^{-1} = \int_0^{z^{\ast} } \lambda e^{-\lambda z}\,dz = 1-e^{-\lambda z^{\ast} }. \]

\[ p_{\text{max} }(z)=\mathbf{1}_{z=z_{\max} }, \qquad p_{\text{rand} }(z)=\frac{1}{z_{\max} }\mathbf{1}_{0 \le z \le z_{\max} }. \]

Proof (valid probability density). Assume each component is a valid density/mass function on \([0,z_{\max}]\): \(\int p_{\text{hit} }=1\), \(\int p_{\text{short} }=1\), \(\int p_{\text{rand} }=1\), and \(p_{\text{max} }\) integrates to 1 as a point mass at \(z_{\max}\). Then, for the mixture:

\[ \int_0^{z_{\max} } p(z\mid\mathbf{x},\mathbf{m})\,dz = \sum_k w_k \int_0^{z_{\max} } p_k(z)\,dz = \sum_k w_k = 1. \]

Therefore the mixture is normalized and nonnegative. This proof generalizes immediately to mixtures that include additional components (e.g., a “no-return” region), provided each component is normalized.

The beam model can be accurate but computationally demanding because each particle requires ray casting to obtain \( z^{\ast i} \). A widely used alternative is the likelihood-field model, which replaces explicit ray casting with a precomputed distance transform on the occupancy grid.

Let the beam endpoint in the world frame be \( \mathbf{p}^i(\mathbf{x},z^i)=\big[x + z^i\cos(\theta+\phi_i),\; y + z^i\sin(\theta+\phi_i)\big]^\top \). Let \( d(\mathbf{p}) \) denote the Euclidean distance from point \(\mathbf{p}\) to the nearest obstacle in the map (obtained from a distance field).

A principled derivation starts by modeling the endpoint noise as isotropic Gaussian around the (unknown) true hit point on the obstacle boundary. If \(\mathbf{p}^\ast\) is the closest obstacle point and the measured endpoint is \(\mathbf{p} = \mathbf{p}^\ast + \boldsymbol{\epsilon}\), with \(\boldsymbol{\epsilon}\sim\mathcal{N}(\mathbf{0},\sigma^2\mathbf{I})\), then:

\[ p(\mathbf{p}\mid\mathbf{x},\mathbf{m}) \propto \exp\!\left(-\frac{\|\mathbf{p}-\mathbf{p}^\ast\|^2}{2\sigma^2}\right) = \exp\!\left(-\frac{d(\mathbf{p})^2}{2\sigma^2}\right), \]

where the last equality uses \( d(\mathbf{p})=\min_{\mathbf{q}\in\partial\mathbf{m} }\|\mathbf{p}-\mathbf{q}\| \), i.e., the distance to the closest obstacle boundary point. This yields the per-beam likelihood:

\[ p(z^i \mid \mathbf{x},\mathbf{m}) \approx w_{\text{hit} }\, \exp\!\left(-\frac{d(\mathbf{p}^i(\mathbf{x},z^i))^2}{2\sigma_{\text{hit} }^2}\right) + w_{\text{rand} }\,\frac{1}{z_{\max} }. \]

Remark (why the uniform term matters). Without the \(w_{\text{rand} }\) term, a single beam that lands in free space far from obstacles can force \(p(z^i\mid\mathbf{x},\mathbf{m})\) to be exponentially tiny, dominating the product and causing weight collapse in the presence of dynamic obstacles, glass, or missing map structure.

Proof sketch (MAP equivalence under log transform). For any particle pose, define the scan likelihood as a product. Since \(\log\) is strictly increasing:

\[ \arg\max_\mathbf{x} \prod_{i=1}^K p_i(\mathbf{x}) \;=\; \arg\max_\mathbf{x} \sum_{i=1}^K \log p_i(\mathbf{x}). \]

Thus log-likelihood computation preserves the maximum-likelihood pose while improving numerical stability.

Vision measurements can be incorporated into MCL when the map includes recognizable landmarks (e.g., fiducials, corners, keypoints with descriptors) with known world positions. This lesson uses a minimal bearing-only model to avoid requiring full camera calibration and depth estimation.

Suppose the measurement set is \(\mathbf{z}^V = \{(j_i, \beta_i)\}_{i=1}^M\), where \(j_i\) is the associated landmark ID (from feature matching) and \(\beta_i\) is the observed bearing in the robot frame. With landmark position \(\boldsymbol{\ell}_j=(\ell_{j,x},\ell_{j,y})\), the predicted bearing is:

\[ \beta_i^\ast(\mathbf{x},j_i) = \operatorname{wrap}\!\Big( \operatorname{atan2}(\ell_{j_i,y}-y,\;\ell_{j_i,x}-x) - \theta \Big), \qquad e_i = \operatorname{wrap}(\beta_i - \beta_i^\ast). \]

A Gaussian bearing noise model on the circle can be approximated (for small errors) by a wrapped Gaussian:

\[ p_{\text{in} }(\beta_i \mid \mathbf{x},j_i) \approx \frac{1}{\sqrt{2\pi\sigma_\beta^2} } \exp\!\left(-\frac{e_i^2}{2\sigma_\beta^2}\right). \]

Real feature matching has outliers (wrong associations, transient objects, motion blur). A robust likelihood mixes the inlier model with a uniform outlier model on \([ -\pi, \pi )\):

\[ p(\beta_i \mid \mathbf{x},j_i) = (1-\epsilon)\,p_{\text{in} }(\beta_i \mid \mathbf{x},j_i) + \epsilon\,\frac{1}{2\pi}, \quad 0 \le \epsilon \le 1. \]

Proof (normalization on the circle). Since \(p_{\text{in} }\) integrates to 1 over one period and the uniform term integrates to 1:

\[ \int_{-\pi}^\pi p(\beta\mid\mathbf{x},j)\,d\beta = (1-\epsilon)\cdot 1 + \epsilon\cdot 1 = 1. \]

Finally, assuming conditional independence across matched features:

\[ p(\mathbf{z}^V \mid \mathbf{x},\mathbf{m}) \approx \prod_{i=1}^M p(\beta_i \mid \mathbf{x},j_i). \]

Products of many small likelihoods frequently underflow in floating-point arithmetic. Therefore, implementations should accumulate log-likelihoods: \(\log w^{(n)} = \sum_i \log p_i\). Normalization can then be performed by the log-sum-exp identity:

\[ \operatorname{LSE}(\ell_1,\dots,\ell_N) = \log\left(\sum_{n=1}^N e^{\ell_n}\right) = m + \log\left(\sum_{n=1}^N e^{\ell_n-m}\right), \quad m = \max_n \ell_n. \]

The normalized weights are then:

\[ \bar{w}^{(n)} = \frac{e^{\ell_n} }{\sum_{j=1}^N e^{\ell_j} } = e^{\ell_n - \operatorname{LSE}(\ell_1,\dots,\ell_N)}. \]

Proposition (invariance to additive constants). For any constant \(c\), replacing all log-likelihoods by \(\ell_n' = \ell_n + c\) leaves normalized weights unchanged.

\[ \bar{w}'^{(n)} = \frac{e^{\ell_n+c} }{\sum_j e^{\ell_j+c} } = \frac{e^c e^{\ell_n} }{e^c \sum_j e^{\ell_j} } = \bar{w}^{(n)}. \]

Temperature scaling (controlled peakiness). If the observation model is “too peaked” (common with dense scans), use \(\alpha \in (0,1]\) and define \(\ell_n^{(\alpha)} = \alpha\,\ell_n\), i.e., \( p^{\alpha}(\mathbf{z}\mid\mathbf{x}) \). This preserves the maximum-likelihood particle (since scaling by \(\alpha>0\) is monotone) but increases effective sample size by reducing variance of weights.

The following implementations are likelihood evaluators that you can plug into an MCL loop (Lesson 2). In practice, you will: (i) build or load a map, (ii) precompute the distance field once, (iii) compute \(\log p(\mathbf{z}\mid\mathbf{x}^{(n)},\mathbf{m})\) per particle, and (iv) normalize and resample.

Typical integration points:
Python: ROS2 rclpy, sensor_msgs/LaserScan, NumPy/SciPy.
C++: ROS2 rclcpp, Eigen, nav_msgs/OccupancyGrid (or costmaps).
Java: OpenCV Java for feature extraction; ROS Java variants where available.
MATLAB/Simulink: Robotics System Toolbox (ROS/ROS2), Navigation Toolbox, Image Processing Toolbox (bwdist).
Wolfram: built-in array operations and distance transforms for prototyping likelihood functions.

Chapter8_Lesson3.py

#!/usr/bin/env python3
"""
Chapter8_Lesson3.py

Autonomous Mobile Robots (Control Engineering) - Chapter 8, Lesson 3
Sensor Likelihoods for LiDAR and Vision

This file provides:
1) A simple 2D occupancy grid and an approximate Euclidean distance transform
   (multi-source Dijkstra / brushfire).
2) A LiDAR likelihood-field sensor model (endpoint distance to nearest obstacle).
3) A bearing-only vision landmark likelihood with an explicit outlier mixture.
4) Numerically stable log-weight normalization (log-sum-exp).

Dependencies:
- Standard library only.
Optional accelerations (not required):
- numpy, scipy for faster distance transforms and vectorization.

Typical robotics integration:
- ROS/ROS2: subscribe to sensor_msgs/LaserScan, run the likelihood on each particle.
- Map source: nav_msgs/OccupancyGrid (2D) or a costmap.
"""

from __future__ import annotations
from dataclasses import dataclass
import heapq
import math
from typing import List, Tuple, Dict


# -----------------------------
# Geometry helpers
# -----------------------------
def wrap_to_pi(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    a = (a + math.pi) % (2.0 * math.pi)
    return a - math.pi


@dataclass
class Pose2D:
    x: float
    y: float
    theta: float  # radians


# -----------------------------
# Occupancy grid and distance field
# -----------------------------
@dataclass
class OccupancyGrid2D:
    """
    Binary occupancy grid.
    occ[y][x] = 1 for obstacle, 0 for free.

    Coordinate convention:
    - World: meters
    - Grid: integer indices (gx, gy)
    - origin: world coordinate of grid cell (0,0) corner
    """
    width: int
    height: int
    resolution: float
    origin_x: float
    origin_y: float
    occ: List[List[int]]  # shape [height][width], 0/1

    @staticmethod
    def empty(width: int, height: int, resolution: float, origin_x: float, origin_y: float) -> "OccupancyGrid2D":
        occ = [[0 for _ in range(width)] for _ in range(height)]
        return OccupancyGrid2D(width, height, resolution, origin_x, origin_y, occ)

    def in_bounds(self, gx: int, gy: int) -> bool:
        return 0 <= gx < self.width and 0 <= gy < self.height

    def set_obstacle(self, gx: int, gy: int) -> None:
        if self.in_bounds(gx, gy):
            self.occ[gy][gx] = 1

    def world_to_grid(self, x: float, y: float) -> Tuple[int, int]:
        gx = int(math.floor((x - self.origin_x) / self.resolution))
        gy = int(math.floor((y - self.origin_y) / self.resolution))
        return gx, gy


def distance_transform_brushfire(grid: OccupancyGrid2D) -> List[List[float]]:
    """
    Approximate Euclidean distance transform using multi-source Dijkstra on an 8-connected grid.

    Returns:
        dist[y][x] in meters, distance to nearest obstacle cell center (approx).
    """
    INF = 1e18
    dist = [[INF for _ in range(grid.width)] for _ in range(grid.height)]
    pq: List[Tuple[float, int, int]] = []

    # Initialize with all obstacle cells as sources (distance 0)
    for gy in range(grid.height):
        for gx in range(grid.width):
            if grid.occ[gy][gx] == 1:
                dist[gy][gx] = 0.0
                heapq.heappush(pq, (0.0, gx, gy))

    # 8-neighborhood: (dx, dy, step_cost)
    nbh = [
        (-1,  0, 1.0), (1,  0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
        (-1, -1, math.sqrt(2.0)), (-1, 1, math.sqrt(2.0)), (1, -1, math.sqrt(2.0)), (1, 1, math.sqrt(2.0))
    ]

    while pq:
        d, gx, gy = heapq.heappop(pq)
        if d > dist[gy][gx]:
            continue
        for dx, dy, c in nbh:
            nx, ny = gx + dx, gy + dy
            if not grid.in_bounds(nx, ny):
                continue
            nd = d + c
            if nd < dist[ny][nx]:
                dist[ny][nx] = nd
                heapq.heappush(pq, (nd, nx, ny))

    # Convert from cell-steps to meters
    for gy in range(grid.height):
        for gx in range(grid.width):
            dist[gy][gx] *= grid.resolution
    return dist


# -----------------------------
# LiDAR likelihood-field model
# -----------------------------
@dataclass
class LikelihoodFieldModel:
    grid: OccupancyGrid2D
    dist_field_m: List[List[float]]  # meters
    z_max: float
    sigma_hit: float
    w_hit: float
    w_rand: float
    max_dist: float = 2.0  # clamp distance for numerical robustness

    def endpoint_distance(self, wx: float, wy: float) -> float:
        gx, gy = self.grid.world_to_grid(wx, wy)
        if not self.grid.in_bounds(gx, gy):
            return self.max_dist
        return min(self.max_dist, self.dist_field_m[gy][gx])

    def log_likelihood(self, pose: Pose2D, ranges: List[float], rel_angles: List[float]) -> float:
        """
        Likelihood-field:
            p(z^i | x, m) ≈ w_hit * exp(-d^2/(2*sigma^2)) + w_rand*(1/z_max)
        """
        sig2 = self.sigma_hit * self.sigma_hit
        inv_z = 1.0 / self.z_max
        logp = 0.0

        for z, a_rel in zip(ranges, rel_angles):
            zc = min(max(z, 0.0), self.z_max)
            a = pose.theta + a_rel
            wx = pose.x + zc * math.cos(a)
            wy = pose.y + zc * math.sin(a)

            d = self.endpoint_distance(wx, wy)
            p_hit = math.exp(-(d * d) / (2.0 * sig2))
            p = self.w_hit * p_hit + self.w_rand * inv_z
            logp += math.log(max(p, 1e-12))
        return logp


# -----------------------------
# Vision bearing-only landmark likelihood
# -----------------------------
@dataclass
class VisionBearingModel:
    landmarks: Dict[int, Tuple[float, float]]  # id -> (x, y)
    sigma_bearing: float
    eps_outlier: float

    def log_likelihood(self, pose: Pose2D, obs: List[Tuple[int, float]]) -> float:
        """
        Each observation: (landmark_id, measured_bearing_in_robot_frame)
        Robust mixture:
            p = (1-eps)*N_wrap(e;0,sigma^2) + eps*(1/2pi)
        """
        sig2 = self.sigma_bearing * self.sigma_bearing
        norm = 1.0 / math.sqrt(2.0 * math.pi * sig2)
        uni = 1.0 / (2.0 * math.pi)

        logp = 0.0
        for lid, beta in obs:
            if lid not in self.landmarks:
                logp += math.log(uni)
                continue
            lx, ly = self.landmarks[lid]
            pred = wrap_to_pi(math.atan2(ly - pose.y, lx - pose.x) - pose.theta)
            innov = wrap_to_pi(beta - pred)

            p_in = norm * math.exp(-(innov * innov) / (2.0 * sig2))
            p = (1.0 - self.eps_outlier) * p_in + self.eps_outlier * uni
            logp += math.log(max(p, 1e-15))
        return logp


# -----------------------------
# Numerically stable normalization
# -----------------------------
def logsumexp(logw: List[float]) -> float:
    m = max(logw)
    s = sum(math.exp(w - m) for w in logw)
    return m + math.log(s)


def normalize_log_weights(logw: List[float]) -> List[float]:
    lse = logsumexp(logw)
    w = [math.exp(v - lse) for v in logw]
    s = sum(w)
    return [wi / s for wi in w]


# -----------------------------
# Minimal demo
# -----------------------------
def main() -> None:
    # Synthetic map: a wall and a pillar
    grid = OccupancyGrid2D.empty(width=120, height=120, resolution=0.05, origin_x=-3.0, origin_y=-3.0)

    # Wall: y = 0 (approx)
    for gx in range(10, 110):
        grid.set_obstacle(gx, 60)

    # Pillar block
    for gy in range(20, 30):
        for gx in range(85, 95):
            grid.set_obstacle(gx, gy)

    dist = distance_transform_brushfire(grid)

    lidar = LikelihoodFieldModel(
        grid=grid,
        dist_field_m=dist,
        z_max=8.0,
        sigma_hit=0.15,
        w_hit=0.95,
        w_rand=0.05,
        max_dist=2.0
    )

    vision = VisionBearingModel(
        landmarks={
            1: (-1.0,  0.0),
            2: ( 1.0,  0.5),
            3: ( 0.0, -1.0),
        },
        sigma_bearing=math.radians(3.0),
        eps_outlier=0.10
    )

    # Three particles
    particles = [
        Pose2D(-0.5, -0.8, math.radians(90.0)),
        Pose2D(-0.2, -0.6, math.radians(88.0)),
        Pose2D( 0.8,  0.1, math.radians(30.0)),
    ]

    # Synthetic scan: 9 beams centered around forward direction
    rel_angles = [math.radians(-40.0 + 80.0 * i / 8.0) for i in range(9)]
    ranges = [2.8, 3.0, 3.2, 3.1, 3.0, 3.2, 2.9, 2.7, 2.6]

    # Vision observations: (id, bearing)
    obs = [(1, math.radians(70.0)), (2, math.radians(20.0)), (3, math.radians(-95.0))]

    logw = []
    for p in particles:
        ll_lidar = lidar.log_likelihood(p, ranges, rel_angles)
        ll_vis = vision.log_likelihood(p, obs)
        logw.append(ll_lidar + ll_vis)

    w = normalize_log_weights(logw)

    print("log-weights:", [round(v, 3) for v in logw])
    print("weights:", [round(v, 6) for v in w])


if __name__ == "__main__":
    main()
      

Chapter8_Lesson3.cpp

/*
Chapter8_Lesson3.cpp

Autonomous Mobile Robots (Control Engineering) - Chapter 8, Lesson 3
Sensor Likelihoods for LiDAR and Vision

This file provides:
1) A 2D occupancy grid and approximate Euclidean distance transform (multi-source Dijkstra).
2) A LiDAR likelihood-field model.
3) A bearing-only vision landmark likelihood with outlier mixture.
4) Log-sum-exp normalization for particle weights.

Robotics ecosystem notes:
- ROS2 integration: subscribe to sensor_msgs::msg::LaserScan, build/update nav_msgs::msg::OccupancyGrid,
  and evaluate likelihoods per particle.
- Eigen can be used for vectorization; this example uses only the C++ standard library.

Build:
  g++ -O2 -std=c++17 Chapter8_Lesson3.cpp -o Chapter8_Lesson3
Run:
  ./Chapter8_Lesson3
*/

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
#include <queue>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

static inline double wrapToPi(double a) {
    double twoPi = 2.0 * M_PI;
    a = std::fmod(a + M_PI, twoPi);
    if (a < 0) a += twoPi;
    return a - M_PI;
}

struct Pose2D {
    double x{0}, y{0}, theta{0};
};

struct OccupancyGrid2D {
    int w{0}, h{0};
    double resolution{0.05};
    double origin_x{-3.0}, origin_y{-3.0};
    std::vector<uint8_t> occ; // 1 obstacle, 0 free; row-major [y*w + x]

    OccupancyGrid2D(int w_, int h_, double res, double ox, double oy)
        : w(w_), h(h_), resolution(res), origin_x(ox), origin_y(oy), occ(static_cast<size_t>(w_*h_), 0) {}

    inline bool inBounds(int gx, int gy) const {
        return (0 <= gx && gx < w && 0 <= gy && gy < h);
    }

    inline int idx(int gx, int gy) const { return gy * w + gx; }

    void setObstacle(int gx, int gy) {
        if (inBounds(gx, gy)) occ[static_cast<size_t>(idx(gx, gy))] = 1;
    }

    inline uint8_t get(int gx, int gy) const { return occ[static_cast<size_t>(idx(gx, gy))]; }

    std::pair<int,int> worldToGrid(double x, double y) const {
        int gx = static_cast<int>(std::floor((x - origin_x) / resolution));
        int gy = static_cast<int>(std::floor((y - origin_y) / resolution));
        return {gx, gy};
    }
};

std::vector<double> distanceTransformBrushfire(const OccupancyGrid2D& grid) {
    // Multi-source Dijkstra on 8-neighborhood, step costs 1 and sqrt(2), then scale by resolution.
    const double INF = std::numeric_limits<double>::infinity();
    std::vector<double> dist(static_cast<size_t>(grid.w * grid.h), INF);

    using Node = std::tuple<double,int,int>; // (d, gx, gy)
    struct Cmp {
        bool operator()(const Node& a, const Node& b) const {
            return std::get<0>(a) > std::get<0>(b); // min-heap by distance
        }
    };
    std::priority_queue<Node, std::vector<Node>, Cmp> pq;

    for (int gy = 0; gy < grid.h; ++gy) {
        for (int gx = 0; gx < grid.w; ++gx) {
            if (grid.get(gx, gy) == 1) {
                int id = grid.idx(gx, gy);
                dist[static_cast<size_t>(id)] = 0.0;
                pq.emplace(0.0, gx, gy);
            }
        }
    }

    const int nbh[8][2] = { {-1,0},{1,0},{0,-1},{0,1},{-1,-1},{-1,1},{1,-1},{1,1} };

    while (!pq.empty()) {
        auto [d, gx, gy] = pq.top();
        pq.pop();
        int id = grid.idx(gx, gy);
        if (d > dist[static_cast<size_t>(id)]) continue;

        for (auto &off : nbh) {
            int ngx = gx + off[0], ngy = gy + off[1];
            if (!grid.inBounds(ngx, ngy)) continue;
            double step = (off[0] == 0 || off[1] == 0) ? 1.0 : std::sqrt(2.0);
            double nd = d + step;
            int nid = grid.idx(ngx, ngy);
            if (nd < dist[static_cast<size_t>(nid)]) {
                dist[static_cast<size_t>(nid)] = nd;
                pq.emplace(nd, ngx, ngy);
            }
        }
    }

    for (auto &v : dist) v *= grid.resolution;
    return dist;
}

struct LikelihoodFieldModel {
    const OccupancyGrid2D& grid;
    const std::vector<double>& dist_m;
    double z_max{8.0};
    double sigma_hit{0.15};
    double w_hit{0.95};
    double w_rand{0.05};
    double max_dist{2.0};

    double endpointDistance(double wx, double wy) const {
        auto [gx, gy] = grid.worldToGrid(wx, wy);
        if (!grid.inBounds(gx, gy)) return max_dist;
        double d = dist_m[static_cast<size_t>(grid.idx(gx, gy))];
        return std::min(max_dist, d);
    }

    double logLikelihood(const Pose2D& pose,
                         const std::vector<double>& ranges,
                         const std::vector<double>& rel_angles) const {
        const double sig2 = sigma_hit * sigma_hit;
        const double invz = 1.0 / z_max;
        const size_t n = std::min(ranges.size(), rel_angles.size());
        double logp = 0.0;

        for (size_t i = 0; i < n; ++i) {
            double z = std::clamp(ranges[i], 0.0, z_max);
            double a = pose.theta + rel_angles[i];
            double wx = pose.x + z * std::cos(a);
            double wy = pose.y + z * std::sin(a);

            double d = endpointDistance(wx, wy);
            double p_hit = std::exp(-(d*d) / (2.0*sig2));
            double p = w_hit * p_hit + w_rand * invz;
            logp += std::log(std::max(p, 1e-12));
        }
        return logp;
    }
};

struct VisionBearingModel {
    std::unordered_map<int, std::pair<double,double>> landmarks; // id -> (x,y)
    double sigma_bearing{(3.0 * M_PI / 180.0)};
    double eps_outlier{0.10};

    double logLikelihood(const Pose2D& pose,
                         const std::vector<int>& ids,
                         const std::vector<double>& bearings) const {
        const double sig2 = sigma_bearing * sigma_bearing;
        const double norm = 1.0 / std::sqrt(2.0 * M_PI * sig2);
        const double uni  = 1.0 / (2.0 * M_PI);
        const size_t n = std::min(ids.size(), bearings.size());

        double logp = 0.0;
        for (size_t i = 0; i < n; ++i) {
            auto it = landmarks.find(ids[i]);
            if (it == landmarks.end()) {
                logp += std::log(uni);
                continue;
            }
            const auto [lx, ly] = it->second;
            double pred = wrapToPi(std::atan2(ly - pose.y, lx - pose.x) - pose.theta);
            double innov = wrapToPi(bearings[i] - pred);

            double p_in = norm * std::exp(-(innov*innov)/(2.0*sig2));
            double p = (1.0 - eps_outlier) * p_in + eps_outlier * uni;
            logp += std::log(std::max(p, 1e-15));
        }
        return logp;
    }
};

static inline double logSumExp(const std::vector<double>& logw) {
    double m = *std::max_element(logw.begin(), logw.end());
    double s = 0.0;
    for (double v : logw) s += std::exp(v - m);
    return m + std::log(s);
}

static inline std::vector<double> normalizeLogWeights(const std::vector<double>& logw) {
    double lse = logSumExp(logw);
    std::vector<double> w(logw.size());
    double sum = 0.0;
    for (size_t i = 0; i < logw.size(); ++i) {
        w[i] = std::exp(logw[i] - lse);
        sum += w[i];
    }
    for (auto &v : w) v /= sum;
    return w;
}

int main() {
    // Create a synthetic map: wall + pillar
    OccupancyGrid2D grid(120, 120, 0.05, -3.0, -3.0);
    for (int gx = 10; gx < 110; ++gx) grid.setObstacle(gx, 60);
    for (int gy = 20; gy < 30; ++gy)
        for (int gx = 85; gx < 95; ++gx)
            grid.setObstacle(gx, gy);

    auto dist = distanceTransformBrushfire(grid);

    LikelihoodFieldModel lidar{grid, dist, 8.0, 0.15, 0.95, 0.05, 2.0};

    VisionBearingModel vision;
    vision.landmarks = {
        {1, {-1.0,  0.0} },
        {2, { 1.0,  0.5} },
        {3, { 0.0, -1.0} }
    };
    vision.sigma_bearing = 3.0 * M_PI / 180.0;
    vision.eps_outlier = 0.10;

    std::vector<Pose2D> particles = {
        {-0.5, -0.8, 90.0 * M_PI / 180.0},
        {-0.2, -0.6, 88.0 * M_PI / 180.0},
        { 0.8,  0.1, 30.0 * M_PI / 180.0}
    };

    // Synthetic scan: 9 beams
    std::vector<double> rel_angles;
    rel_angles.reserve(9);
    for (int i = 0; i < 9; ++i) rel_angles.push_back((-40.0 + 80.0 * i / 8.0) * M_PI / 180.0);
    std::vector<double> ranges = {2.8, 3.0, 3.2, 3.1, 3.0, 3.2, 2.9, 2.7, 2.6};

    // Vision observations
    std::vector<int> ids = {1,2,3};
    std::vector<double> bears = {70.0 * M_PI / 180.0, 20.0 * M_PI / 180.0, -95.0 * M_PI / 180.0};

    std::vector<double> logw;
    logw.reserve(particles.size());
    for (const auto& p : particles) {
        double ll_l = lidar.logLikelihood(p, ranges, rel_angles);
        double ll_v = vision.logLikelihood(p, ids, bears);
        logw.push_back(ll_l + ll_v);
    }

    auto w = normalizeLogWeights(logw);

    std::cout << "log-weights: ";
    for (double v : logw) std::cout << v << " ";
    std::cout << "\nweights:     ";
    for (double v : w) std::cout << v << " ";
    std::cout << "\n";
    return 0;
}
      

Chapter8_Lesson3.java

/*
Chapter8_Lesson3.java

Autonomous Mobile Robots (Control Engineering) - Chapter 8, Lesson 3
Sensor Likelihoods for LiDAR and Vision

This file provides:
1) A 2D occupancy grid and approximate Euclidean distance transform (multi-source Dijkstra, 8-neighbors).
2) A LiDAR likelihood-field model.
3) A bearing-only vision landmark likelihood with an outlier mixture.
4) Stable log-weight normalization.

Robotics ecosystem notes:
- Java robotics stacks often use OpenCV Java bindings for vision, and (legacy) rosjava for ROS1.
- For ROS2, Java client libraries exist but are less common; the likelihood logic here is framework-agnostic.

Compile/Run:
  javac Chapter8_Lesson3.java
  java Chapter8_Lesson3
*/
import java.util.*;

public class Chapter8_Lesson3 {

    static class Pose2D {
        double x, y, theta;
        Pose2D(double x, double y, double theta) { this.x = x; this.y = y; this.theta = theta; }
    }

    static double wrapToPi(double a) {
        double twoPi = 2.0 * Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        return a - Math.PI;
    }

    static class OccupancyGrid2D {
        final int w, h;
        final double resolution;
        final double originX, originY;
        final byte[] occ; // 1 obstacle, 0 free

        OccupancyGrid2D(int w, int h, double resolution, double originX, double originY) {
            this.w = w; this.h = h;
            this.resolution = resolution;
            this.originX = originX; this.originY = originY;
            this.occ = new byte[w*h];
        }

        boolean inBounds(int gx, int gy) { return (0 <= gx && gx < w && 0 <= gy && gy < h); }
        int idx(int gx, int gy) { return gy*w + gx; }

        void setOcc(int gx, int gy, int v) { occ[idx(gx,gy)] = (byte)v; }
        int  getOcc(int gx, int gy) { return occ[idx(gx,gy)]; }

        int[] worldToGrid(double x, double y) {
            int gx = (int)Math.floor((x - originX) / resolution);
            int gy = (int)Math.floor((y - originY) / resolution);
            return new int[]{gx, gy};
        }
    }

    static float[] distanceTransformBrushfire(OccupancyGrid2D grid) {
        final int W = grid.w, H = grid.h;
        final float INF = Float.POSITIVE_INFINITY;
        float[] dist = new float[W*H];
        Arrays.fill(dist, INF);

        class Node {
            float d; int gx, gy;
            Node(float d, int gx, int gy) { this.d = d; this.gx = gx; this.gy = gy; }
        }

        PriorityQueue<Node> pq = new PriorityQueue<>(Comparator.comparingDouble(n -> n.d));

        for (int gy = 0; gy < H; gy++) {
            for (int gx = 0; gx < W; gx++) {
                if (grid.getOcc(gx, gy) != 0) {
                    int id = grid.idx(gx, gy);
                    dist[id] = 0.0f;
                    pq.add(new Node(0.0f, gx, gy));
                }
            }
        }

        int[][] nbh = new int[][]{
                {-1,0},{1,0},{0,-1},{0,1},
                {-1,-1},{-1,1},{1,-1},{1,1}
        };

        while (!pq.isEmpty()) {
            Node cur = pq.poll();
            int id = grid.idx(cur.gx, cur.gy);
            if (cur.d > dist[id]) continue;

            for (int[] dxy : nbh) {
                int ngx = cur.gx + dxy[0];
                int ngy = cur.gy + dxy[1];
                if (!grid.inBounds(ngx, ngy)) continue;
                float step = (dxy[0] == 0 || dxy[1] == 0) ? 1.0f : (float)Math.sqrt(2.0);
                float nd = cur.d + step;
                int jd = grid.idx(ngx, ngy);
                if (nd < dist[jd]) {
                    dist[jd] = nd;
                    pq.add(new Node(nd, ngx, ngy));
                }
            }
        }

        for (int i = 0; i < dist.length; i++) dist[i] *= (float)grid.resolution;
        return dist;
    }

    static class LikelihoodFieldModel {
        final OccupancyGrid2D grid;
        final float[] distFieldM;
        final double zMax, sigmaHit, wHit, wRand, maxDist;

        LikelihoodFieldModel(OccupancyGrid2D grid, float[] distFieldM,
                             double zMax, double sigmaHit,
                             double wHit, double wRand, double maxDist) {
            this.grid = grid; this.distFieldM = distFieldM;
            this.zMax = zMax; this.sigmaHit = sigmaHit;
            this.wHit = wHit; this.wRand = wRand; this.maxDist = maxDist;
        }

        double endpointDistance(double wx, double wy) {
            int[] g = grid.worldToGrid(wx, wy);
            int gx = g[0], gy = g[1];
            if (!grid.inBounds(gx, gy)) return maxDist;
            float d = distFieldM[grid.idx(gx, gy)];
            return Math.min(maxDist, (double)d);
        }

        double logLikelihood(Pose2D pose, double[] ranges, double[] relAngles) {
            int n = Math.min(ranges.length, relAngles.length);
            double invZ = 1.0 / zMax;
            double sig2 = sigmaHit * sigmaHit;
            double logp = 0.0;

            for (int i = 0; i < n; i++) {
                double z = Math.max(0.0, Math.min(ranges[i], zMax));
                double a = pose.theta + relAngles[i];
                double wx = pose.x + z * Math.cos(a);
                double wy = pose.y + z * Math.sin(a);

                double d = endpointDistance(wx, wy);
                double pHit = Math.exp(-(d*d) / (2.0*sig2));
                double p = wHit * pHit + wRand * invZ;
                logp += Math.log(Math.max(p, 1e-12));
            }
            return logp;
        }
    }

    static class VisionBearingModel {
        final Map<Integer, double[]> landmarks; // id -> [x,y]
        final double sigmaBearing, epsOutlier;

        VisionBearingModel(Map<Integer, double[]> landmarks, double sigmaBearing, double epsOutlier) {
            this.landmarks = landmarks;
            this.sigmaBearing = sigmaBearing;
            this.epsOutlier = epsOutlier;
        }

        double logLikelihood(Pose2D pose, int[] ids, double[] bearings) {
            int n = Math.min(ids.length, bearings.length);
            double sig2 = sigmaBearing * sigmaBearing;
            double norm = 1.0 / Math.sqrt(2.0 * Math.PI * sig2);
            double uni  = 1.0 / (2.0 * Math.PI);

            double logp = 0.0;
            for (int i = 0; i < n; i++) {
                int id = ids[i];
                double meas = bearings[i];
                double[] lm = landmarks.get(id);
                if (lm == null) {
                    logp += Math.log(uni);
                    continue;
                }
                double lx = lm[0], ly = lm[1];
                double pred = wrapToPi(Math.atan2(ly - pose.y, lx - pose.x) - pose.theta);
                double innov = wrapToPi(meas - pred);

                double pIn = norm * Math.exp(-(innov*innov) / (2.0*sig2));
                double p = (1.0 - epsOutlier) * pIn + epsOutlier * uni;
                logp += Math.log(Math.max(p, 1e-15));
            }
            return logp;
        }
    }

    static double[] normalizeLogWeights(double[] logw) {
        double m = logw[0];
        for (double v : logw) m = Math.max(m, v);
        double s = 0.0;
        for (double v : logw) s += Math.exp(v - m);
        double lse = m + Math.log(s);

        double[] w = new double[logw.length];
        double sumw = 0.0;
        for (int i = 0; i < logw.length; i++) {
            w[i] = Math.exp(logw[i] - lse);
            sumw += w[i];
        }
        for (int i = 0; i < w.length; i++) w[i] /= sumw;
        return w;
    }

    public static void main(String[] args) {
        // Create a synthetic map (wall + pillar)
        int W = 120, H = 120;
        OccupancyGrid2D grid = new OccupancyGrid2D(W, H, 0.05, -3.0, -3.0);
        for (int gx = 10; gx < 110; gx++) grid.setOcc(gx, 60, 1);
        for (int gy = 20; gy < 30; gy++)
            for (int gx = 85; gx < 95; gx++)
                grid.setOcc(gx, gy, 1);

        float[] dist = distanceTransformBrushfire(grid);
        LikelihoodFieldModel lidar = new LikelihoodFieldModel(grid, dist, 8.0, 0.15, 0.95, 0.05, 2.0);

        Map<Integer, double[]> landmarks = new HashMap<>();
        landmarks.put(1, new double[]{-1.0, 0.0});
        landmarks.put(2, new double[]{ 1.0, 0.5});
        landmarks.put(3, new double[]{ 0.0,-1.0});
        VisionBearingModel vision = new VisionBearingModel(landmarks, Math.toRadians(3.0), 0.10);

        Pose2D[] particles = new Pose2D[]{
                new Pose2D(-0.5, -0.8, Math.toRadians(90.0)),
                new Pose2D(-0.2, -0.6, Math.toRadians(88.0)),
                new Pose2D( 0.8,  0.1, Math.toRadians(30.0))
        };

        // Synthetic scan: 9 beams
        double[] relAngles = new double[9];
        for (int i = 0; i < 9; i++) relAngles[i] = Math.toRadians(-40.0 + 80.0 * i / 8.0);
        double[] ranges = new double[]{2.8, 3.0, 3.2, 3.1, 3.0, 3.2, 2.9, 2.7, 2.6};

        // Vision observations: (id, bearing)
        int[] ids = new int[]{1,2,3};
        double[] bearings = new double[]{Math.toRadians(70.0), Math.toRadians(20.0), Math.toRadians(-95.0)};

        double[] logw = new double[particles.length];
        for (int i = 0; i < particles.length; i++) {
            double llL = lidar.logLikelihood(particles[i], ranges, relAngles);
            double llV = vision.logLikelihood(particles[i], ids, bearings);
            logw[i] = llL + llV;
        }

        double[] w = normalizeLogWeights(logw);
        System.out.println("log-weights: " + Arrays.toString(logw));
        System.out.println("weights:     " + Arrays.toString(w));
    }
}
      

Chapter8_Lesson3.m

% Chapter8_Lesson3.m
%
% Autonomous Mobile Robots (Control Engineering) - Chapter 8, Lesson 3
% Sensor Likelihoods for LiDAR and Vision
%
% This script demonstrates:
% 1) LiDAR likelihood-field model using a precomputed distance transform.
% 2) Bearing-only vision landmark likelihood with an outlier mixture.
% 3) Numerically stable normalization of particle weights from log-likelihoods.
%
% Robotics toolbox notes:
% - MATLAB Robotics System Toolbox: occupancyMap / binaryOccupancyMap, rayIntersection, ROS/ROS2 I/O.
% - Navigation Toolbox provides localization/planning utilities.
% - bwdist (Image Processing Toolbox) gives an exact Euclidean distance transform on grids.
%
% To run:
%   Chapter8_Lesson3

clear; clc;

%% Map (binary occupancy)
W = 120; H = 120; res = 0.05;  % cells, meters/cell
origin = [-3.0, -3.0];         % world of cell(1,1) corner
occ = zeros(H, W, 'uint8');
occ(61, 11:110) = 1;           % horizontal wall (MATLAB indexing)
occ(21:30, 86:95) = 1;         % pillar block

dist_field = compute_distance_field(occ, res);  % meters

%% LiDAR likelihood-field parameters
z_max = 8.0;
sigma_hit = 0.15;
w_hit = 0.95;
w_rand = 0.05;
max_dist = 2.0;

%% Vision landmark map (bearing-only)
landmarks = [ ...
    1, -1.0,  0.0; ...
    2,  1.0,  0.5; ...
    3,  0.0, -1.0  ...
]; % [id, x, y]
sigma_bearing = deg2rad(3.0);
eps_outlier = 0.10;

%% Particles
particles = [ ...
   -0.5, -0.8, deg2rad(90.0); ...
   -0.2, -0.6, deg2rad(88.0); ...
    0.8,  0.1, deg2rad(30.0)  ...
];

%% Synthetic measurements
rel_angles = linspace(deg2rad(-40), deg2rad(40), 9);
ranges = [2.8, 3.0, 3.2, 3.1, 3.0, 3.2, 2.9, 2.7, 2.6];

obs_ids = [1, 2, 3];
obs_bear = deg2rad([70.0, 20.0, -95.0]);

%% Compute log-weights
logw = zeros(size(particles,1), 1);
for i = 1:size(particles,1)
    pose = particles(i,:);
    ll_lidar = lidar_loglikelihood_field(pose, ranges, rel_angles, dist_field, res, origin, ...
        z_max, sigma_hit, w_hit, w_rand, max_dist);
    ll_vis = vision_bearing_loglikelihood(pose, obs_ids, obs_bear, landmarks, sigma_bearing, eps_outlier);
    logw(i) = ll_lidar + ll_vis;
end

w = normalize_log_weights(logw);
disp('log-weights:'); disp(logw.');
disp('weights:'); disp(w.');

%% Optional: create a minimal Simulink model (programmatically)
% The model demonstrates a MATLAB Function block computing a scan log-likelihood.
% Uncomment to generate:
% create_simulink_demo_model();

%% -------------------- Local functions --------------------
function dist_field = compute_distance_field(occ, res)
% Compute distance to nearest obstacle cell (in meters).
% If bwdist exists, use it; else approximate with a brushfire transform.
    if exist('bwdist', 'file') == 2
        dist_cells = bwdist(occ > 0);      % distance to nearest nonzero (obstacle)
        dist_field = dist_cells * res;
    else
        dist_field = brushfire_distance(occ, res);
    end
end

function dist = brushfire_distance(occ, res)
% Approximate Euclidean distance using multi-source Dijkstra on 8-neighbors.
    [H,W] = size(occ);
    INF = 1e9;
    dist = INF * ones(H,W);
    % priority queue emulation (inefficient but dependency-free)
    Q = [];
    [ys,xs] = find(occ > 0);
    for k = 1:numel(xs)
        dist(ys(k), xs(k)) = 0;
        Q(end+1,:) = [0, ys(k), xs(k)]; %#ok<AGROW>
    end
    nbh = [ -1 0 1.0; 1 0 1.0; 0 -1 1.0; 0 1 1.0; ...
            -1 -1 sqrt(2); -1 1 sqrt(2); 1 -1 sqrt(2); 1 1 sqrt(2) ];

    while ~isempty(Q)
        % extract min
        [~,ii] = min(Q(:,1));
        cur = Q(ii,:); Q(ii,:) = [];
        d = cur(1); y = cur(2); x = cur(3);
        if d > dist(y,x), continue; end
        for j = 1:size(nbh,1)
            yy = y + nbh(j,1); xx = x + nbh(j,2); c = nbh(j,3);
            if yy < 1 || yy > H || xx < 1 || xx > W, continue; end
            nd = d + c;
            if nd < dist(yy,xx)
                dist(yy,xx) = nd;
                Q(end+1,:) = [nd, yy, xx]; %#ok<AGROW>
            end
        end
    end
    dist = dist * res;
end

function ll = lidar_loglikelihood_field(pose, ranges, rel_angles, dist_field, res, origin, ...
    z_max, sigma_hit, w_hit, w_rand, max_dist)
% LiDAR likelihood field model (robust):
%   p = w_hit*exp(-d^2/(2*sigma^2)) + w_rand*(1/z_max)
%   ll = sum log(p)
    x = pose(1); y = pose(2); th = pose(3);
    sig2 = sigma_hit^2;
    inv_z = 1.0 / z_max;
    ll = 0.0;

    for k = 1:numel(ranges)
        z = min(max(ranges(k), 0.0), z_max);
        a = th + rel_angles(k);
        wx = x + z*cos(a); wy = y + z*sin(a);

        [gx,gy,inside] = world_to_grid(wx, wy, res, origin, size(dist_field));
        if inside
            d = min(max_dist, dist_field(gy,gx));
        else
            d = max_dist;
        end

        p_hit = exp(-(d*d)/(2.0*sig2));
        p = w_hit*p_hit + w_rand*inv_z;
        ll = ll + log(max(p, 1e-12));
    end
end

function ll = vision_bearing_loglikelihood(pose, ids, bearings, landmarks, sigma_bearing, eps_outlier)
% Bearing-only landmark likelihood with outlier mixture.
    x = pose(1); y = pose(2); th = pose(3);
    sig2 = sigma_bearing^2;
    normc = 1.0 / sqrt(2*pi*sig2);
    uni = 1.0 / (2*pi);
    ll = 0.0;

    for i = 1:numel(ids)
        id = ids(i);
        idx = find(landmarks(:,1) == id, 1);
        if isempty(idx)
            ll = ll + log(uni);
            continue;
        end
        lx = landmarks(idx,2); ly = landmarks(idx,3);
        pred = wrap_to_pi(atan2(ly - y, lx - x) - th);
        innov = wrap_to_pi(bearings(i) - pred);

        p_in = normc * exp(-(innov*innov)/(2.0*sig2));
        p = (1-eps_outlier)*p_in + eps_outlier*uni;
        ll = ll + log(max(p, 1e-15));
    end
end

function a = wrap_to_pi(a)
    a = mod(a + pi, 2*pi) - pi;
end

function [gx,gy,inside] = world_to_grid(wx, wy, res, origin, sz)
    gx = floor((wx - origin(1))/res) + 1;
    gy = floor((wy - origin(2))/res) + 1;
    inside = (gx >= 1 && gx <= sz(2) && gy >= 1 && gy <= sz(1));
end

function w = normalize_log_weights(logw)
% Stable normalization via log-sum-exp.
    m = max(logw);
    s = sum(exp(logw - m));
    lse = m + log(s);
    w = exp(logw - lse);
    w = w / sum(w);
end

function create_simulink_demo_model()
% Programmatically create a minimal Simulink model with a MATLAB Function block.
% Requires Simulink.
    mdl = 'Chapter8_Lesson3_Simulink';
    if bdIsLoaded(mdl), close_system(mdl, 0); end
    new_system(mdl); open_system(mdl);

    add_block('simulink/Sources/Constant', [mdl '/pose'], 'Value', '[0;0;0]');
    add_block('simulink/Sources/Constant', [mdl '/ranges'], 'Value', '[1;1;1]');
    add_block('simulink/Sources/Constant', [mdl '/angles'], 'Value', '[-0.5;0;0.5]');
    add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/loglikelihood']);

    set_param([mdl '/loglikelihood'], 'Script', ...
        [ 'function ll = f(pose,ranges,angles)\n' ...
          '% Replace with a call to lidar_loglikelihood_field using base-workspace dist_field\n' ...
          'll = 0;\n' ...
          'end\n' ]);

    add_block('simulink/Sinks/Display', [mdl '/Display']);

    add_line(mdl, 'pose/1', 'loglikelihood/1');
    add_line(mdl, 'ranges/1', 'loglikelihood/2');
    add_line(mdl, 'angles/1', 'loglikelihood/3');
    add_line(mdl, 'loglikelihood/1', 'Display/1');

    save_system(mdl);
end
      

Chapter8_Lesson3.nb

Notebook[{
 Cell["Chapter 8, Lesson 3 — Sensor Likelihoods for LiDAR and Vision", "Title"],
 Cell["Chapter8_Lesson3.nb", "Subtitle"],
 Cell["This notebook demonstrates a likelihood-field LiDAR model and a bearing-only vision landmark model, plus log-sum-exp normalization.", "Text"],

 Cell[BoxData[
  RowBox[{"(*", " ", "Map: binary occupancy grid (1=obstacle, 0=free)", " ", "*)"}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"W", "=", "120"}], ";",
   RowBox[{"H", "=", "120"}], ";",
   RowBox[{"res", "=", "0.05"}], ";",
   RowBox[{"origin", "=", RowBox[{"{", RowBox[{"-3.0", ",", "-3.0"}], "}"}]}], ";",
   "\n",
   RowBox[{"occ", "=", RowBox[{"ConstantArray", "[", RowBox[{"0", ",", RowBox[{"{", RowBox[{"H", ",", "W"}], "}"}]}], "]"}]}], ";",
   "\n",
   RowBox[{"(* wall *)"}],
   RowBox[{"Do", "[", RowBox[{RowBox[{RowBox[{"occ", "[", RowBox[{"[", RowBox[{"61", ",", "gx"}], "]"}], "]"}], "=", "1"}], ",",
     RowBox[{"{", RowBox[{"gx", ",", "11", ",", "110"}], "}"}]}], "]"}], ";",
   "\n",
   RowBox[{"(* pillar *)"}],
   RowBox[{"Do", "[",
     RowBox[{RowBox[{RowBox[{"occ", "[", RowBox[{"[", RowBox[{"gy", ",", "gx"}], "]"}], "]"}], "=", "1"}], ",",
      RowBox[{"{", RowBox[{"gy", ",", "21", ",", "30"}], "}"}], ",",
      RowBox[{"{", RowBox[{"gx", ",", "86", ",", "95"}], "}"}]}], "]"}], ";"
  }]], "Input"],

 Cell["Distance transform (requires DistanceTransform in your Mathematica version).", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"img", "=", RowBox[{"Image", "[", RowBox[{"occ", ",", "\"Bit\""}], "]"}]}], ";",
   "\n",
   RowBox[{"distImg", "=", RowBox[{"DistanceTransform", "[", "img", "]"}]}], ";",
   "\n",
   RowBox[{"dist", "=", RowBox[{"ImageData", "[", "distImg", "]"}]}], ";",
   "\n",
   RowBox[{"distM", "=", RowBox[{"dist", "*", "res"}]}], ";"
  }]], "Input"],

 Cell["Utility functions.", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"wrapToPi", "[", "a_", "]"}], ":=",
   RowBox[{"Module", "[", RowBox[{RowBox[{"{", "b", "}"}], ",",
     RowBox[{
      RowBox[{"b", "=", RowBox[{"Mod", "[", RowBox[{RowBox[{"a", "+", "Pi"}], ",", RowBox[{"2", "Pi"}]}], "]"}]}], ";",
      RowBox[{"b", "-", "Pi"}]}]}], "]"}]}]], "Input"],

 Cell[BoxData[
  RowBox[{
   RowBox[{"logSumExp", "[", "v_List", "]"}], ":=",
   RowBox[{"Module", "[", RowBox[{RowBox[{"{", RowBox[{"m", ",", "s"}], "}"}], ",",
     RowBox[{
      RowBox[{"m", "=", RowBox[{"Max", "[", "v", "]"}]}], ";",
      RowBox[{"s", "=", RowBox[{"Total", "[", RowBox[{"Exp", "[", RowBox[{"v", "-", "m"}], "]"}], "]"}]}], ";",
      RowBox[{"m", "+", RowBox[{"Log", "[", "s", "]"}]}]}]}], "]"}]}]], "Input"],

 Cell["LiDAR likelihood field log-likelihood.", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"lidarLogLikelihood", "[",
    RowBox[{"pose_", ",", "ranges_", ",", "angles_", ",", "zMax_", ",", "sigmaHit_", ",", "wHit_", ",", "wRand_", ",", "maxDist_"}], "]"}], ":=",
   RowBox[{"Module", "[", RowBox[{
     RowBox[{"{", RowBox[{"x", ",", "y", ",", "th", ",", "sig2", ",", "invZ", ",", "ll", ",", "n"}], "}"}], ",",
     RowBox[{
      RowBox[{"x", "=", RowBox[{"pose", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
      RowBox[{"y", "=", RowBox[{"pose", "[", RowBox[{"[", "2", "]"}], "]"}]}], ";",
      RowBox[{"th", "=", RowBox[{"pose", "[", RowBox[{"[", "3", "]"}], "]"}]}], ";",
      RowBox[{"sig2", "=", RowBox[{"sigmaHit", "^", "2"}]}], ";",
      RowBox[{"invZ", "=", RowBox[{"1.0", "/", "zMax"}]}], ";",
      RowBox[{"ll", "=", "0.0"}], ";",
      RowBox[{"n", "=", RowBox[{"Min", "[", RowBox[{RowBox[{"Length", "[", "ranges", "]"}], ",", RowBox[{"Length", "[", "angles", "]"}]}], "]"}]}], ";",
      "\n",
      RowBox[{"Do", "[",
       RowBox[{
        RowBox[{"Module", "[", RowBox[{
          RowBox[{"{", RowBox[{"z", ",", "a", ",", "wx", ",", "wy", ",", "gx", ",", "gy", ",", "inside", ",", "d", ",", "pHit", ",", "p"}], "}"}], ",",
          RowBox[{
           RowBox[{"z", "=", RowBox[{"Clip", "[", RowBox[{RowBox[{"ranges", "[", RowBox[{"[", "k", "]"}], "]"}], ",", RowBox[{"{", RowBox[{"0.0", ",", "zMax"}], "}"}]}], "]"}]}], ";",
           RowBox[{"a", "=", RowBox[{"th", "+", RowBox[{"angles", "[", RowBox[{"[", "k", "]"}], "]"}]}]}], ";",
           RowBox[{"wx", "=", RowBox[{"x", "+", RowBox[{"z", "*", RowBox[{"Cos", "[", "a", "]"}]}]}]}], ";",
           RowBox[{"wy", "=", RowBox[{"y", "+", RowBox[{"z", "*", RowBox[{"Sin", "[", "a", "]"}]}]}]}], ";",
           RowBox[{"gx", "=", RowBox[{"Floor", "[", RowBox[{RowBox[{"(", RowBox[{"wx", "-", RowBox[{"origin", "[", RowBox[{"[", "1", "]"}], "]"}]}], ")"}], "/", "res"}], "]"}], "+", "1"}]}], ";",
           RowBox[{"gy", "=", RowBox[{"Floor", "[", RowBox[{RowBox[{"(", RowBox[{"wy", "-", RowBox[{"origin", "[", RowBox[{"[", "2", "]"}], "]"}]}], ")"}], "/", "res"}], "]"}], "+", "1"}]}], ";",
           RowBox[{"inside", "=", RowBox[{"And", "[", RowBox[{RowBox[{"1", "<=", "gx", "<=", "W"}], ",", RowBox[{"1", "<=", "gy", "<=", "H"}]}], "]"}]}], ";",
           RowBox[{"d", "=", RowBox[{"If", "[", RowBox[{"inside", ",", RowBox[{"Min", "[", RowBox[{"maxDist", ",", RowBox[{"distM", "[", RowBox[{"[", RowBox[{"gy", ",", "gx"}], "]"}], "]"}]}], "]"}], ",", "maxDist"}], "]"}]}], ";",
           RowBox[{"pHit", "=", RowBox[{"Exp", "[", RowBox[{RowBox[{"-", RowBox[{"d", "^", "2"}]}], "/", RowBox[{"(", RowBox[{"2.0", "*", "sig2"}], ")"}]}], "]"}]}], ";",
           RowBox[{"p", "=", RowBox[{"wHit", "*", "pHit", "+", RowBox[{"wRand", "*", "invZ"}]}]}], ";",
           RowBox[{"ll", "=", RowBox[{"ll", "+", RowBox[{"Log", "[", RowBox[{"Max", "[", RowBox[{"p", ",", "1*^-12"}], "]"}], "]"}]}]}], ";"
          }]}], "]"}], ",",
        RowBox[{"{", RowBox[{"k", ",", "1", ",", "n"}], "}"}]}], "]"}], ";",
      "ll"
     }]}], "]"}]}]], "Input"],

 Cell["Vision bearing-only landmark likelihood.", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"visionLogLikelihood", "[", RowBox[{"pose_", ",", "obs_", ",", "landmarks_Association", ",", "sigma_", ",", "eps_"}], "]"}], ":=",
   RowBox[{"Module", "[", RowBox[{
     RowBox[{"{", RowBox[{"x", ",", "y", ",", "th", ",", "sig2", ",", "norm", ",", "uni", ",", "ll"}], "}"}], ",",
     RowBox[{
      RowBox[{"x", "=", RowBox[{"pose", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
      RowBox[{"y", "=", RowBox[{"pose", "[", RowBox[{"[", "2", "]"}], "]"}]}], ";",
      RowBox[{"th", "=", RowBox[{"pose", "[", RowBox[{"[", "3", "]"}], "]"}]}], ";",
      RowBox[{"sig2", "=", RowBox[{"sigma", "^", "2"}]}], ";",
      RowBox[{"norm", "=", RowBox[{"1.0", "/", RowBox[{"Sqrt", "[", RowBox[{"2.0", "*", "Pi", "*", "sig2"}], "]"}]}]}], ";",
      RowBox[{"uni", "=", RowBox[{"1.0", "/", RowBox[{"(", RowBox[{"2.0", "*", "Pi"}], ")"}]}]}], ";",
      RowBox[{"ll", "=", "0.0"}], ";",
      "\n",
      RowBox[{"Do", "[",
       RowBox[{
        RowBox[{"Module", "[", RowBox[{
          RowBox[{"{", RowBox[{"id", ",", "bearingMeas", ",", "lm", ",", "lx", ",", "ly", ",", "pred", ",", "innov", ",", "pIn", ",", "p"}], "}"}], ",",
          RowBox[{
           RowBox[{"id", "=", RowBox[{"obs", "[", RowBox[{"[", RowBox[{"k", ",", "1"}], "]"}], "]"}]}], ";",
           RowBox[{"bearingMeas", "=", RowBox[{"obs", "[", RowBox[{"[", RowBox[{"k", ",", "2"}], "]"}], "]"}]}], ";",
           RowBox[{"If", "[", RowBox[{RowBox[{"KeyExistsQ", "[", RowBox[{"landmarks", ",", "id"}], "]"}], ",",
             RowBox[{
              RowBox[{"lm", "=", RowBox[{"landmarks", "[", "id", "]"}]}], ";",
              RowBox[{"lx", "=", RowBox[{"lm", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
              RowBox[{"ly", "=", RowBox[{"lm", "[", RowBox[{"[", "2", "]"}], "]"}]}], ";",
              RowBox[{"pred", "=", RowBox[{"wrapToPi", "[", RowBox[{RowBox[{"ArcTan", "[", RowBox[{RowBox[{"lx", "-", "x"}], ",", RowBox[{"ly", "-", "y"}]}], "]"}], "-", "th"}], "]"}]}], ";",
              RowBox[{"innov", "=", RowBox[{"wrapToPi", "[", RowBox[{"bearingMeas", "-", "pred"}], "]"}]}], ";",
              RowBox[{"pIn", "=", RowBox[{"norm", "*", RowBox[{"Exp", "[", RowBox[{RowBox[{"-", RowBox[{"innov", "^", "2"}]}], "/", RowBox[{"(", RowBox[{"2.0", "*", "sig2"}], ")"}]}], "]"}]}]}], ";",
              RowBox[{"p", "=", RowBox[{RowBox[{"(", RowBox[{"1.0", "-", "eps"}], ")"}], "*", "pIn", "+", RowBox[{"eps", "*", "uni"}]}]}], ";"
             }],
             RowBox[{"p", "=", "uni"}]
            ]"}], "]"}], ";",
           RowBox[{"ll", "=", RowBox[{"ll", "+", RowBox[{"Log", "[", RowBox[{"Max", "[", RowBox[{"p", ",", "1*^-15"}], "]"}], "]"}]}]}], ";"
          }]}], "]"}], ",",
        RowBox[{"{", RowBox[{"k", ",", "1", ",", RowBox[{"Length", "[", "obs", "]"}]}], "}"}]}], "]"}], ";",
      "ll"
     }]}], "]"}]}]], "Input"],

 Cell["Demo: compute weights for three particles.", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"particles", "=", RowBox[{"{",
    RowBox[{
     RowBox[{"{", RowBox[{"-0.5", ",", "-0.8", ",", RowBox[{"90", "Degree"}]}], "}"}], ",",
     RowBox[{"{", RowBox[{"-0.2", ",", "-0.6", ",", RowBox[{"88", "Degree"}]}], "}"}], ",",
     RowBox[{"{", RowBox[{"0.8", ",", "0.1", ",", RowBox[{"30", "Degree"}]}], "}"}]
    }], "}"}]}], ";",
   "\n",
   RowBox[{"ranges", "=", RowBox[{"{", RowBox[{"2.8", ",", "3.0", ",", "3.2", ",", "3.1", ",", "3.0", ",", "3.2", ",", "2.9", ",", "2.7", ",", "2.6"}], "}"}]}], ";",
   "\n",
   RowBox[{"angles", "=", RowBox[{"Table", "[", RowBox[{RowBox[{"(-40", "+", RowBox[{"80", "*", RowBox[{"(", RowBox[{"i", "/", "8"}], ")"}]}]}], ")"}], "Degree"}], ",", RowBox[{"{", RowBox[{"i", ",", "0", ",", "8"}], "}"}]}], "]"}]}], ";",
   "\n",
   RowBox[{"landmarks", "=", RowBox[{"<|", RowBox[{"1", "->", RowBox[{"{", RowBox[{"-1.0", ",", "0.0"}], "}"}], ",", "2", "->", RowBox[{"{", RowBox[{"1.0", ",", "0.5"}], "}"}], ",", "3", "->", RowBox[{"{", RowBox[{"0.0", ",", RowBox[{"-1.0"}]}], "}"}]}], "|>"}]}], ";",
   "\n",
   RowBox[{"obs", "=", RowBox[{"{", RowBox[{
     RowBox[{"{", RowBox[{"1", ",", RowBox[{"70", "Degree"}]}], "}"}], ",",
     RowBox[{"{", RowBox[{"2", ",", RowBox[{"20", "Degree"}]}], "}"}], ",",
     RowBox[{"{", RowBox[{"3", ",", RowBox[{"-95", "Degree"}]}], "}"}]
   }], "}"}]}], ";",
   "\n",
   RowBox[{"zMax", "=", "8.0"}], ";",
   RowBox[{"sigmaHit", "=", "0.15"}], ";",
   RowBox[{"wHit", "=", "0.95"}], ";",
   RowBox[{"wRand", "=", "0.05"}], ";",
   RowBox[{"maxDist", "=", "2.0"}], ";",
   RowBox[{"sigmaBear", "=", RowBox[{"3", "Degree"}]}], ";",
   RowBox[{"eps", "=", "0.10"}], ";",
   "\n",
   RowBox[{"logw", "=", RowBox[{"Table", "[",
     RowBox[{
      RowBox[{
       RowBox[{"lidarLogLikelihood", "[", RowBox[{RowBox[{"particles", "[", RowBox[{"[", "i", "]"}], "]"}], ",", "ranges", ",", "angles", ",", "zMax", ",", "sigmaHit", ",", "wHit", ",", "wRand", ",", "maxDist"}], "]"}], "+",
       RowBox[{"visionLogLikelihood", "[", RowBox[{RowBox[{"particles", "[", RowBox[{"[", "i", "]"}], "]"}], ",", "obs", ",", "landmarks", ",", "sigmaBear", ",", "eps"}], "]"}]
      }],
      ",", RowBox[{"{", RowBox[{"i", ",", "1", ",", RowBox[{"Length", "[", "particles", "]"}]}], "}"}]}], "]"}]}], ";",
   "\n",
   RowBox[{"lse", "=", RowBox[{"logSumExp", "[", "logw", "]"}]}], ";",
   RowBox[{"w", "=", RowBox[{"Exp", "[", RowBox[{"logw", "-", "lse"}], "]"}]}], ";",
   RowBox[{"w", "=", RowBox[{"w", "/", RowBox[{"Total", "[", "w", "]"}]}]}], ";",
   "\n",
   RowBox[{"logw", "\n", "w"}]
  }]], "Input"]
},
WindowSize -> {1000, 700},
StyleDefinitions -> "Default.nb"
]
      

Problem 1 (Truncated-Gaussian normalization for the hit model): Let \(p_{\text{hit} }(z\mid z^\ast) = \eta\exp(-(z-z^\ast)^2/(2\sigma^2))\mathbf{1}_{0 \le z \le z_{\max} }\). Derive an explicit expression for \(\eta\) in terms of the standard normal CDF \(\Phi\).

Solution: Substitute \(u=(z-z^\ast)/\sigma\), so \(dz = \sigma\,du\):

\[ \eta^{-1} = \int_0^{z_{\max} } \exp\!\left(-\frac{(z-z^\ast)^2}{2\sigma^2}\right)dz = \sigma \int_{(0-z^\ast)/\sigma}^{(z_{\max}-z^\ast)/\sigma} \exp\!\left(-\frac{u^2}{2}\right)du = \\ \sigma\sqrt{2\pi} \left[ \Phi\!\left(\frac{z_{\max}-z^\ast}{\sigma}\right) - \Phi\!\left(\frac{-z^\ast}{\sigma}\right) \right]. \]

Therefore \( \eta = \big(\sigma\sqrt{2\pi}(\Phi((z_{\max}-z^\ast)/\sigma)-\Phi(-z^\ast/\sigma))\big)^{-1} \).

Problem 2 (Beam-mixture decision structure): For a single range measurement \(z\), define a practical rule-based evaluation order for the mixture: detect \(z=z_{\max}\) (max-return) and otherwise evaluate the continuous components. Provide a flow consistent with the mixture model.

Solution: One consistent evaluation flow is shown below (the final probability is always the full mixture; the flow only optimizes computation).

flowchart TD
  Z["Beam measurement z"] --> R["Is z == z_max?"]
  R -->|yes| MAX["Use p_max \n(plus p_rand if modeled)"]
  R -->|no| S["Is z less than z_star?"]
  S -->|yes| SHORT["Evaluate p_short, p_hit, p_rand"]
  S -->|no| HIT["Evaluate p_hit, p_rand"]
  SHORT --> MIX["Compute mixture p = sum_k w_k p_k"]
  HIT --> MIX
  MAX --> MIX
        

Problem 3 (Deriving the likelihood-field exponential): Assume the nearest obstacle point to the measured endpoint \(\mathbf{p}\) is \(\mathbf{p}^\ast\). If \(\mathbf{p}=\mathbf{p}^\ast+\boldsymbol{\epsilon}\) with \(\boldsymbol{\epsilon}\sim\mathcal{N}(\mathbf{0},\sigma^2\mathbf{I})\), show that \(p(\mathbf{p}\mid\mathbf{x},\mathbf{m}) \propto \exp(-d(\mathbf{p})^2/(2\sigma^2))\).

Solution: The Gaussian density is:

\[ p(\mathbf{p}\mid\mathbf{p}^\ast) = \frac{1}{2\pi\sigma^2} \exp\!\left(-\frac{\|\mathbf{p}-\mathbf{p}^\ast\|^2}{2\sigma^2}\right). \]

If the obstacle boundary point is unknown, a Laplace (max) approximation uses the closest feasible boundary point: \(\mathbf{p}^\ast = \arg\min_{\mathbf{q}\in\partial\mathbf{m} }\|\mathbf{p}-\mathbf{q}\|\). Then \(\|\mathbf{p}-\mathbf{p}^\ast\| = d(\mathbf{p})\), yielding:

\[ p(\mathbf{p}\mid\mathbf{x},\mathbf{m}) \approx C\, \exp\!\left(-\frac{d(\mathbf{p})^2}{2\sigma^2}\right), \]

where \(C\) absorbs constants (including local curvature effects) that are approximately pose-independent for small noise.

Problem 4 (Log-sum-exp invariance and numerical stability): Let unnormalized log-weights be \(\ell_n\). Prove that subtracting \(m=\max_n \ell_n\) before exponentiation does not change normalized weights and reduces overflow/underflow risk.

Solution: Define:

\[ \tilde{w}_n = e^{\ell_n},\quad \tilde{w}_n' = e^{\ell_n-m}. \]

Then \(\tilde{w}_n' = e^{-m}\tilde{w}_n\), so:

\[ \frac{\tilde{w}_n'}{\sum_j \tilde{w}_j'} = \frac{e^{-m}\tilde{w}_n}{\sum_j e^{-m}\tilde{w}_j} = \frac{\tilde{w}_n}{\sum_j \tilde{w}_j}. \]

Numerically, each exponent now satisfies \(\ell_n-m \le 0\), preventing overflow and improving precision when weights differ by many orders of magnitude.

Problem 5 (Outlier mixture in vision): For \(p(\beta)=(1-\epsilon)p_{\text{in} }(\beta)+\epsilon/(2\pi)\), show that \(p(\beta) \ge \epsilon/(2\pi)\) for all \(\beta\) and interpret the effect on MCL robustness.

Solution: Since \(p_{\text{in} }(\beta)\ge 0\) and \(1-\epsilon\ge 0\):

\[ p(\beta) = (1-\epsilon)p_{\text{in} }(\beta)+\frac{\epsilon}{2\pi} \ge \frac{\epsilon}{2\pi}. \]

Hence the log-likelihood is lower-bounded: \(\log p(\beta) \ge \log(\epsilon/(2\pi))\), so a single mismatched feature cannot drive the entire particle weight to numerical zero, preventing catastrophic weight collapse in the presence of outliers.

We constructed observation likelihoods for particle-filter localization under explicit noise/outlier processes. For LiDAR, we derived the robust beam mixture model and the computationally efficient likelihood-field model based on distance transforms. For vision, we developed a bearing-only landmark likelihood with an outlier mixture. Finally, we established log-domain weight computation and log-sum-exp normalization, and introduced temperature scaling as a principled way to control likelihood peakiness and mitigate particle degeneracy.