In previous chapters, students learned localization (EKF/UKF/MCL), mapping (occupancy grids), scan matching, and SLAM formulations separately. In a full AMR autonomy stack, these modules must operate as a coupled system. The central challenge is that the map update depends on the pose estimate, while future pose estimates depend on the map. This creates a feedback loop between \( \hat{\mathbf{x} }_k \) and \( \mathcal{M}_k \).

We denote robot pose at step \( k \) by \( \mathbf{x}_k=[x_k,\;y_k,\;\theta_k]^T \), controls by \( \mathbf{u}_k \), sensor measurements by \( \mathbf{z}_k \), and the map by \( \mathcal{M} \). The capstone integration target is an online estimator of \( p(\mathbf{x}_{0:k}, \mathcal{M}\mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k}) \).

flowchart TD
  A["Wheel/IMU + LiDAR/Vision streams"] --> B["Time sync and preprocessing"]
  B --> C["Motion prediction"]
  C --> D["Pose correction (landmarks/scan match)"]
  D --> E["Validated pose estimate and covariance"]
  E --> F["Occupancy / submap update"]
  F --> G["Map used by next localization step"]
  G --> D
  F --> H["Navigation-ready map products"]
        

The full Bayesian recursion for localization + mapping can be written as:

\[ p(\mathbf{x}_{0:k}, \mathcal{M}\mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k}) \propto p(\mathbf{z}_k \mid \mathbf{x}_k, \mathcal{M}) \, p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k) \, p(\mathbf{x}_{0:k-1}, \mathcal{M}\mid \mathbf{z}_{1:k-1}, \mathbf{u}_{1:k-1}) \]

This equation is exact under the Markov assumptions of the mobile robot model. In practice, we use approximations: (i) a Gaussian filter for pose (EKF/UKF), particle filter, or graph optimizer, and (ii) a conditionally independent grid map approximation for occupancy cells.

A common online decomposition for occupancy-grid mapping uses the current pose estimate:

\[ p(\mathcal{M}\mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k}) \approx \int p(\mathcal{M}\mid \mathbf{x}_{0:k}, \mathbf{z}_{1:k})\; p(\mathbf{x}_{0:k}\mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k})\; d\mathbf{x}_{0:k} \]

In a real-time capstone implementation, this integral is often approximated by a point estimate \( \hat{\mathbf{x} }_{0:k} \) or by a small local uncertainty approximation. The key engineering implication is that localization bias propagates into map distortion.

Proof sketch of recursion validity. By Bayes rule, \( p(A\mid B) \propto p(B\mid A)p(A) \). Set \( A=(\mathbf{x}_{0:k},\mathcal{M}) \) and \( B=(\mathbf{z}_{1:k},\mathbf{u}_{1:k}) \), then factor the joint using the conditional independence assumptions \( \mathbf{z}_k \perp (\mathbf{x}_{0:k-1}, \mathbf{z}_{1:k-1}, \mathbf{u}_{1:k}) \mid (\mathbf{x}_k,\mathcal{M}) \) and \( \mathbf{x}_k \perp (\mathbf{x}_{0:k-2}, \mathbf{z}_{1:k-1}) \mid (\mathbf{x}_{k-1},\mathbf{u}_k) \).

For a differential-drive (or skid-steer with low-slip approximation), the discrete-time motion model is

\[ \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k,\qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k) \]

with one simple first-order form

\[ \begin{aligned} x_k &= x_{k-1} + v_k \cos(\theta_{k-1})\Delta t \\ y_k &= y_{k-1} + v_k \sin(\theta_{k-1})\Delta t \\ \theta_k &= \theta_{k-1} + \omega_k \Delta t \end{aligned} \]

The EKF covariance prediction uses Jacobians \( \mathbf{F}_k=\partial f/\partial \mathbf{x} \) and \( \mathbf{G}_k=\partial f/\partial \mathbf{n}_u \):

\[ \mathbf{P}_{k\mid k-1} = \mathbf{F}_k \mathbf{P}_{k-1\mid k-1} \mathbf{F}_k^T + \mathbf{G}_k \mathbf{Q}_k \mathbf{G}_k^T \]

For a landmark range-bearing observation of landmark \( j \), the measurement model is

\[ \mathbf{z}_{k,j} = \begin{bmatrix} \sqrt{(l_x^j - x_k)^2 + (l_y^j - y_k)^2} \\ \operatorname{atan2}(l_y^j - y_k,\; l_x^j - x_k) - \theta_k \end{bmatrix} + \mathbf{v}_{k,j}, \qquad \mathbf{v}_{k,j}\sim\mathcal{N}(\mathbf{0},\mathbf{R}_{k,j}) \]

The innovation and correction equations are:

\[ \mathbf{r}_{k,j} = \mathbf{z}_{k,j} - h_j(\hat{\mathbf{x} }_{k\mid k-1}), \qquad \mathbf{S}_{k,j} = \mathbf{H}_{k,j}\mathbf{P}_{k\mid k-1}\mathbf{H}_{k,j}^T + \mathbf{R}_{k,j} \]

\[ \mathbf{K}_{k,j} = \mathbf{P}_{k\mid k-1}\mathbf{H}_{k,j}^T\mathbf{S}_{k,j}^{-1}, \qquad \hat{\mathbf{x} }_{k\mid k} = \hat{\mathbf{x} }_{k\mid k-1} + \mathbf{K}_{k,j}\mathbf{r}_{k,j} \]

For robust integration, a Mahalanobis gate rejects inconsistent associations:

\[ d_{M}^2 = \mathbf{r}_{k,j}^T \mathbf{S}_{k,j}^{-1} \mathbf{r}_{k,j}, \qquad d_{M}^2 < \chi^2_{m,\alpha} \]

where \( m \) is the measurement dimension (e.g., 2 for range-bearing). This prevents corrupted localization updates from polluting the map.

For a binary occupancy grid, each cell \( m_i \in \{0,1\} \) is updated via log-odds:

\[ L_{k,i} = \log\frac{p(m_i=1\mid \mathbf{z}_{1:k}, \hat{\mathbf{x} }_{1:k})}{1-p(m_i=1\mid \mathbf{z}_{1:k}, \hat{\mathbf{x} }_{1:k})} \]

\[ L_{k,i} = L_{k-1,i} + \log\frac{p(m_i=1\mid z_k,\hat{\mathbf{x} }_k)}{1-p(m_i=1\mid z_k,\hat{\mathbf{x} }_k)} - \log\frac{p(m_i=1)}{1-p(m_i=1)} \]

In a simplified inverse sensor model with neutral prior, this becomes additive updates: free cells along a beam receive \( \ell_{\text{free} } < 0 \) and the endpoint receives \( \ell_{\text{occ} } > 0 \).

The probability view is recovered by the logistic map

\[ p(m_i=1) = \frac{1}{1+\exp(-L_{k,i})} \]

Why localization uncertainty matters: if the true pose is \( \mathbf{x}_k \) but the map is updated using \( \hat{\mathbf{x} }_k \), beam endpoints are displaced. For small pose error \( \delta \mathbf{x}_k \), endpoint perturbation is approximately

\[ \delta \mathbf{p}_{\text{end} } \approx \mathbf{J}_{\text{ray} }(\hat{\mathbf{x} }_k, z_k)\, \delta \mathbf{x}_k \]

so the endpoint covariance is

\[ \mathbf{\Sigma}_{\text{end} } \approx \mathbf{J}_{\text{ray} } \mathbf{P}_{k\mid k} \mathbf{J}_{\text{ray} }^T \]

This explains observed “fuzzy walls” when localization covariance grows.

flowchart TD
  A["Predicted pose and covariance"] --> B["Measurement association and gating"]
  B --> C["Pose correction"]
  C --> D["Ray casting / inverse sensor model"]
  D --> E["Log-odds cell updates"]
  E --> F["Updated grid or submap"]
  F --> G["Scan matching / map-based localization"]
  G --> B
        

In capstone projects, two practical integration patterns are common:

Loose coupling. A localization module outputs \( (\hat{\mathbf{x} }_k,\mathbf{P}_k) \), and the mapper consumes it. This is easier to debug and matches modular ROS2 pipelines.

Tight coupling. Pose and map variables are optimized jointly (e.g., graph-based SLAM), where scan-matching or loop-closure terms enter the same estimation problem.

A graph-based integrated objective (pose graph view) can be written as

\[ J(\mathbf{X}) = \sum_{(i,j)\in \mathcal{E}_{\text{odom} } } \left\| \mathbf{e}_{ij}^{\text{odom} }(\mathbf{x}_i,\mathbf{x}_j) \right\|_{\mathbf{\Omega}_{ij} }^2 + \sum_{(i,j)\in \mathcal{E}_{\text{loop} } } \rho\!\left( \left\| \mathbf{e}_{ij}^{\text{loop} }(\mathbf{x}_i,\mathbf{x}_j) \right\|_{\mathbf{\Lambda}_{ij} }^2 \right) \]

where \( \rho(\cdot) \) is a robust kernel. The resulting optimized trajectory is then used to build a globally consistent map (or to correct a set of submaps).

A submap strategy is often the best capstone compromise: maintain local maps over short horizons (good real-time performance), then align/optimize submaps using loop closures. This reduces repeated rewriting of the global grid while preserving consistency.

Result 1 (Joseph-form covariance remains symmetric positive semidefinite). Let \( \mathbf{P}_{k\mid k-1}\succeq 0 \) and \( \mathbf{R}_k \succ 0 \). The Joseph update

\[ \mathbf{P}_{k\mid k} = (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k\mid k-1}(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^T + \mathbf{K}_k\mathbf{R}_k\mathbf{K}_k^T \]

is symmetric positive semidefinite.

Proof. For any vector \( \mathbf{a} \),

\[ \mathbf{a}^T\mathbf{P}_{k\mid k}\mathbf{a} = \mathbf{b}^T\mathbf{P}_{k\mid k-1}\mathbf{b} + \mathbf{c}^T\mathbf{R}_k\mathbf{c} \]

with \( \mathbf{b}=(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^T\mathbf{a} \) and \( \mathbf{c}=\mathbf{K}_k^T\mathbf{a} \). Since both terms are nonnegative, the sum is nonnegative. Symmetry follows by construction.

Result 2 (Additivity of log-odds updates). If the prior log-odds is \( L_{k-1,i} \) and the inverse sensor model contributes a constant log-likelihood ratio for a cell classification event, then repeated independent beam updates produce a sum in log-odds space. This justifies efficient incremental grid mapping with integer-like accumulators.

Result 3 (Consistency check via NEES). For pose error \( \tilde{\mathbf{x} }_k = \mathbf{x}_k - \hat{\mathbf{x} }_k \), the normalized estimation error squared is

\[ \epsilon_k = \tilde{\mathbf{x} }_k^T \mathbf{P}_{k\mid k}^{-1} \tilde{\mathbf{x} }_k \]

and under a consistent Gaussian estimator, \( E[\epsilon_k] \) is approximately the state dimension. This is a powerful capstone diagnostic for whether localization uncertainty is under- or over-estimated.

The implementation pattern is the same in Python, C++, Java, MATLAB/Simulink, and Mathematica:

1. Predict pose and covariance from odometry/IMU.
2. Associate and gate observations, then correct pose.
3. Update occupancy grid (or local submap) using the corrected pose.
4. Monitor metrics such as RMSE, NEES, and map entropy.

For production AMR systems, typical libraries include: ROS2, Eigen, PCL, GTSAM, and Ceres (C++); NumPy/SciPy/OpenCV (Python); EJML or Apache Commons Math (Java); Navigation Toolbox and Robotics System Toolbox (MATLAB/Simulink). The code below is intentionally from-scratch and educational, but it aligns with these libraries.

File: Chapter20_Lesson3.py

This script implements a complete educational loop: EKF localization, Mahalanobis gating, synthetic landmark measurements, and occupancy-grid log-odds mapping.

# Chapter20_Lesson3.py
"""
Localization + Mapping Integration (Capstone AMR)
------------------------------------------------
Educational implementation of a loosely coupled pipeline:
  1) Differential-drive prediction (wheel + yaw-rate)
  2) Landmark EKF correction
  3) Occupancy-grid log-odds mapping from 2D range scans
The code is intentionally compact and readable for course use.
"""

from dataclasses import dataclass
import math
import numpy as np


@dataclass
class Config:
    dt: float = 0.1
    wheel_base: float = 0.34
    q_v: float = 0.02          # process noise on linear velocity
    q_w: float = 0.03          # process noise on yaw rate
    r_range: float = 0.15      # landmark range noise
    r_bearing: float = 0.03    # landmark bearing noise
    grid_res: float = 0.20
    grid_size: int = 120       # square grid
    l_occ: float = 0.85
    l_free: float = -0.40
    l_min: float = -4.0
    l_max: float = 4.0


def wrap_angle(a: float) -> float:
    return (a + np.pi) % (2.0 * np.pi) - np.pi


class EKFLocalizer:
    def __init__(self, cfg: Config):
        self.cfg = cfg
        self.x = np.array([1.0, 1.0, 0.0], dtype=float)  # [x, y, theta]
        self.P = np.diag([0.05, 0.05, 0.02])

    def predict(self, v: float, w: float):
        dt = self.cfg.dt
        x, y, th = self.x

        # State propagation
        if abs(w) < 1e-6:
            nx = x + v * math.cos(th) * dt
            ny = y + v * math.sin(th) * dt
            nth = th
        else:
            nx = x + (v / w) * (math.sin(th + w * dt) - math.sin(th))
            ny = y - (v / w) * (math.cos(th + w * dt) - math.cos(th))
            nth = th + w * dt

        self.x = np.array([nx, ny, wrap_angle(nth)])

        # Jacobian wrt state (first-order)
        F = np.eye(3)
        F[0, 2] = -v * math.sin(th) * dt
        F[1, 2] =  v * math.cos(th) * dt

        # Jacobian wrt control noise [delta_v, delta_w]
        G = np.zeros((3, 2))
        G[0, 0] = math.cos(th) * dt
        G[1, 0] = math.sin(th) * dt
        G[2, 1] = dt
        Q = np.diag([self.cfg.q_v**2, self.cfg.q_w**2])

        self.P = F @ self.P @ F.T + G @ Q @ G.T

    def correct_landmark(self, z_range: float, z_bearing: float, landmark_xy):
        lx, ly = landmark_xy
        x, y, th = self.x
        dx = lx - x
        dy = ly - y
        q = dx * dx + dy * dy
        if q < 1e-9:
            return

        zhat = np.array([math.sqrt(q), wrap_angle(math.atan2(dy, dx) - th)])

        H = np.array([
            [-dx / math.sqrt(q), -dy / math.sqrt(q), 0.0],
            [dy / q,            -dx / q,           -1.0],
        ])

        R = np.diag([self.cfg.r_range**2, self.cfg.r_bearing**2])
        S = H @ self.P @ H.T + R
        K = self.P @ H.T @ np.linalg.inv(S)

        innov = np.array([z_range - zhat[0], wrap_angle(z_bearing - zhat[1])])

        # Chi-square gating for 2D measurement
        d2 = float(innov.T @ np.linalg.inv(S) @ innov)
        if d2 > 9.21:  # approx chi-square(2, 0.99)
            return

        self.x = self.x + K @ innov
        self.x[2] = wrap_angle(self.x[2])

        I = np.eye(3)
        # Joseph form for numerical stability and PSD covariance
        self.P = (I - K @ H) @ self.P @ (I - K @ H).T + K @ R @ K.T


class OccupancyGrid:
    def __init__(self, cfg: Config):
        self.cfg = cfg
        self.logodds = np.zeros((cfg.grid_size, cfg.grid_size), dtype=float)
        self.origin_world = np.array([0.0, 0.0])  # lower-left corner in meters

    def world_to_grid(self, x: float, y: float):
        gx = int((x - self.origin_world[0]) / self.cfg.grid_res)
        gy = int((y - self.origin_world[1]) / self.cfg.grid_res)
        return gx, gy

    def in_bounds(self, gx: int, gy: int) -> bool:
        n = self.cfg.grid_size
        return 0 <= gx < n and 0 <= gy < n

    def bresenham(self, x0, y0, x1, y1):
        points = []
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx - dy
        x, y = x0, y0
        while True:
            points.append((x, y))
            if x == x1 and y == y1:
                break
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x += sx
            if e2 < dx:
                err += dx
                y += sy
        return points

    def update_scan(self, pose, scan_ranges, scan_angles, max_range=8.0):
        rx, ry, rth = pose
        g0x, g0y = self.world_to_grid(rx, ry)
        if not self.in_bounds(g0x, g0y):
            return

        for r, a in zip(scan_ranges, scan_angles):
            if r <= 0.05:
                continue
            rr = min(r, max_range)
            ex = rx + rr * math.cos(rth + a)
            ey = ry + rr * math.sin(rth + a)
            g1x, g1y = self.world_to_grid(ex, ey)
            if not self.in_bounds(g1x, g1y):
                continue

            ray = self.bresenham(g0x, g0y, g1x, g1y)
            # Free cells along the ray (except terminal)
            for (cx, cy) in ray[:-1]:
                self.logodds[cy, cx] = np.clip(
                    self.logodds[cy, cx] + self.cfg.l_free,
                    self.cfg.l_min,
                    self.cfg.l_max,
                )
            # Occupied terminal cell only if not max-range truncation
            if r < max_range:
                cx, cy = ray[-1]
                self.logodds[cy, cx] = np.clip(
                    self.logodds[cy, cx] + self.cfg.l_occ,
                    self.cfg.l_min,
                    self.cfg.l_max,
                )

    def occupancy_probability(self):
        return 1.0 / (1.0 + np.exp(-self.logodds))


def simulate_controls(k: int):
    # Smooth commands for a capstone-style indoor path
    v = 0.35 + 0.05 * math.sin(0.05 * k)
    w = 0.25 * math.sin(0.03 * k)
    return v, w


def synthetic_landmark_measurement(true_pose, landmark):
    tx, ty, tth = true_pose
    lx, ly = landmark
    dx, dy = lx - tx, ly - ty
    rng = math.hypot(dx, dy) + np.random.normal(0.0, 0.08)
    brg = wrap_angle(math.atan2(dy, dx) - tth + np.random.normal(0.0, 0.02))
    return rng, brg


def synthetic_scan(true_pose, n_beams=61, max_range=8.0):
    # Simple simulated room with one internal obstacle rectangle
    x, y, th = true_pose
    angles = np.linspace(-1.2, 1.2, n_beams)
    ranges = np.full_like(angles, max_range, dtype=float)

    # World boundaries and one box obstacle (axis-aligned)
    segments = [
        ((0.0, 0.0), (12.0, 0.0)), ((12.0, 0.0), (12.0, 12.0)),
        ((12.0, 12.0), (0.0, 12.0)), ((0.0, 12.0), (0.0, 0.0)),
        ((4.5, 4.0), (7.5, 4.0)), ((7.5, 4.0), (7.5, 6.5)),
        ((7.5, 6.5), (4.5, 6.5)), ((4.5, 6.5), (4.5, 4.0)),
    ]

    def ray_segment_intersection(px, py, dx, dy, ax, ay, bx, by):
        sx, sy = (bx - ax), (by - ay)
        denom = (-sx * dy + dx * sy)
        if abs(denom) < 1e-9:
            return None
        s = (-dy * (px - ax) + dx * (py - ay)) / denom
        t = ( sx * (py - ay) - sy * (px - ax)) / denom
        if 0.0 <= s <= 1.0 and t >= 0.0:
            return t
        return None

    for i, a in enumerate(angles):
        ang = th + a
        dx, dy = math.cos(ang), math.sin(ang)
        best = max_range
        for (p0, p1) in segments:
            t = ray_segment_intersection(x, y, dx, dy, p0[0], p0[1], p1[0], p1[1])
            if t is not None and t < best:
                best = t
        ranges[i] = max(0.05, best + np.random.normal(0.0, 0.03))
    return ranges, angles


def main():
    np.random.seed(4)
    cfg = Config()
    ekf = EKFLocalizer(cfg)
    grid = OccupancyGrid(cfg)

    # Ground-truth pose and landmarks
    x_true = np.array([1.0, 1.0, 0.0], dtype=float)
    landmarks = [(2.0, 10.0), (10.0, 2.5), (9.5, 10.5), (2.0, 5.5)]

    traj_true = []
    traj_est = []

    for k in range(240):
        v_cmd, w_cmd = simulate_controls(k)

        # Truth propagation with noise (plant)
        x_true[0] += v_cmd * math.cos(x_true[2]) * cfg.dt
        x_true[1] += v_cmd * math.sin(x_true[2]) * cfg.dt
        x_true[2] = wrap_angle(x_true[2] + w_cmd * cfg.dt + np.random.normal(0.0, 0.005))

        # Localization prediction with noisy control
        v_meas = v_cmd + np.random.normal(0.0, 0.02)
        w_meas = w_cmd + np.random.normal(0.0, 0.01)
        ekf.predict(v_meas, w_meas)

        # Landmark correction every 5 steps (simulate detector)
        if k % 5 == 0:
            for lm in landmarks:
                rng, brg = synthetic_landmark_measurement(x_true, lm)
                if rng < 7.5:
                    ekf.correct_landmark(rng, brg, lm)

        # Mapping update every step using estimated pose (typical online pipeline)
        scan_r, scan_a = synthetic_scan(x_true)
        grid.update_scan(ekf.x.copy(), scan_r, scan_a)

        traj_true.append(x_true.copy())
        traj_est.append(ekf.x.copy())

    traj_true = np.array(traj_true)
    traj_est = np.array(traj_est)

    pos_rmse = np.sqrt(np.mean(np.sum((traj_true[:, :2] - traj_est[:, :2]) ** 2, axis=1)))
    yaw_rmse = np.sqrt(np.mean((np.unwrap(traj_true[:, 2]) - np.unwrap(traj_est[:, 2])) ** 2))

    print("Final estimated pose:", ekf.x)
    print("Final covariance diag:", np.diag(ekf.P))
    print("Position RMSE [m]:", round(float(pos_rmse), 4))
    print("Yaw RMSE [rad]:", round(float(yaw_rmse), 4))

    occ_prob = grid.occupancy_probability()
    print("Map occupancy probability stats:",
          "min=", round(float(np.min(occ_prob)), 3),
          "max=", round(float(np.max(occ_prob)), 3),
          "mean=", round(float(np.mean(occ_prob)), 3))


if __name__ == "__main__":
    main()

      

File: Chapter20_Lesson3.cpp

This C++ version uses Eigen for compact matrix operations and demonstrates the same integration logic. In a ROS2 capstone, this would naturally map to a localization node and a mapping node exchanging PoseWithCovariance and scan/map messages.

// Chapter20_Lesson3.cpp
// Localization + Mapping Integration (Capstone AMR)
// Educational Eigen-based example: EKF prediction/correction + occupancy log-odds update.

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <random>
#include <tuple>
#include <utility>
#include <vector>
#include <algorithm>

struct Config {
    double dt = 0.1;
    double qv = 0.02;
    double qw = 0.03;
    double rr = 0.15;
    double rb = 0.03;
    double gridRes = 0.20;
    int gridSize = 120;
    double lOcc = 0.85;
    double lFree = -0.40;
    double lMin = -4.0;
    double lMax = 4.0;
};

static double WrapAngle(double a) {
    while (a > M_PI) a -= 2.0 * M_PI;
    while (a < -M_PI) a += 2.0 * M_PI;
    return a;
}

class EKFLocalizer {
public:
    explicit EKFLocalizer(const Config& cfg) : cfg_(cfg) {
        x_ << 1.0, 1.0, 0.0;
        P_.setZero();
        P_(0,0) = 0.05; P_(1,1) = 0.05; P_(2,2) = 0.02;
    }

    void Predict(double v, double w) {
        const double dt = cfg_.dt;
        const double x = x_(0), y = x_(1), th = x_(2);

        x_(0) = x + v * std::cos(th) * dt;
        x_(1) = y + v * std::sin(th) * dt;
        x_(2) = WrapAngle(th + w * dt);

        Eigen::Matrix3d F = Eigen::Matrix3d::Identity();
        F(0,2) = -v * std::sin(th) * dt;
        F(1,2) =  v * std::cos(th) * dt;

        Eigen::Matrix<double,3,2> G;
        G.setZero();
        G(0,0) = std::cos(th) * dt;
        G(1,0) = std::sin(th) * dt;
        G(2,1) = dt;

        Eigen::Matrix2d Q = Eigen::Matrix2d::Zero();
        Q(0,0) = cfg_.qv * cfg_.qv;
        Q(1,1) = cfg_.qw * cfg_.qw;

        P_ = F * P_ * F.transpose() + G * Q * G.transpose();
    }

    void CorrectLandmark(double zRange, double zBearing, const Eigen::Vector2d& lm) {
        const double dx = lm(0) - x_(0);
        const double dy = lm(1) - x_(1);
        const double q = dx*dx + dy*dy;
        if (q < 1e-12) return;

        Eigen::Vector2d zhat;
        zhat(0) = std::sqrt(q);
        zhat(1) = WrapAngle(std::atan2(dy, dx) - x_(2));

        Eigen::Matrix<double,2,3> H;
        H << -dx/std::sqrt(q), -dy/std::sqrt(q), 0.0,
              dy/q,            -dx/q,           -1.0;

        Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
        R(0,0) = cfg_.rr * cfg_.rr;
        R(1,1) = cfg_.rb * cfg_.rb;

        Eigen::Matrix2d S = H * P_ * H.transpose() + R;
        Eigen::Matrix<double,3,2> K = P_ * H.transpose() * S.inverse();

        Eigen::Vector2d innov;
        innov(0) = zRange - zhat(0);
        innov(1) = WrapAngle(zBearing - zhat(1));

        double d2 = innov.transpose() * S.inverse() * innov;
        if (d2 > 9.21) return;

        x_ = x_ + K * innov;
        x_(2) = WrapAngle(x_(2));

        Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
        P_ = (I - K*H) * P_ * (I - K*H).transpose() + K * R * K.transpose();
    }

    const Eigen::Vector3d& state() const { return x_; }
    const Eigen::Matrix3d& cov() const { return P_; }

private:
    Config cfg_;
    Eigen::Vector3d x_;
    Eigen::Matrix3d P_;
};

class OccupancyGrid {
public:
    explicit OccupancyGrid(const Config& cfg) : cfg_(cfg) {
        logOdds_.assign(cfg_.gridSize * cfg_.gridSize, 0.0);
    }

    std::pair<int,int> WorldToGrid(double x, double y) const {
        return {static_cast<int>(x / cfg_.gridRes), static_cast<int>(y / cfg_.gridRes)};
    }

    bool InBounds(int gx, int gy) const {
        return gx >= 0 && gy >= 0 && gx < cfg_.gridSize && gy < cfg_.gridSize;
    }

    std::vector<std::pair<int,int>> Bresenham(int x0, int y0, int x1, int y1) const {
        std::vector<std::pair<int,int>> pts;
        int dx = std::abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
        int dy = -std::abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
        int err = dx + dy;
        int x = x0, y = y0;
        while (true) {
            pts.push_back({x,y});
            if (x == x1 && y == y1) break;
            int e2 = 2 * err;
            if (e2 >= dy) { err += dy; x += sx; }
            if (e2 <= dx) { err += dx; y += sy; }
        }
        return pts;
    }

    void UpdateRay(double rx, double ry, double ex, double ey, bool hit) {
        auto [g0x, g0y] = WorldToGrid(rx, ry);
        auto [g1x, g1y] = WorldToGrid(ex, ey);
        if (!InBounds(g0x, g0y) || !InBounds(g1x, g1y)) return;

        auto ray = Bresenham(g0x, g0y, g1x, g1y);
        for (size_t i = 0; i + 1 < ray.size(); ++i) {
            Accumulate(ray[i].first, ray[i].second, cfg_.lFree);
        }
        if (hit && !ray.empty()) {
            Accumulate(ray.back().first, ray.back().second, cfg_.lOcc);
        }
    }

    void PrintSummary() const {
        double mn = 1e9, mx = -1e9, avg = 0.0;
        for (double l : logOdds_) {
            double p = 1.0 / (1.0 + std::exp(-l));
            mn = std::min(mn, p);
            mx = std::max(mx, p);
            avg += p;
        }
        avg /= static_cast<double>(logOdds_.size());
        std::cout << "Map occupancy prob stats: min=" << mn
                  << " max=" << mx << " mean=" << avg << "\n";
    }

private:
    void Accumulate(int gx, int gy, double delta) {
        if (!InBounds(gx, gy)) return;
        double& cell = logOdds_[gy * cfg_.gridSize + gx];
        cell = std::clamp(cell + delta, cfg_.lMin, cfg_.lMax);
    }

    Config cfg_;
    std::vector<double> logOdds_;
};

static std::mt19937_64 rng(7);
static std::normal_distribution<double> n01(0.0, 1.0);

int main() {
    Config cfg;
    EKFLocalizer ekf(cfg);
    OccupancyGrid grid(cfg);

    Eigen::Vector3d xTrue(1.0, 1.0, 0.0);
    std::vector<Eigen::Vector2d> landmarks = {
        {2.0, 10.0}, {10.0, 2.5}, {9.5, 10.5}, {2.0, 5.5}
    };

    auto randn = [](double s){ return s * n01(rng); };

    double sqErrPos = 0.0, sqErrYaw = 0.0;
    int n = 0;

    for (int k = 0; k < 240; ++k) {
        double vCmd = 0.35 + 0.05 * std::sin(0.05 * k);
        double wCmd = 0.25 * std::sin(0.03 * k);

        // Truth
        xTrue(0) += vCmd * std::cos(xTrue(2)) * cfg.dt;
        xTrue(1) += vCmd * std::sin(xTrue(2)) * cfg.dt;
        xTrue(2) = WrapAngle(xTrue(2) + wCmd * cfg.dt + randn(0.005));

        // EKF prediction
        ekf.Predict(vCmd + randn(0.02), wCmd + randn(0.01));

        // Landmark corrections
        if (k % 5 == 0) {
            for (const auto& lm : landmarks) {
                double dx = lm(0) - xTrue(0);
                double dy = lm(1) - xTrue(1);
                double range = std::sqrt(dx*dx + dy*dy);
                if (range < 7.5) {
                    double zRange = range + randn(0.08);
                    double zBearing = WrapAngle(std::atan2(dy, dx) - xTrue(2) + randn(0.02));
                    ekf.CorrectLandmark(zRange, zBearing, lm);
                }
            }
        }

        // Lightweight synthetic rays (front, left, right) for grid update
        const auto est = ekf.state();
        std::vector<double> beamAngles = {-0.7, 0.0, 0.7};
        for (double a : beamAngles) {
            double rr = 5.0; // pretend obstacle at fixed distance in demo
            double ex = est(0) + rr * std::cos(est(2) + a);
            double ey = est(1) + rr * std::sin(est(2) + a);
            grid.UpdateRay(est(0), est(1), ex, ey, true);
        }

        const auto xe = ekf.state();
        double ex = xe(0) - xTrue(0);
        double ey = xe(1) - xTrue(1);
        sqErrPos += ex*ex + ey*ey;
        sqErrYaw += std::pow(WrapAngle(xe(2) - xTrue(2)), 2);
        ++n;
    }

    std::cout << "Final estimated pose: " << ekf.state().transpose() << "\n";
    std::cout << "Final covariance diagonal: " << ekf.cov().diagonal().transpose() << "\n";
    std::cout << "Position RMSE [m]: " << std::sqrt(sqErrPos / n) << "\n";
    std::cout << "Yaw RMSE [rad]: " << std::sqrt(sqErrYaw / n) << "\n";
    grid.PrintSummary();

    return 0;
}

      

File: Chapter20_Lesson3.java

The Java implementation is intentionally self-contained and avoids external dependencies. It is suitable for students who want to understand the estimator numerics without framework overhead.

// Chapter20_Lesson3.java
// Localization + Mapping Integration (Capstone AMR)
// Pure Java educational implementation of odometry prediction + landmark correction + occupancy log-odds.

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

public class Chapter20_Lesson3 {

    static class Config {
        double dt = 0.1;
        double qv = 0.02;
        double qw = 0.03;
        double rr = 0.15;
        double rb = 0.03;
        double gridRes = 0.20;
        int gridSize = 120;
        double lOcc = 0.85;
        double lFree = -0.40;
        double lMin = -4.0;
        double lMax = 4.0;
    }

    static double wrap(double a) {
        while (a > Math.PI) a -= 2.0 * Math.PI;
        while (a < -Math.PI) a += 2.0 * Math.PI;
        return a;
    }

    static class EKFLocalizer {
        Config cfg;
        double[] x = new double[]{1.0, 1.0, 0.0}; // x, y, theta
        double[][] P = new double[][]{
                {0.05, 0, 0},
                {0, 0.05, 0},
                {0, 0, 0.02}
        };

        EKFLocalizer(Config cfg) {
            this.cfg = cfg;
        }

        void predict(double v, double w) {
            double th = x[2];
            x[0] += v * Math.cos(th) * cfg.dt;
            x[1] += v * Math.sin(th) * cfg.dt;
            x[2] = wrap(x[2] + w * cfg.dt);

            double[][] F = {
                    {1, 0, -v * Math.sin(th) * cfg.dt},
                    {0, 1,  v * Math.cos(th) * cfg.dt},
                    {0, 0, 1}
            };
            double[][] G = {
                    {Math.cos(th) * cfg.dt, 0},
                    {Math.sin(th) * cfg.dt, 0},
                    {0, cfg.dt}
            };
            double[][] Q = {
                    {cfg.qv * cfg.qv, 0},
                    {0, cfg.qw * cfg.qw}
            };
            P = add(mul(F, mul(P, tr(F))), mul(G, mul(Q, tr(G))));
        }

        void correctLandmark(double zRange, double zBearing, double lx, double ly) {
            double dx = lx - x[0];
            double dy = ly - x[1];
            double q = dx * dx + dy * dy;
            if (q < 1e-12) return;

            double zhatRange = Math.sqrt(q);
            double zhatBearing = wrap(Math.atan2(dy, dx) - x[2]);

            double[][] H = {
                    {-dx / Math.sqrt(q), -dy / Math.sqrt(q), 0},
                    { dy / q,            -dx / q,          -1}
            };
            double[][] R = {
                    {cfg.rr * cfg.rr, 0},
                    {0, cfg.rb * cfg.rb}
            };

            double[][] S = add(mul(H, mul(P, tr(H))), R);
            double[] innov = new double[]{zRange - zhatRange, wrap(zBearing - zhatBearing)};
            double[][] SInv = inv2(S);

            double d2 = quadForm(innov, SInv);
            if (d2 > 9.21) return;

            double[][] K = mul(P, mul(tr(H), SInv));
            double[] dxState = mul(K, innov);
            for (int i = 0; i < 3; i++) x[i] += dxState[i];
            x[2] = wrap(x[2]);

            double[][] I = eye3();
            double[][] KH = mul(K, H);
            double[][] IKH = sub(I, KH);
            P = add(mul(IKH, mul(P, tr(IKH))), mul(K, mul(R, tr(K))));
        }
    }

    static class OccupancyGrid {
        Config cfg;
        double[][] L;

        OccupancyGrid(Config cfg) {
            this.cfg = cfg;
            this.L = new double[cfg.gridSize][cfg.gridSize];
        }

        int[] worldToGrid(double x, double y) {
            int gx = (int) (x / cfg.gridRes);
            int gy = (int) (y / cfg.gridRes);
            return new int[]{gx, gy};
        }

        boolean inBounds(int gx, int gy) {
            return gx >= 0 && gy >= 0 && gx < cfg.gridSize && gy < cfg.gridSize;
        }

        List<int[]> bresenham(int x0, int y0, int x1, int y1) {
            List<int[]> pts = new ArrayList<>();
            int dx = Math.abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
            int dy = -Math.abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
            int err = dx + dy;
            int x = x0, y = y0;
            while (true) {
                pts.add(new int[]{x, y});
                if (x == x1 && y == y1) break;
                int e2 = 2 * err;
                if (e2 >= dy) { err += dy; x += sx; }
                if (e2 <= dx) { err += dx; y += sy; }
            }
            return pts;
        }

        void updateRay(double rx, double ry, double ex, double ey, boolean hit) {
            int[] g0 = worldToGrid(rx, ry);
            int[] g1 = worldToGrid(ex, ey);
            if (!inBounds(g0[0], g0[1]) || !inBounds(g1[0], g1[1])) return;

            List<int[]> ray = bresenham(g0[0], g0[1], g1[0], g1[1]);
            for (int i = 0; i < ray.size() - 1; i++) {
                int[] p = ray.get(i);
                L[p[1]][p[0]] = clip(L[p[1]][p[0]] + cfg.lFree, cfg.lMin, cfg.lMax);
            }
            if (hit && !ray.isEmpty()) {
                int[] p = ray.get(ray.size() - 1);
                L[p[1]][p[0]] = clip(L[p[1]][p[0]] + cfg.lOcc, cfg.lMin, cfg.lMax);
            }
        }

        void printSummary() {
            double mn = 1.0, mx = 0.0, sum = 0.0;
            int n = cfg.gridSize * cfg.gridSize;
            for (int y = 0; y < cfg.gridSize; y++) {
                for (int x = 0; x < cfg.gridSize; x++) {
                    double p = 1.0 / (1.0 + Math.exp(-L[y][x]));
                    mn = Math.min(mn, p);
                    mx = Math.max(mx, p);
                    sum += p;
                }
            }
            System.out.printf("Map occupancy prob stats: min=%.3f max=%.3f mean=%.3f%n", mn, mx, sum / n);
        }
    }

    // ---------- Linear algebra helpers for small matrices ----------
    static double[][] add(double[][] A, double[][] B) {
        double[][] C = new double[A.length][A[0].length];
        for (int i = 0; i < A.length; i++) for (int j = 0; j < A[0].length; j++) C[i][j] = A[i][j] + B[i][j];
        return C;
    }
    static double[][] sub(double[][] A, double[][] B) {
        double[][] C = new double[A.length][A[0].length];
        for (int i = 0; i < A.length; i++) for (int j = 0; j < A[0].length; j++) C[i][j] = A[i][j] - B[i][j];
        return C;
    }
    static double[][] tr(double[][] A) {
        double[][] T = new double[A[0].length][A.length];
        for (int i = 0; i < A.length; i++) for (int j = 0; j < A[0].length; j++) T[j][i] = A[i][j];
        return T;
    }
    static double[][] mul(double[][] A, double[][] B) {
        double[][] C = new double[A.length][B[0].length];
        for (int i = 0; i < A.length; i++) {
            for (int k = 0; k < B.length; k++) {
                for (int j = 0; j < B[0].length; j++) C[i][j] += A[i][k] * B[k][j];
            }
        }
        return C;
    }
    static double[] mul(double[][] A, double[] x) {
        double[] y = new double[A.length];
        for (int i = 0; i < A.length; i++) for (int j = 0; j < x.length; j++) y[i] += A[i][j] * x[j];
        return y;
    }
    static double quadForm(double[] x, double[][] A) {
        double[] t = mul(A, x);
        double s = 0.0;
        for (int i = 0; i < x.length; i++) s += x[i] * t[i];
        return s;
    }
    static double[][] inv2(double[][] A) {
        double a = A[0][0], b = A[0][1], c = A[1][0], d = A[1][1];
        double det = a * d - b * c;
        return new double[][]{ { d / det, -b / det}, {-c / det, a / det} };
    }
    static double[][] eye3() {
        return new double[][]{ {1,0,0},{0,1,0},{0,0,1} };
    }
    static double clip(double v, double lo, double hi) {
        return Math.max(lo, Math.min(hi, v));
    }

    public static void main(String[] args) {
        Config cfg = new Config();
        EKFLocalizer ekf = new EKFLocalizer(cfg);
        OccupancyGrid grid = new OccupancyGrid(cfg);

        Random rnd = new Random(5);
        double[] xTrue = new double[]{1.0, 1.0, 0.0};
        double[][] landmarks = {
                {2.0, 10.0}, {10.0, 2.5}, {9.5, 10.5}, {2.0, 5.5}
        };

        double ssePos = 0.0, sseYaw = 0.0;
        int N = 0;

        for (int k = 0; k < 240; k++) {
            double vCmd = 0.35 + 0.05 * Math.sin(0.05 * k);
            double wCmd = 0.25 * Math.sin(0.03 * k);

            xTrue[0] += vCmd * Math.cos(xTrue[2]) * cfg.dt;
            xTrue[1] += vCmd * Math.sin(xTrue[2]) * cfg.dt;
            xTrue[2] = wrap(xTrue[2] + wCmd * cfg.dt + 0.005 * rnd.nextGaussian());

            ekf.predict(vCmd + 0.02 * rnd.nextGaussian(), wCmd + 0.01 * rnd.nextGaussian());

            if (k % 5 == 0) {
                for (double[] lm : landmarks) {
                    double dx = lm[0] - xTrue[0];
                    double dy = lm[1] - xTrue[1];
                    double range = Math.hypot(dx, dy);
                    if (range < 7.5) {
                        double zR = range + 0.08 * rnd.nextGaussian();
                        double zB = wrap(Math.atan2(dy, dx) - xTrue[2] + 0.02 * rnd.nextGaussian());
                        ekf.correctLandmark(zR, zB, lm[0], lm[1]);
                    }
                }
            }

            // Simple mapping update using three synthetic rays
            double[] angs = new double[]{-0.7, 0.0, 0.7};
            for (double a : angs) {
                double rr = 5.0;
                double ex = ekf.x[0] + rr * Math.cos(ekf.x[2] + a);
                double ey = ekf.x[1] + rr * Math.sin(ekf.x[2] + a);
                grid.updateRay(ekf.x[0], ekf.x[1], ex, ey, true);
            }

            double ex = ekf.x[0] - xTrue[0];
            double ey = ekf.x[1] - xTrue[1];
            ssePos += ex * ex + ey * ey;
            sseYaw += Math.pow(wrap(ekf.x[2] - xTrue[2]), 2);
            N++;
        }

        System.out.printf("Final estimated pose: [%.4f, %.4f, %.4f]%n", ekf.x[0], ekf.x[1], ekf.x[2]);
        System.out.printf("Final covariance diag: [%.5f, %.5f, %.5f]%n", ekf.P[0][0], ekf.P[1][1], ekf.P[2][2]);
        System.out.printf("Position RMSE [m]: %.4f%n", Math.sqrt(ssePos / N));
        System.out.printf("Yaw RMSE [rad]: %.4f%n", Math.sqrt(sseYaw / N));
        grid.printSummary();
    }
}

      

File: Chapter20_Lesson3.m

This script is MATLAB-first and Simulink-ready: each block (prediction, correction, mapping) can be converted to a MATLAB Function block for a Simulink capstone model.

% Chapter20_Lesson3.m
% Localization + Mapping Integration (Capstone AMR)
% MATLAB / Simulink-oriented educational script:
%   - EKF localization with landmark updates
%   - Occupancy-grid log-odds mapping with ray updates
%
% Related toolboxes (optional for extension):
%   Navigation Toolbox, Robotics System Toolbox, Sensor Fusion and Tracking Toolbox

clear; clc; rng(6);

%% Configuration
dt = 0.1;
qv = 0.02; qw = 0.03;
rr = 0.15; rb = 0.03;
gridRes = 0.2; gridSize = 120;
lOcc = 0.85; lFree = -0.40; lMin = -4; lMax = 4;

%% State and covariance
xhat = [1.0; 1.0; 0.0];
P = diag([0.05, 0.05, 0.02]);

%% Ground truth and map
xtrue = [1.0; 1.0; 0.0];
landmarks = [2.0,10.0; 10.0,2.5; 9.5,10.5; 2.0,5.5];
L = zeros(gridSize, gridSize); % log-odds grid

trajTrue = zeros(240,3);
trajEst  = zeros(240,3);

%% Main integration loop
for k = 1:240
    vCmd = 0.35 + 0.05*sin(0.05*k);
    wCmd = 0.25*sin(0.03*k);

    % ----- Truth propagation -----
    xtrue(1) = xtrue(1) + vCmd*cos(xtrue(3))*dt;
    xtrue(2) = xtrue(2) + vCmd*sin(xtrue(3))*dt;
    xtrue(3) = wrapToPiLocal(xtrue(3) + wCmd*dt + 0.005*randn);

    % ----- EKF prediction -----
    vMeas = vCmd + 0.02*randn;
    wMeas = wCmd + 0.01*randn;
    th = xhat(3);

    xhat(1) = xhat(1) + vMeas*cos(th)*dt;
    xhat(2) = xhat(2) + vMeas*sin(th)*dt;
    xhat(3) = wrapToPiLocal(xhat(3) + wMeas*dt);

    F = [1,0,-vMeas*sin(th)*dt;
         0,1, vMeas*cos(th)*dt;
         0,0,1];
    G = [cos(th)*dt, 0;
         sin(th)*dt, 0;
         0, dt];
    Q = diag([qv^2, qw^2]);
    P = F*P*F' + G*Q*G';

    % ----- Landmark corrections every 5 steps -----
    if mod(k,5) == 0
        for i = 1:size(landmarks,1)
            dx = landmarks(i,1) - xtrue(1);
            dy = landmarks(i,2) - xtrue(2);
            zRange = hypot(dx,dy) + 0.08*randn;
            if zRange > 7.5
                continue;
            end
            zBear = wrapToPiLocal(atan2(dy,dx) - xtrue(3) + 0.02*randn);

            dxh = landmarks(i,1) - xhat(1);
            dyh = landmarks(i,2) - xhat(2);
            q = dxh^2 + dyh^2;
            if q < 1e-12
                continue;
            end

            zhat = [sqrt(q); wrapToPiLocal(atan2(dyh,dxh) - xhat(3))];
            H = [-dxh/sqrt(q), -dyh/sqrt(q), 0;
                  dyh/q,       -dxh/q,      -1];
            R = diag([rr^2, rb^2]);

            S = H*P*H' + R;
            innov = [zRange - zhat(1); wrapToPiLocal(zBear - zhat(2))];
            d2 = innov' * (S \ innov);

            if d2 < 9.21
                K = P*H'/S;
                xhat = xhat + K*innov;
                xhat(3) = wrapToPiLocal(xhat(3));
                I = eye(3);
                P = (I-K*H)*P*(I-K*H)' + K*R*K'; % Joseph form
            end
        end
    end

    % ----- Mapping update with three synthetic rays (educational simplification) -----
    beamAngles = [-0.7, 0.0, 0.7];
    for a = beamAngles
        rrHit = 5.0;
        ex = xhat(1) + rrHit*cos(xhat(3)+a);
        ey = xhat(2) + rrHit*sin(xhat(3)+a);
        [L] = updateRayLogOdds(L, xhat(1), xhat(2), ex, ey, true, ...
                               gridRes, lOcc, lFree, lMin, lMax);
    end

    trajTrue(k,:) = xtrue.';
    trajEst(k,:)  = xhat.';
end

%% Metrics
posRMSE = sqrt(mean(sum((trajTrue(:,1:2)-trajEst(:,1:2)).^2, 2)));
yawErr = wrapToPiVector(trajTrue(:,3)-trajEst(:,3));
yawRMSE = sqrt(mean(yawErr.^2));

occProb = 1 ./ (1 + exp(-L));
fprintf('Final estimated pose: [%.4f %.4f %.4f]\n', xhat(1), xhat(2), xhat(3));
fprintf('Final covariance diag: [%.5f %.5f %.5f]\n', P(1,1), P(2,2), P(3,3));
fprintf('Position RMSE [m]: %.4f\n', posRMSE);
fprintf('Yaw RMSE [rad]: %.4f\n', yawRMSE);
fprintf('Map occupancy probability stats: min=%.3f max=%.3f mean=%.3f\n', ...
        min(occProb(:)), max(occProb(:)), mean(occProb(:)));

%% Optional quick visualization
figure;
imagesc(occProb); axis image; colorbar;
title('Occupancy Probability Map (estimated-pose updates)');
xlabel('grid x'); ylabel('grid y');

figure;
plot(trajTrue(:,1), trajTrue(:,2), 'LineWidth', 1.2); hold on;
plot(trajEst(:,1),  trajEst(:,2),  '--', 'LineWidth', 1.2);
axis equal; grid on; legend('True', 'Estimated');
title('Trajectory: truth vs EKF estimate');
xlabel('x [m]'); ylabel('y [m]');

%% Local functions
function a = wrapToPiLocal(a)
    a = mod(a + pi, 2*pi) - pi;
end

function v = wrapToPiVector(v)
    v = mod(v + pi, 2*pi) - pi;
end

function [L] = updateRayLogOdds(L, rx, ry, ex, ey, hit, gridRes, lOcc, lFree, lMin, lMax)
    [g0x, g0y] = worldToGrid(rx, ry, gridRes);
    [g1x, g1y] = worldToGrid(ex, ey, gridRes);
    [H, W] = size(L);
    if ~inBounds(g0x, g0y, W, H) || ~inBounds(g1x, g1y, W, H)
        return;
    end

    ray = bresenham(g0x, g0y, g1x, g1y);
    if size(ray,1) >= 2
        for i = 1:size(ray,1)-1
            cx = ray(i,1); cy = ray(i,2);
            L(cy, cx) = clip(L(cy, cx) + lFree, lMin, lMax);
        end
    end
    if hit
        cx = ray(end,1); cy = ray(end,2);
        L(cy, cx) = clip(L(cy, cx) + lOcc, lMin, lMax);
    end
end

function [gx, gy] = worldToGrid(x, y, gridRes)
    gx = floor(x/gridRes) + 1;
    gy = floor(y/gridRes) + 1;
end

function ok = inBounds(gx, gy, W, H)
    ok = gx >= 1 && gx <= W && gy >= 1 && gy <= H;
end

function pts = bresenham(x0, y0, x1, y1)
    dx = abs(x1 - x0); sx = sign(x1 - x0); if sx == 0, sx = 1; end
    dy = -abs(y1 - y0); sy = sign(y1 - y0); if sy == 0, sy = 1; end
    err = dx + dy;
    x = x0; y = y0;
    pts = [x, y];
    while ~(x == x1 && y == y1)
        e2 = 2*err;
        if e2 >= dy
            err = err + dy;
            x = x + sx;
        end
        if e2 <= dx
            err = err + dx;
            y = y + sy;
        end
        pts = [pts; x, y]; %#ok<AGROW>
    end
end

function y = clip(x, lo, hi)
    y = min(max(x, lo), hi);
end

      

File: Chapter20_Lesson3.nb

The Mathematica notebook derives Jacobians symbolically and demonstrates log-odds mapping and NEES computation. This is especially useful for checking derivations before coding them in Python/C++/MATLAB.

(* Chapter20_Lesson3.nb *)
Notebook[{
  Cell["Localization + Mapping Integration (Capstone AMR)", "Title"],
  Cell["Chapter20_Lesson3.nb", "Section"],
  Cell[TextData[{
    "This notebook derives Jacobians used in EKF localization and demonstrates ",
    "log-odds occupancy updates symbolically and numerically."
  }], "Text"],

  Cell[BoxData[
    ToBoxes[
      Clear[x, y, th, v, w, dt];
      f = {
        x + v Cos[th] dt,
        y + v Sin[th] dt,
        th + w dt
      };
      F = D[f, { {x, y, th} }] // Simplify;
      G = D[f, { {v, w} }] // Simplify;
      {f, F, G}
    ]
  ], "Input"],

  Cell["Landmark range-bearing measurement model Jacobian", "Subsection"],
  Cell[BoxData[
    ToBoxes[
      Clear[lx, ly];
      h = {
        Sqrt[(lx - x)^2 + (ly - y)^2],
        ArcTan[lx - x, ly - y] - th
      };
      H = D[h, { {x, y, th} }] // FullSimplify;
      H
    ]
  ], "Input"],

  Cell["Joseph covariance update preserves symmetry/PSD structure", "Subsection"],
  Cell[BoxData[
    ToBoxes[
      Clear[P, K, Hm, R];
      (* Symbolic statement of Joseph form *)
      PJoseph = (IdentityMatrix[3] - K.Hm).P.Transpose[IdentityMatrix[3] - K.Hm] + K.R.Transpose[K];
      PJoseph
    ]
  ], "Input"],

  Cell["Log-odds occupancy update demo", "Subsection"],
  Cell[BoxData[
    ToBoxes[
      lPrior = ConstantArray[0., {20, 20}];
      lOcc = 0.85; lFree = -0.40;
      ray = { {4, 4}, {5, 4}, {6, 4}, {7, 4}, {8, 4}, {9, 4} };
      lPost = lPrior;
      Do[lPost[[ray[[i, 2]], ray[[i, 1]]]] += lFree, {i, 1, Length[ray] - 1}];
      lPost[[ray[[-1, 2]], ray[[-1, 1]]]] += lOcc;
      pOcc = 1/(1 + Exp[-lPost]);
      MatrixPlot[pOcc, PlotLabel -> "Occupancy Probability", Frame -> True]
    ]
  ], "Input"],

  Cell["Simple consistency metric computation (NEES) for 2D pose", "Subsection"],
  Cell[BoxData[
    ToBoxes[
      e = {0.12, -0.08, 0.03};
      Pnum = { {0.05, 0.00, 0.00}, {0.00, 0.05, 0.00}, {0.00, 0.00, 0.02} };
      nees = e . Inverse[Pnum] . e // N;
      nees
    ]
  ], "Input"],

  Cell["Notes", "Subsection"],
  Cell[TextData[{
    "For a full capstone implementation, connect this notebook's symbolic Jacobians ",
    "to a ROS2 or MATLAB/Simulink online pipeline, and use submaps plus loop-closure ",
    "constraints for long-horizon drift control."
  }], "Text"]
},
WindowSize->{1100, 800},
WindowMargins->{ {Automatic, 100}, {Automatic, 50} },
StyleDefinitions->"Default.nb"]

      

Problem 1 (Pose-to-map error propagation): Let a LiDAR endpoint in world coordinates be \( \mathbf{p}(\mathbf{x}_k,z_k) \). Show that for small pose error \( \delta\mathbf{x}_k \), endpoint covariance is approximately \( \mathbf{\Sigma}_{\text{end} } \approx \mathbf{J}_{\text{ray} }\mathbf{P}_k\mathbf{J}_{\text{ray} }^T \).

Solution: Apply first-order linearization around the estimated pose:

\[ \mathbf{p}(\mathbf{x}_k,z_k) \approx \mathbf{p}(\hat{\mathbf{x} }_k,z_k) + \mathbf{J}_{\text{ray} }(\hat{\mathbf{x} }_k,z_k)(\mathbf{x}_k-\hat{\mathbf{x} }_k) \]

Therefore the endpoint error is approximately linear in the pose error. Taking covariance on both sides yields the result. This explains why large localization covariance creates spatially blurred occupancy boundaries.

Problem 2 (Joseph form positivity): Prove that the Joseph covariance update remains positive semidefinite if \( \mathbf{P}_{k\mid k-1}\succeq 0 \) and \( \mathbf{R}_k \succeq 0 \).

Solution: For any nonzero vector \( \mathbf{a} \),

\[ \mathbf{a}^T\mathbf{P}_{k\mid k}\mathbf{a} = \mathbf{a}^T(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}_{k\mid k-1}(\mathbf{I}-\mathbf{K}\mathbf{H})^T\mathbf{a} + \mathbf{a}^T\mathbf{K}\mathbf{R}\mathbf{K}^T\mathbf{a} \]

Each term is a quadratic form in a positive semidefinite matrix, so each term is nonnegative. Their sum is nonnegative, hence \( \mathbf{P}_{k\mid k}\succeq 0 \).

Problem 3 (Log-odds additivity): Suppose a cell receives two independent “free” observations and one “occupied” observation with contributions \( \ell_f \) and \( \ell_o \). Write the final log-odds and occupancy probability.

Solution: With neutral prior log-odds \( L_0=0 \),

\[ L = 2\ell_f + \ell_o \]

and the resulting probability is \( p = (1+\exp(-L))^{-1} \). If \( 2|\ell_f| > \ell_o \), the cell remains biased toward free; otherwise it trends toward occupied.

Problem 4 (Mahalanobis gating threshold): A range-bearing innovation has \( d_M^2 = 7.4 \). Should the update be accepted for a 2D measurement using a 99% gate?

Solution: For measurement dimension 2, a typical 99% threshold is about \( \chi^2_{2,0.99}\approx 9.21 \). Since \( 7.4 < 9.21 \), the innovation passes the gate and the correction is accepted.

Problem 5 (Capstone architecture choice): Your AMR runs in real time on limited compute, and long loops occasionally cause drift. Should you choose a purely global grid or a submap architecture?

Solution: A submap architecture is usually better. Use local submaps for stable online updates and periodic pose-graph optimization for loop closures. This reduces global map rewrites and improves real-time behavior while preserving consistency over long trajectories.

Localization and mapping integration is the core of a full AMR autonomy capstone. The mathematical coupling is explicit: localization uncertainty shapes map quality, and the map supports future localization. In practice, reliable integration depends on (i) stable covariance propagation, (ii) correct gating and data association, (iii) disciplined log-odds updates, and (iv) architecture choices such as submaps and loop-closure optimization. The provided multi-language implementations give a consistent baseline that can be extended into the final capstone project.