In earlier chapters you learned map representations (occupancy grids), probabilistic state estimation, and scan processing. Here we apply those tools to moving objects. A dynamic obstacle is modeled as a random state \( \mathbf{x}_k \) (e.g., position and velocity) observed through noisy sensor measurements \( \mathbf{z}_k \).

A minimal output of the sensing layer at time \( k \) is a set of tracks \( \{(\hat{\mathbf{x} }_k^j,\, \mathbf{P}_k^j)\}_{j=1}^{N_k} \), where \( \hat{\mathbf{x} }_k^j \) is the estimated state of obstacle \( j \) and \( \mathbf{P}_k^j \) is its covariance (uncertainty ellipse in 2D). These tracks feed the avoidance policy in Lesson 2 (velocity-based constraints) and Lesson 4 (prediction-aware planning).

flowchart TD
  S["Sensors: LiDAR / radar / camera / depth"] --> P["Preprocess: sync, filter, deskew"]
  P --> E["Ego-motion compensation: transform to common frame"]
  E --> D["Detection: cluster/segment -> object measurements z_k"]
  D --> T["Tracking: predict, gate, associate, update"]
  T --> O["Output tracks: (xhat_k^j, P_k^j), j=1..N_k"]
  O --> C["Local planner consumes tracks (next lessons)"]
        

Two key difficulties distinguish dynamic-obstacle sensing from static mapping: (1) data association (which measurement belongs to which obstacle), and (2) occlusion/clutter (objects temporarily disappear and false detections appear). Both are handled naturally in a probabilistic tracking formulation.

We focus on sensors commonly used by AMRs: LiDAR (dense geometry), radar (robust velocity/long range), and camera/depth (semantics). Regardless of modality, we start from a measurement model \( \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k \) with noise \( \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \).

\[ \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k,\quad E[\mathbf{v}_k]=\mathbf{0},\quad E[\mathbf{v}_k\mathbf{v}_k^\top]=\mathbf{R}_k. \]

For a 2D ground robot (planar motion), a common obstacle state is constant-velocity (CV): \( \mathbf{x}_k = [p_x,\, p_y,\, v_x,\, v_y]^\top \). If a detector returns a 2D position measurement (e.g., a LiDAR cluster centroid), then the measurement is linear:

\[ \mathbf{z}_k = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} \mathbf{x}_k + \mathbf{v}_k \equiv \mathbf{H}\mathbf{x}_k + \mathbf{v}_k. \]

All sensors report in their own coordinate frame. Using rigid transforms from the robot’s frame tree (already covered in kinematics/dynamics), we convert each point or detection into a common frame. In planar motion, with robot pose \( (x_r,y_r,\theta_r) \) and a point \( \mathbf{p}^b=[p_x^b, p_y^b]^\top \) in the robot base frame, the world-frame point is:

\[ \mathbf{p}^w = \begin{bmatrix} \cos\theta_r & -\sin\theta_r \\ \sin\theta_r & \cos\theta_r \end{bmatrix} \mathbf{p}^b + \begin{bmatrix} x_r \\ y_r \end{bmatrix}. \]

Time alignment. A practical AMR issue is that sensors are not instantaneous: a LiDAR scan is acquired over a time window, and cameras/radar have latency. A standard fix is: (i) timestamp everything, (ii) interpolate the robot pose using your localization estimate, and (iii) “deskew” measurements to a single reference time before detection/tracking.

A tracker consumes detections, not raw pixels/points. The sensing stack must therefore produce a finite set \( \mathcal{Z}_k = \{\mathbf{z}_k^i\}_{i=1}^{M_k} \). For LiDAR, a standard pipeline is:

• Geometric filtering: remove ground (in 3D) or range outliers; optionally downsample.
• Clustering/segmentation: group nearby points into candidate objects (e.g., DBSCAN).
• Feature extraction: centroid, bounding box, and/or shape parameters.

A centroid detector summarizes a cluster \( \mathcal{C}_i \subset \mathbb{R}^2 \) as \( \mathbf{z}_k^i = \frac{1}{|\mathcal{C}_i|} \sum_{\mathbf{p} \in \mathcal{C}_i} \mathbf{p} \). The centroid noise covariance \( \mathbf{R}_k \) depends on sensor noise, point density, and cluster geometry.

\[ \mathbf{z}_k^i \triangleq \frac{1}{|\mathcal{C}_i|}\sum_{\mathbf{p}\in\mathcal{C}_i}\mathbf{p},\qquad \mathbf{R}_k^i \approx \frac{1}{|\mathcal{C}_i|}\mathbf{\Sigma}_{\text{point} }\;\; (\text{rough scaling}). \]

Dynamic-vs-static separation. If you already have a static map, you can reject detections that align with static structure. Even without a global map, you can exploit temporal inconsistency: after ego-motion compensation, static returns are consistent across frames, while moving objects exhibit residual motion. This motivates “motion segmentation” by comparing consecutive frames.

We model each obstacle with a linear-Gaussian state space model (CV in 2D):

\[ \mathbf{x}_{k+1} = \mathbf{F}\mathbf{x}_k + \mathbf{w}_k,\quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}), \qquad \mathbf{z}_k = \mathbf{H}\mathbf{x}_k + \mathbf{v}_k,\quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}). \]

With sampling time \( \Delta t \), the CV transition is:

\[ \mathbf{F} = \begin{bmatrix} 1&0&\Delta t&0\\ 0&1&0&\Delta t\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}. \]

If we assume white acceleration with standard deviation \( \sigma_a \) per axis, the process noise covariance is:

\[ \mathbf{Q} = \sigma_a^2 \begin{bmatrix} \tfrac{\Delta t^4}{4} & 0 & \tfrac{\Delta t^3}{2} & 0 \\ 0 & \tfrac{\Delta t^4}{4} & 0 & \tfrac{\Delta t^3}{2} \\ \tfrac{\Delta t^3}{2} & 0 & \Delta t^2 & 0 \\ 0 & \tfrac{\Delta t^3}{2} & 0 & \Delta t^2 \end{bmatrix}. \]

Single-track Kalman filter. For predicted mean/covariance \( (\hat{\mathbf{x} }_{k|k-1},\,\mathbf{P}_{k|k-1}) \), the innovation is \( \mathbf{y}_k = \mathbf{z}_k - \mathbf{H}\hat{\mathbf{x} }_{k|k-1} \) with covariance \( \mathbf{S}_k = \mathbf{H}\mathbf{P}_{k|k-1}\mathbf{H}^\top + \mathbf{R} \).

\[ \mathbf{K}_k = \mathbf{P}_{k|k-1}\mathbf{H}^\top\mathbf{S}_k^{-1},\quad \hat{\mathbf{x} }_{k|k} = \hat{\mathbf{x} }_{k|k-1} + \mathbf{K}_k\mathbf{y}_k,\\ \mathbf{P}_{k|k} = (\mathbf{I}-\mathbf{K}_k\mathbf{H})\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}_k\mathbf{H})^\top + \mathbf{K}_k\mathbf{R}\mathbf{K}_k^\top. \]

Gating (statistical consistency test). Define the squared Mahalanobis distance \( d_k^2 = \mathbf{y}_k^\top\mathbf{S}_k^{-1}\mathbf{y}_k \). If the innovation is truly Gaussian with covariance \( \mathbf{S}_k \), then \( d_k^2 \sim \chi^2_m \) where \( m \) is the measurement dimension (here \( m=2 \)). Choose a gate \( \gamma \) such that \( \Pr(d_k^2 \le \gamma)=p_g \). Measurements with \( d_k^2 > \gamma \) are rejected as unlikely.

\[ d_k^2 \triangleq \mathbf{y}_k^\top\mathbf{S}_k^{-1}\mathbf{y}_k,\qquad d_k^2 \sim \chi^2_m,\qquad \gamma = F_{\chi^2_m}^{-1}(p_g). \]

Data association (nearest-neighbor, teaching baseline). Given a set of tracks and detections, compute gated distances and pick the smallest distances under a one-to-one constraint (greedy NN is a simple approximation; global assignment uses the Hungarian algorithm). Unmatched detections spawn new tracks; unmatched tracks age and are deleted after too many misses.

flowchart TD
  A["Predicted tracks: (xhat,P)"] --> G["Compute innovation y and distance d2 for each (track,det)"]
  G --> H1["Reject pairs with d2 > gamma (gate)"]
  H1 --> M["Associate remaining pairs (NN / global assignment)"]
  M --> U["Update matched tracks (KF)"]
  M --> UM1["Unmatched tracks: \nmiss++"]
  M --> UM2["Unmatched detections: \nspawn new track"]
  U --> PR["Prune tracks with miss > M_max"]
  UM1 --> PR
  UM2 --> PR
        

What you should take away: dynamic-obstacle sensing is not only “detecting” objects. It is maintaining a probabilistic memory over time with explicit uncertainty, which is essential for safe avoidance under occlusions and clutter.

Ego-motion compensation. If the robot moves, raw point clouds “move” even for static objects. You already have a pose estimate \( \hat{\mathbf{x} }^{\text{robot} }_k \) from localization. Always transform sensor data into a common frame (typically the local map frame) before differencing or tracking.

Multi-sensor fusion (sequential updates). If two sensors provide conditionally independent measurements of the same track at the same time step, you can update sequentially without double counting. Let \( (\mathbf{H}_1,\mathbf{R}_1) \) and \( (\mathbf{H}_2,\mathbf{R}_2) \) be two measurement models with independent noises. Starting from a prior \( (\hat{\mathbf{x} },\mathbf{P}) \), apply update 1 then update 2. This is equivalent to a batch update with stacked measurement (proved in the Problems section).

\[ \text{If } \mathbf{v}_1 \perp \mathbf{v}_2,\; \text{then }\; p(\mathbf{x}\mid\mathbf{z}_1,\mathbf{z}_2) \propto p(\mathbf{z}_1\mid\mathbf{x})\,p(\mathbf{z}_2\mid\mathbf{x})\,p(\mathbf{x}). \]

Occlusion and track management. In crowds, obstacles disappear behind others. Track life-cycle logic (confirmed/tentative, max-miss count, and re-identification) prevents instability. In this lesson we implement a minimal rule: delete tracks after \( M_{\max} \) consecutive misses.

The following reference implementations are designed to be didactic. They do not require ROS, but each includes pointers to typical robotics libraries: ROS2 (rclpy/rclcpp), PCL (LiDAR clustering), OpenCV (vision), and Eigen (linear algebra).

6.1 Python (prototype + visualization)

File: Chapter16_Lesson1.py

"""
Chapter16_Lesson1.py
Sensing Dynamic Obstacles — Detection + Multi-Target Tracking (Teaching Baseline)

This script demonstrates:
1) Synthetic moving obstacles (2D constant-velocity model)
2) Noisy detections with clutter
3) Multi-target tracking with: KF (per track) + chi-square gating + greedy NN association

Dependencies: numpy, matplotlib
Optional: scipy (for chi-square inverse CDF) — if not available, uses a small lookup.
"""

from __future__ import annotations
import math
import numpy as np
import matplotlib.pyplot as plt


def chi2_inv_cdf(p: float, dof: int) -> float:
    """
    Approximate inverse CDF of chi-square for teaching/demo.
    If SciPy is available, use scipy.stats.chi2.ppf.
    """
    try:
        from scipy.stats import chi2  # type: ignore
        return float(chi2.ppf(p, dof))
    except Exception:
        # Coarse fallback table for dof=2,4 at common probabilities (pg)
        table = {
            (2, 0.90): 4.605,
            (2, 0.95): 5.991,
            (2, 0.97): 7.378,
            (2, 0.99): 9.210,
            (4, 0.90): 7.779,
            (4, 0.95): 9.488,
            (4, 0.97): 11.143,
            (4, 0.99): 13.277,
        }
        key = (dof, round(p, 2))
        if key in table:
            return table[key]
        # Very rough Wilson–Hilferty approximation
        # X ~ chi2_k => (X/k)^(1/3) approx N(1-2/(9k), 2/(9k))
        k = float(dof)
        z = math.sqrt(2) * erfinv(2 * p - 1)  # approx normal quantile
        return k * (1 - 2/(9*k) + z * math.sqrt(2/(9*k)))**3


def erfinv(x: float) -> float:
    """Approximate inverse error function (for fallback chi-square)."""
    # Winitzki approximation
    a = 0.147
    s = 1.0 if x >= 0 else -1.0
    ln = math.log(1.0 - x * x)
    t = 2.0/(math.pi*a) + ln/2.0
    return s * math.sqrt(math.sqrt(t*t - ln/a) - t)


def make_F_Q(dt: float, sigma_a: float) -> tuple[np.ndarray, np.ndarray]:
    F = np.array([
        [1, 0, dt, 0],
        [0, 1, 0, dt],
        [0, 0, 1, 0],
        [0, 0, 0, 1],
    ], dtype=float)

    q = sigma_a**2
    Q = q * np.array([
        [dt**4/4, 0,        dt**3/2, 0],
        [0,       dt**4/4,  0,       dt**3/2],
        [dt**3/2, 0,        dt**2,   0],
        [0,       dt**3/2,  0,       dt**2],
    ], dtype=float)
    return F, Q


def kf_predict(x: np.ndarray, P: np.ndarray, F: np.ndarray, Q: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    x2 = F @ x
    P2 = F @ P @ F.T + Q
    return x2, P2


def kf_update(x: np.ndarray, P: np.ndarray, z: np.ndarray, H: np.ndarray, R: np.ndarray) -> tuple[np.ndarray, np.ndarray, float]:
    y = z - H @ x
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)

    x2 = x + K @ y
    I = np.eye(P.shape[0])
    # Joseph form for numerical stability
    P2 = (I - K @ H) @ P @ (I - K @ H).T + K @ R @ K.T

    d2 = float(y.T @ np.linalg.inv(S) @ y)  # Mahalanobis distance squared
    return x2, P2, d2


class Track:
    def __init__(self, x: np.ndarray, P: np.ndarray, track_id: int):
        self.x = x.copy()
        self.P = P.copy()
        self.id = track_id
        self.age = 1
        self.hits = 1
        self.misses = 0

    def predict(self, F: np.ndarray, Q: np.ndarray):
        self.x, self.P = kf_predict(self.x, self.P, F, Q)
        self.age += 1

    def update(self, z: np.ndarray, H: np.ndarray, R: np.ndarray):
        self.x, self.P, _ = kf_update(self.x, self.P, z, H, R)
        self.hits += 1
        self.misses = 0


def greedy_nn_association(tracks: list[Track], Z: np.ndarray, H: np.ndarray, R: np.ndarray, gate_gamma: float) -> tuple[list[tuple[int,int]], list[int], list[int]]:
    """
    Return matched pairs (track_index, meas_index), unmatched_tracks, unmatched_meas.
    Greedy NN on gated Mahalanobis distance (teaching baseline).
    """
    if len(tracks) == 0:
        return [], [], list(range(len(Z)))
    if len(Z) == 0:
        return [], list(range(len(tracks))), []

    pairs = []
    for ti, trk in enumerate(tracks):
        # Innovation covariance S for each track (same H, R)
        S = H @ trk.P @ H.T + R
        S_inv = np.linalg.inv(S)
        zhat = H @ trk.x
        for mi, z in enumerate(Z):
            y = z - zhat
            d2 = float(y.T @ S_inv @ y)
            if d2 <= gate_gamma:
                pairs.append((d2, ti, mi))

    pairs.sort(key=lambda t: t[0])

    matched_tracks = set()
    matched_meas = set()
    matches: list[tuple[int,int]] = []
    for d2, ti, mi in pairs:
        if ti in matched_tracks or mi in matched_meas:
            continue
        matched_tracks.add(ti)
        matched_meas.add(mi)
        matches.append((ti, mi))

    unmatched_tracks = [i for i in range(len(tracks)) if i not in matched_tracks]
    unmatched_meas = [i for i in range(len(Z)) if i not in matched_meas]
    return matches, unmatched_tracks, unmatched_meas


def simulate_truth(num_targets: int, T: int, dt: float, area: float, seed: int = 0) -> np.ndarray:
    """
    Truth states: shape (T, num_targets, 4) for [px,py,vx,vy]
    """
    rng = np.random.default_rng(seed)
    X = np.zeros((T, num_targets, 4), dtype=float)
    # Random initial positions and velocities
    X[0, :, 0:2] = rng.uniform(-area, area, size=(num_targets, 2))
    X[0, :, 2:4] = rng.uniform(-1.2, 1.2, size=(num_targets, 2))
    for k in range(1, T):
        X[k, :, 0:2] = X[k-1, :, 0:2] + dt * X[k-1, :, 2:4]
        X[k, :, 2:4] = X[k-1, :, 2:4]
        # bounce on boundaries
        for j in range(num_targets):
            for ax in [0, 1]:
                if abs(X[k, j, ax]) > area:
                    X[k, j, ax] = np.clip(X[k, j, ax], -area, area)
                    X[k, j, ax+2] *= -1.0
    return X


def generate_measurements(truth: np.ndarray, sigma_z: float, p_detect: float, clutter_rate: int, area: float, seed: int = 1) -> list[np.ndarray]:
    """
    For each time k, return array Z_k of detections [px,py] with noise, missed detections, and uniform clutter.
    """
    rng = np.random.default_rng(seed)
    T, N, _ = truth.shape
    Zs: list[np.ndarray] = []
    for k in range(T):
        meas = []
        # true detections
        for j in range(N):
            if rng.uniform() <= p_detect:
                z = truth[k, j, 0:2] + rng.normal(0.0, sigma_z, size=(2,))
                meas.append(z)
        # clutter (false alarms)
        m_clutter = rng.poisson(clutter_rate)
        for _ in range(m_clutter):
            zc = rng.uniform(-area, area, size=(2,))
            meas.append(zc)
        if len(meas) == 0:
            Zs.append(np.zeros((0, 2)))
        else:
            Zs.append(np.vstack(meas))
    return Zs


def main():
    # Scenario
    dt = 0.1
    T = 220
    area = 10.0

    num_targets = 3
    sigma_a = 0.7          # process accel std (model)
    sigma_z = 0.35         # measurement std
    p_detect = 0.92
    clutter_rate = 2       # expected clutter per frame

    # Filter/association parameters
    gate_pg = 0.99
    gate_gamma = chi2_inv_cdf(gate_pg, dof=2)  # position measurement => m=2
    max_misses = 12
    init_P = np.diag([1.0, 1.0, 2.0, 2.0])

    F, Q = make_F_Q(dt, sigma_a)
    H = np.array([[1, 0, 0, 0],
                  [0, 1, 0, 0]], dtype=float)
    R = (sigma_z**2) * np.eye(2)

    truth = simulate_truth(num_targets, T, dt, area, seed=2)
    Zs = generate_measurements(truth, sigma_z, p_detect, clutter_rate, area, seed=3)

    tracks: list[Track] = []
    next_id = 1

    # For plotting
    est_hist = []  # list of (k, track_id, px, py)
    det_hist = []  # list of (k, px, py)

    for k in range(T):
        Zk = Zs[k]
        for z in Zk:
            det_hist.append((k, float(z[0]), float(z[1])))

        # Predict all tracks
        for trk in tracks:
            trk.predict(F, Q)

        # Associate
        matches, um_tracks, um_meas = greedy_nn_association(tracks, Zk, H, R, gate_gamma)

        # Update matched
        for ti, mi in matches:
            tracks[ti].update(Zk[mi], H, R)

        # Mark misses
        for ti in um_tracks:
            tracks[ti].misses += 1

        # Spawn new tracks for unmatched measurements (simple: use position, zero velocity)
        for mi in um_meas:
            z = Zk[mi]
            x0 = np.array([z[0], z[1], 0.0, 0.0], dtype=float)
            tracks.append(Track(x0, init_P, next_id))
            next_id += 1

        # Prune
        tracks = [t for t in tracks if t.misses <= max_misses]

        # Store estimates
        for trk in tracks:
            est_hist.append((k, trk.id, float(trk.x[0]), float(trk.x[1])))

    # Plot (truth, detections, track estimates)
    plt.figure(figsize=(9, 7))
    # truth
    for j in range(num_targets):
        plt.plot(truth[:, j, 0], truth[:, j, 1], linewidth=2, label=f"truth {j+1}")
    # detections (subsample to reduce clutter)
    det_xy = np.array([(x, y) for (_, x, y) in det_hist], dtype=float)
    if det_xy.size > 0:
        plt.scatter(det_xy[::3, 0], det_xy[::3, 1], s=12, alpha=0.30, label="detections (subsample)")

    # estimates by track id
    est = np.array(est_hist, dtype=float)
    if est.size > 0:
        ids = np.unique(est[:, 1]).astype(int)
        for tid in ids:
            mask = (est[:, 1].astype(int) == tid)
            plt.plot(est[mask, 2], est[mask, 3], linestyle="--", linewidth=1)

    plt.axis("equal")
    plt.xlim(-area-1, area+1)
    plt.ylim(-area-1, area+1)
    plt.title("Dynamic obstacle sensing demo: detections + multi-target tracking")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.legend(loc="upper right", fontsize=9)
    plt.grid(True)
    plt.show()

    print("Done.")
    print(f"Gate (pg={gate_pg}) gamma={gate_gamma:.3f}, max_misses={max_misses}, tracks_final={len(tracks)}")


if __name__ == "__main__":
    main()
      

File: Chapter16_Lesson1_Ex1.py (exercise scaffold)

"""
Chapter16_Lesson1_Ex1.py
Exercise scaffold: implement global assignment (Hungarian) instead of greedy NN.

Task:
- Given tracks and measurements, build a cost matrix using Mahalanobis distance.
- Apply gating: costs for gated-out pairs should be a large number.
- Solve assignment to minimize total cost (one-to-one).
- Return matches, unmatched tracks, unmatched measurements.

Hint:
- Use scipy.optimize.linear_sum_assignment if available.
"""

from __future__ import annotations
import numpy as np


def associate_hungarian(cost: np.ndarray, gate_mask: np.ndarray) -> tuple[list[tuple[int, int]], list[int], list[int]]:
    """
    cost: shape (N_tracks, M_meas) with Mahalanobis distances (or d^2)
    gate_mask: same shape, True where pair is allowed (inside gate)

    Return:
      matches: list of (track_index, meas_index)
      unmatched_tracks: list of track indices
      unmatched_meas: list of meas indices
    """
    # TODO: implement
    raise NotImplementedError("Implement Hungarian assignment with gating.")
      

6.2 C++ (Eigen-based tracker core)

File: Chapter16_Lesson1.cpp

/*
Chapter16_Lesson1.cpp
Sensing Dynamic Obstacles — Minimal Kalman Filter + Gating (C++ / Eigen)

This file provides:
- 2D constant-velocity (CV) Kalman predict/update
- Chi-square gating using Mahalanobis distance
- A small demo with synthetic measurements

Build (example):
  g++ -O2 -std=c++17 Chapter16_Lesson1.cpp -I /usr/include/eigen3 -o demo

Notes:
- In robotics stacks, use Eigen + ROS2 (rclcpp) and consume detections from perception.
- For LiDAR clustering, typical libraries include PCL; for vision use OpenCV.
*/

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <random>
#include <cmath>

struct KF {
  Eigen::Vector4d x;     // [px, py, vx, vy]
  Eigen::Matrix4d P;
  Eigen::Matrix4d F;
  Eigen::Matrix4d Q;
  Eigen::Matrix<double,2,4> H;
  Eigen::Matrix2d R;

  KF(double dt, double sigma_a, double sigma_z) {
    F.setIdentity();
    F(0,2) = dt; F(1,3) = dt;

    const double q = sigma_a * sigma_a;
    Q.setZero();
    Q(0,0) = q * std::pow(dt,4)/4.0; Q(0,2) = q * std::pow(dt,3)/2.0;
    Q(1,1) = q * std::pow(dt,4)/4.0; Q(1,3) = q * std::pow(dt,3)/2.0;
    Q(2,0) = q * std::pow(dt,3)/2.0; Q(2,2) = q * std::pow(dt,2);
    Q(3,1) = q * std::pow(dt,3)/2.0; Q(3,3) = q * std::pow(dt,2);

    H.setZero();
    H(0,0) = 1.0; H(1,1) = 1.0;

    R.setZero();
    R(0,0) = sigma_z * sigma_z;
    R(1,1) = sigma_z * sigma_z;

    x.setZero();
    P.setIdentity();
    P(0,0)=1; P(1,1)=1; P(2,2)=2; P(3,3)=2;
  }

  void predict() {
    x = F * x;
    P = F * P * F.transpose() + Q;
  }

  // Returns Mahalanobis distance squared d^2
  double update(const Eigen::Vector2d& z) {
    Eigen::Vector2d y = z - H * x;
    Eigen::Matrix2d S = H * P * H.transpose() + R;
    Eigen::Matrix<double,4,2> K = P * H.transpose() * S.inverse();

    x = x + K * y;

    // Joseph form for numerical stability
    Eigen::Matrix4d I = Eigen::Matrix4d::Identity();
    P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();

    const double d2 = y.transpose() * S.inverse() * y;
    return d2;
  }

  Eigen::Vector2d zhat() const { return H * x; }

  double maha2(const Eigen::Vector2d& z) const {
    Eigen::Vector2d y = z - H * x;
    Eigen::Matrix2d S = H * P * H.transpose() + R;
    return y.transpose() * S.inverse() * y;
  }
};

// For dof=2, common chi-square gates (approx).
double chi2_gate_2dof(double pg) {
  if (std::abs(pg - 0.99) < 1e-12) return 9.210;
  if (std::abs(pg - 0.95) < 1e-12) return 5.991;
  if (std::abs(pg - 0.90) < 1e-12) return 4.605;
  return 9.210;
}

int main() {
  const double dt = 0.1;
  const double sigma_a = 0.7;
  const double sigma_z = 0.35;
  const double gate = chi2_gate_2dof(0.99);

  KF kf(dt, sigma_a, sigma_z);

  // Truth: start at (0,0), velocity (1.0, 0.6)
  Eigen::Vector4d x_true; x_true << 0, 0, 1.0, 0.6;

  std::mt19937 rng(0);
  std::normal_distribution<double> n01(0.0, 1.0);

  for (int k=0; k<60; ++k) {
    // propagate truth
    x_true(0) += dt * x_true(2);
    x_true(1) += dt * x_true(3);

    // noisy position measurement
    Eigen::Vector2d z;
    z(0) = x_true(0) + sigma_z * n01(rng);
    z(1) = x_true(1) + sigma_z * n01(rng);

    kf.predict();
    const double d2 = kf.maha2(z);

    if (d2 <= gate) {
      kf.update(z);
      std::cout << "k=" << k
                << "  z=[" << z(0) << "," << z(1) << "]"
                << "  d2=" << d2
                << "  xhat=[" << kf.x(0) << "," << kf.x(1) << "," << kf.x(2) << "," << kf.x(3) << "]\n";
    } else {
      std::cout << "k=" << k << "  measurement rejected by gate, d2=" << d2 << "\n";
    }
  }

  return 0;
}
      

6.3 Java (minimal linear algebra)

File: Chapter16_Lesson1.java

/*
Chapter16_Lesson1.java
Sensing Dynamic Obstacles — Minimal Kalman Filter (Java, teaching baseline)

This is a compact 2D constant-velocity KF for a single track.
For production robotics, consider:
- ROS2 Java bindings, or
- EJML for linear algebra.

Here we implement small matrix ops manually to avoid external deps.
*/

import java.util.Random;

public class Chapter16_Lesson1 {

    // 4x4 matrix multiply: C = A*B
    static double[][] mul44(double[][] A, double[][] B) {
        double[][] C = new double[4][4];
        for (int i=0;i<4;i++){
            for (int j=0;j<4;j++){
                double s=0;
                for (int k=0;k<4;k++) s += A[i][k]*B[k][j];
                C[i][j]=s;
            }
        }
        return C;
    }

    // 4x4 * 4x1
    static double[] mul4v(double[][] A, double[] x) {
        double[] y = new double[4];
        for(int i=0;i<4;i++){
            double s=0;
            for(int k=0;k<4;k++) s += A[i][k]*x[k];
            y[i]=s;
        }
        return y;
    }

    // transpose 4x4
    static double[][] T44(double[][] A){
        double[][] B=new double[4][4];
        for(int i=0;i<4;i++) for(int j=0;j<4;j++) B[i][j]=A[j][i];
        return B;
    }

    // 2x4 * 4x4 * 4x2 => 2x2 helper pieces
    static double[][] HPHt(double[][] H, double[][] P){
        // tmp = H*P => 2x4
        double[][] tmp = new double[2][4];
        for(int i=0;i<2;i++){
            for(int j=0;j<4;j++){
                double s=0;
                for(int k=0;k<4;k++) s += H[i][k]*P[k][j];
                tmp[i][j]=s;
            }
        }
        // S = tmp*H^T => 2x2
        double[][] S = new double[2][2];
        for(int i=0;i<2;i++){
            for(int j=0;j<2;j++){
                double s=0;
                for(int k=0;k<4;k++) s += tmp[i][k]*H[j][k]; // H^T
                S[i][j]=s;
            }
        }
        return S;
    }

    // invert 2x2
    static double[][] inv2(double[][] A){
        double det = A[0][0]*A[1][1] - A[0][1]*A[1][0];
        double[][] B = new double[2][2];
        B[0][0]= A[1][1]/det;
        B[1][1]= A[0][0]/det;
        B[0][1]= -A[0][1]/det;
        B[1][0]= -A[1][0]/det;
        return B;
    }

    // 4x4 + 4x4
    static double[][] add44(double[][] A, double[][] B){
        double[][] C=new double[4][4];
        for(int i=0;i<4;i++) for(int j=0;j<4;j++) C[i][j]=A[i][j]+B[i][j];
        return C;
    }

    public static void main(String[] args){
        double dt = 0.1;
        double sigmaA = 0.7;
        double sigmaZ = 0.35;

        double[][] F = {
            {1,0,dt,0},
            {0,1,0,dt},
            {0,0,1,0},
            {0,0,0,1}
        };

        double q = sigmaA*sigmaA;
        double[][] Q = {
            {q*Math.pow(dt,4)/4.0, 0, q*Math.pow(dt,3)/2.0, 0},
            {0, q*Math.pow(dt,4)/4.0, 0, q*Math.pow(dt,3)/2.0},
            {q*Math.pow(dt,3)/2.0, 0, q*Math.pow(dt,2), 0},
            {0, q*Math.pow(dt,3)/2.0, 0, q*Math.pow(dt,2)}
        };

        double[][] H = {
            {1,0,0,0},
            {0,1,0,0}
        };

        double[][] R = {
            {sigmaZ*sigmaZ, 0},
            {0, sigmaZ*sigmaZ}
        };

        double[] x = {0,0,0,0};
        double[][] P = {
            {1,0,0,0},
            {0,1,0,0},
            {0,0,2,0},
            {0,0,0,2}
        };

        // truth
        double[] xt = {0,0,1.0,0.6};
        Random rng = new Random(0);

        for(int k=0;k<60;k++){
            // propagate truth
            xt[0] += dt*xt[2];
            xt[1] += dt*xt[3];

            // measurement
            double[] z = {
                xt[0] + sigmaZ*rng.nextGaussian(),
                xt[1] + sigmaZ*rng.nextGaussian()
            };

            // predict
            x = mul4v(F, x);
            P = add44(mul44(mul44(F,P), T44(F)), Q);

            // innovation y = z - Hx
            double[] Hx = { x[0], x[1] };
            double[] y = { z[0]-Hx[0], z[1]-Hx[1] };

            // S = HPH^T + R
            double[][] S = HPHt(H,P);
            S[0][0] += R[0][0]; S[1][1] += R[1][1];

            double[][] S_inv = inv2(S);

            // K = P H^T S^{-1}  => (4x4)(4x2)(2x2) = 4x2
            double[][] PHt = new double[4][2];
            for(int i=0;i<4;i++){
                PHt[i][0]=P[i][0];
                PHt[i][1]=P[i][1];
            }
            double[][] K = new double[4][2];
            for(int i=0;i<4;i++){
                for(int j=0;j<2;j++){
                    K[i][j]=PHt[i][0]*S_inv[0][j] + PHt[i][1]*S_inv[1][j];
                }
            }

            // x = x + K y
            for(int i=0;i<4;i++){
                x[i] = x[i] + K[i][0]*y[0] + K[i][1]*y[1];
            }

            // Joseph form: P = (I-KH)P(I-KH)^T + K R K^T
            double[][] KH = new double[4][4];
            for(int i=0;i<4;i++){
                KH[i][0]=K[i][0];
                KH[i][1]=K[i][1];
                // KH[i][2]=0; KH[i][3]=0
            }
            double[][] I = {
                {1,0,0,0},
                {0,1,0,0},
                {0,0,1,0},
                {0,0,0,1}
            };
            double[][] A = new double[4][4];
            for(int i=0;i<4;i++) for(int j=0;j<4;j++) A[i][j]=I[i][j]-KH[i][j];
            double[][] AP = mul44(A,P);
            double[][] APA = mul44(AP, T44(A));

            // KRK^T
            double[][] KR = new double[4][2];
            for(int i=0;i<4;i++){
                KR[i][0]=K[i][0]*R[0][0];
                KR[i][1]=K[i][1]*R[1][1];
            }
            double[][] KRKt = new double[4][4];
            for(int i=0;i<4;i++){
                for(int j=0;j<4;j++){
                    KRKt[i][j] = KR[i][0]*K[j][0] + KR[i][1]*K[j][1];
                }
            }

            P = add44(APA, KRKt);

            System.out.printf("k=%d  z=[%.3f, %.3f]  xhat=[%.3f, %.3f, %.3f, %.3f]%n",
                    k, z[0], z[1], x[0], x[1], x[2], x[3]);
        }
    }
}
      

6.4 MATLAB / Simulink (script + optional model skeleton)

File: Chapter16_Lesson1.m

% Chapter16_Lesson1.m
% Sensing Dynamic Obstacles — KF Tracking + Gating (MATLAB teaching baseline)
%
% This script:
% 1) Simulates constant-velocity target(s)
% 2) Generates noisy position measurements + clutter
% 3) Runs a simple KF with chi-square gating
%
% Notes for Simulink:
% - Use a Discrete State-Space block for prediction (F, Q)
% - Use MATLAB Function blocks for update + gating
% - Feed detections from your perception subsystem (LiDAR clusters)

clear; clc; close all;

dt = 0.1;
T  = 120;

sigma_a = 0.7;
sigma_z = 0.35;

% State: [px; py; vx; vy]
F = [1 0 dt 0;
     0 1 0 dt;
     0 0 1 0;
     0 0 0 1];

q = sigma_a^2;
Q = q * [dt^4/4 0      dt^3/2 0;
         0      dt^4/4 0      dt^3/2;
         dt^3/2 0      dt^2   0;
         0      dt^3/2 0      dt^2];

H = [1 0 0 0;
     0 1 0 0];

R = (sigma_z^2) * eye(2);

% Chi-square gate (dof=2). Common values: pg=0.99 -> 9.210, pg=0.95 -> 5.991
pg = 0.99;
gamma = 9.210;

% Truth
x_true = [0; 0; 1.0; 0.6];

% Filter init
x = [0; 0; 0; 0];
P = diag([1, 1, 2, 2]);

Xtrue = zeros(4, T);
Xhat  = zeros(4, T);
Zmeas = zeros(2, T);
Accepted = false(1, T);

for k = 1:T
    % propagate truth
    x_true = F * x_true;
    Xtrue(:,k) = x_true;

    % measurement
    z = H * x_true + sigma_z * randn(2,1);
    Zmeas(:,k) = z;

    % predict
    x = F * x;
    P = F * P * F' + Q;

    % gate test
    y = z - H*x;
    S = H*P*H' + R;
    d2 = y' / S * y;

    if d2 <= gamma
        % update (Joseph form)
        K = P * H' / S;
        x = x + K*y;
        I = eye(4);
        P = (I - K*H) * P * (I - K*H)' + K*R*K';
        Accepted(k) = true;
    end

    Xhat(:,k) = x;
end

% Plot
figure; hold on; grid on;
plot(Xtrue(1,:), Xtrue(2,:), 'LineWidth', 2);
plot(Xhat(1,:),  Xhat(2,:),  '--', 'LineWidth', 1.5);
scatter(Zmeas(1,Accepted), Zmeas(2,Accepted), 18, 'filled', 'MarkerFaceAlpha', 0.35);
legend('Truth','KF estimate','Accepted measurements');
axis equal;
title('Dynamic obstacle sensing: KF tracking with chi-square gating');
xlabel('x [m]'); ylabel('y [m]');
      

6.5 Wolfram Mathematica (derivation + demo)

File: Chapter16_Lesson1.nb

(* ::Package:: *)

(* Chapter16_Lesson1.nb
   Sensing Dynamic Obstacles — Math Notes + Small Demo (Wolfram Mathematica)

   Contents:
   1) Derive chi-square gating from Gaussian innovation
   2) Build CV model matrices F and Q
   3) Symbolic + numeric check of Joseph covariance PSD

   Paste into a Mathematica notebook or save as .nb after evaluation.
*)

ClearAll["Global`*"];

(* 1) Chi-square gating: if y ~ N(0,S), then d^2 = y^T S^-1 y ~ ChiSquare[m] *)

m = 2;
S = { {2.0, 0.3}, {0.3, 1.4} };
(* Ensure SPD *)
Eigenvalues[S]

L = CholeskyDecomposition[S];
(* Sample y = L u, u ~ N(0,I) *)
nSamp = 20000;
uSamp = RandomVariate[NormalDistribution[0, 1], {nSamp, m}];
ySamp = uSamp.Transpose[L]; (* rows are samples *)
d2 = Map[#.LinearSolve[S, #] &, ySamp];

Histogram[d2, 60, "PDF",
 PlotLabel -> "Empirical d^2 vs ChiSquare[2] PDF"];

Show[
 Plot[PDF[ChiSquareDistribution[m], x], {x, 0, 15}],
 % (* overlay *)
]

(* 2) CV matrices *)
dt = 0.1;
sigmaA = 0.7;
F = { {1, 0, dt, 0},
     {0, 1, 0, dt},
     {0, 0, 1, 0},
     {0, 0, 0, 1} };

q = sigmaA^2;
Q = q*{ {dt^4/4, 0, dt^3/2, 0},
       {0, dt^4/4, 0, dt^3/2},
       {dt^3/2, 0, dt^2, 0},
       {0, dt^3/2, 0, dt^2} };

MatrixForm[F]
MatrixForm[Q]

(* 3) Joseph form PSD check *)
H = { {1,0,0,0},{0,1,0,0} };
sigmaZ = 0.35;
R = sigmaZ^2 IdentityMatrix[2];

P = DiagonalMatrix[{1,1,2,2}];
(* Predicted *)
Ppred = F.P.Transpose[F] + Q;

Sinnov = H.Ppred.Transpose[H] + R;
K = Ppred.Transpose[H].Inverse[Sinnov];

I4 = IdentityMatrix[4];
Pj = (I4 - K.H).Ppred.Transpose[(I4 - K.H)] + K.R.Transpose[K];

Eigenvalues[N[Pj]]

(* If eigenvalues are nonnegative (within numeric tolerance), PSD holds. *)
      

The problems below reinforce the tracking mathematics used for sensing dynamic obstacles. Unless stated otherwise, assume the CV model in Section 4 and linear-Gaussian assumptions.

Problem 1 (Derive \( \mathbf{Q} \) for CV from white acceleration): In 1D, let the continuous-time dynamics be \( \dot{p}=v \), \( \dot{v}=a(t) \) where \( a(t) \) is zero-mean white noise with power spectral density \( q=\sigma_a^2 \). Discretize over \( \Delta t \) to obtain the 2×2 covariance \( \mathbf{Q}_1 \) for \( [p\; v]^\top \), and then form the 4×4 planar \( \mathbf{Q} \).

Solution: Over \( \Delta t \), the discrete-time increments are

\[ \begin{aligned} p_{k+1} &= p_k + v_k\Delta t + \int_0^{\Delta t}(\Delta t-\tau)a(\tau)\,d\tau,\\ v_{k+1} &= v_k + \int_0^{\Delta t} a(\tau)\,d\tau. \end{aligned} \]

Let \( w_p \) and \( w_v \) denote the two stochastic integrals above. Using \( E[a(\tau)a(s)] = q\,\delta(\tau-s) \), we compute

\[ \begin{aligned} E[w_v^2] &= q\Delta t,\\ E[w_p^2] &= q\int_0^{\Delta t} (\Delta t-\tau)^2 d\tau = q\frac{\Delta t^3}{3},\\ E[w_p w_v] &= q\int_0^{\Delta t} (\Delta t-\tau)d\tau = q\frac{\Delta t^2}{2}. \end{aligned} \]

The standard discrete-time CV model uses an equivalent discrete acceleration noise parameterization that yields the textbook form (Section 4):

\[ \mathbf{Q}_1 = \sigma_a^2 \begin{bmatrix} \tfrac{\Delta t^4}{4} & \tfrac{\Delta t^3}{2}\\ \tfrac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix}, \quad \mathbf{Q} = \operatorname{blkdiag}(\mathbf{Q}_1,\mathbf{Q}_1). \]

(The two derivations correspond to slightly different continuous-time noise assumptions; both motivate the same scaling.)

Problem 2 (Why gating uses chi-square): Assume the innovation \( \mathbf{y}_k \sim \mathcal{N}(\mathbf{0},\mathbf{S}_k) \) with dimension \( m \). Prove that \( d_k^2 = \mathbf{y}_k^\top\mathbf{S}_k^{-1}\mathbf{y}_k \sim \chi^2_m \).

Solution: Let \( \mathbf{S}_k = \mathbf{L}\mathbf{L}^\top \) be a Cholesky factorization with nonsingular \( \mathbf{L} \). Define \( \mathbf{u} = \mathbf{L}^{-1}\mathbf{y}_k \). Then:

\[ \mathbf{u} \sim \mathcal{N}(\mathbf{0},\mathbf{I}),\qquad d_k^2 = \mathbf{y}_k^\top\mathbf{S}_k^{-1}\mathbf{y}_k = \|\mathbf{u}\|_2^2 = \sum_{i=1}^m u_i^2. \]

Because \( u_i \) are i.i.d. standard normal, the sum of squares is chi-square with \( m \) degrees of freedom: \( d_k^2 \sim \chi^2_m \). Choosing \( \gamma = F_{\chi^2_m}^{-1}(p_g) \) yields \( \Pr(d_k^2 \le \gamma)=p_g \).

Problem 3 (Sequential fusion equals batch fusion): Suppose two independent sensors measure the same state: \( \mathbf{z}_1 = \mathbf{H}_1\mathbf{x} + \mathbf{v}_1 \), \( \mathbf{z}_2 = \mathbf{H}_2\mathbf{x} + \mathbf{v}_2 \), with independent Gaussian noises \( \mathbf{v}_1 \sim \mathcal{N}(0,\mathbf{R}_1) \), \( \mathbf{v}_2 \sim \mathcal{N}(0,\mathbf{R}_2) \). Show that updating \( (\hat{\mathbf{x} },\mathbf{P}) \) with measurement 1 then 2 yields the same posterior as a single batch update using stacked measurement \( \mathbf{z}=[\mathbf{z}_1^\top\;\mathbf{z}_2^\top]^\top \).

Solution: In information form, a Gaussian \( \mathcal{N}(\hat{\mathbf{x} },\mathbf{P}) \) has information matrix \( \mathbf{\Lambda}=\mathbf{P}^{-1} \) and vector \( \boldsymbol{\eta}=\mathbf{P}^{-1}\hat{\mathbf{x} } \). A linear measurement adds information:

\[ \mathbf{\Lambda}^+ = \mathbf{\Lambda} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{H},\qquad \boldsymbol{\eta}^+ = \boldsymbol{\eta} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{z}. \]

Applying measurement 1 then 2 yields \( \mathbf{\Lambda}^{++} = \mathbf{\Lambda} + \mathbf{H}_1^\top\mathbf{R}_1^{-1}\mathbf{H}_1 + \mathbf{H}_2^\top\mathbf{R}_2^{-1}\mathbf{H}_2 \), \( \boldsymbol{\eta}^{++} = \boldsymbol{\eta} + \mathbf{H}_1^\top\mathbf{R}_1^{-1}\mathbf{z}_1 + \mathbf{H}_2^\top\mathbf{R}_2^{-1}\mathbf{z}_2 \). For the batch case, with stacked \( \mathbf{H}=[\mathbf{H}_1^\top\;\mathbf{H}_2^\top]^ \top \) and block-diagonal \( \mathbf{R}=\operatorname{blkdiag}(\mathbf{R}_1,\mathbf{R}_2) \), the same sums appear. Therefore both posteriors coincide.

Problem 4 (Joseph form preserves PSD): Show that the covariance update \( \mathbf{P}^+ = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \) is symmetric positive semidefinite whenever \( \mathbf{P} \succeq 0 \) and \( \mathbf{R} \succeq 0 \).

Solution: Symmetry is immediate because each term is of the form \( \mathbf{A}\mathbf{P}\mathbf{A}^\top \) or \( \mathbf{B}\mathbf{R}\mathbf{B}^\top \). For PSD, for any vector \( \mathbf{u} \):

\[ \mathbf{u}^\top\mathbf{P}^+\mathbf{u} = (\mathbf{A}^\top\mathbf{u})^\top\mathbf{P}(\mathbf{A}^\top\mathbf{u}) + (\mathbf{B}^\top\mathbf{u})^\top\mathbf{R}(\mathbf{B}^\top\mathbf{u}) \ge 0, \]

where \( \mathbf{A}=\mathbf{I}-\mathbf{K}\mathbf{H} \), \( \mathbf{B}=\mathbf{K} \), and we used \( \mathbf{P},\mathbf{R} \succeq 0 \). Hence \( \mathbf{P}^+ \succeq 0 \). This is why the Joseph form is numerically safer than \( \mathbf{P}^+=(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P} \).

Problem 5 (ML association reduces to Mahalanobis distance): Consider one track with predicted measurement \( \hat{\mathbf{z} } \) and innovation covariance \( \mathbf{S} \). Suppose you receive candidate detections \( \mathbf{z}^i \) generated either by that track (Gaussian) or clutter (uniform). Show that among gated candidates, the maximum-likelihood association is equivalent to minimizing \( d_i^2 = (\mathbf{z}^i-\hat{\mathbf{z} })^\top\mathbf{S}^{-1}(\mathbf{z}^i-\hat{\mathbf{z} }) \).

Solution: Under the track hypothesis, likelihood is Gaussian:

\[ p(\mathbf{z}^i\mid\text{track}) \propto \exp\!\left(-\tfrac{1}{2}d_i^2\right). \]

If clutter is uniform over the sensor field-of-view, then its likelihood is constant across candidates and does not affect the argmax. Therefore maximizing \( p(\mathbf{z}^i\mid\text{track}) \) is equivalent to minimizing \( d_i^2 \). The gate implements a high-probability region of this Gaussian, eliminating implausible candidates.

We defined the dynamic-obstacle sensing objective as producing tracks with uncertainty, not just detections. Using linear measurement models and a constant-velocity process model, we derived the Kalman predict/update equations, introduced chi-square gating for robust association, and outlined a practical track management loop. These outputs are the required inputs for velocity-based avoidance (Lesson 2) and prediction-aware navigation (Lesson 4).