In the capstone setting, the hardware pool is already fixed by procurement (e.g., wheel encoders, IMU, 2D LiDAR, camera, depth camera, GNSS/RTK). The engineering question is not “which sensor exists in the market,” but “which subset and fusion arrangement should be activated, calibrated, and trusted for the target environment introduced in Lesson 1.”

Let the state for a planar AMR be \( \mathbf{x}_k = [x_k,\; y_k,\; \theta_k,\; v_k,\; b_{g,k}]^T \), where \( b_{g,k} \) is a gyro bias term. We seek a sensor subset \( \mathcal{S} \) that minimizes estimation uncertainty and integration risk while respecting system-level budgets.

flowchart TD
  A["Given hardware inventory"] --> B["Define mission and environment constraints"]
  B --> C["Model each sensor: H, R, rate, latency, power"]
  C --> D["Check observability and complementarity"]
  D --> E["Evaluate compute, bandwidth, synchronization"]
  E --> F["Optimize feasible subsets"]
  F --> G["Select suite + placement + calibration plan"]
  G --> H["Integration test criteria for Lesson 3/4"]
        

Suppose the available sensor set is indexed by \( i = 1,\dots,m \). Define binary decision variables \( z_i \in \{0,1\} \), where \( z_i = 1 \) means sensor \( i \) is included in the active capstone stack. A standard constrained formulation is:

\[ \max_{\mathbf{z}\in\{0,1\}^m} \; J(\mathbf{z}) = w_I I(\mathbf{z}) - w_P P(\mathbf{z}) - w_B B(\mathbf{z}) - w_C C(\mathbf{z}) - w_L L(\mathbf{z}) \]

subject to resource constraints and mandatory sensors (for example, wheel encoder and IMU):

\[ \sum_{i=1}^m z_i p_i \leq P_{\max}, \quad \sum_{i=1}^m z_i b_i \leq B_{\max}, \quad \sum_{i=1}^m z_i c_i \leq C_{\max}, \quad L(\mathbf{z}) \leq L_{\max} \]

\[ z_i = 1 \;\; \text{for all mandatory sensors}\; i \in \mathcal{M}. \]

Here \( p_i \) is power draw, \( b_i \) communication bandwidth, \( c_i \) compute cost (e.g., CPU time per second), and \( L(\mathbf{z}) \) a latency proxy that includes sampling and processing delay. The term \( I(\mathbf{z}) \) is an information score (e.g., log-determinant of the posterior information matrix), discussed below.

Sensor selection must be based on what state components each sensor constrains, not only on nominal accuracy. For a linearized model around a capstone operating condition:

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

\[ \mathbf{y}_k^{(i)} = \mathbf{H}_k^{(i)} \mathbf{x}_k + \mathbf{v}_k^{(i)}, \qquad \mathbf{v}_k^{(i)} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k^{(i)}) \]

Examples of complementary constraints:

• Wheel encoders constrain longitudinal motion increments but drift under slip (Chapter 5).
• IMU provides high-rate inertial increments but accumulates bias (Chapters 5 and 7).
• LiDAR scan matching adds geometric pose corrections (Chapters 10 and 12).
• Camera/depth sensors aid odometry and local geometry but are sensitive to lighting/texture (Chapters 13 and 18).
• GNSS/RTK provides absolute position outdoors but can degrade near occlusion and multipath (Chapter 18).

Two sensors with similar \( \mathbf{H} \) rows often provide redundant information, while sensors with different sensitivity directions can dramatically improve conditioning. Therefore, the design metric must account for the matrix structure, not only scalar noise variances.

For a finite horizon of length \( N \), define the weighted observability Gramian (linearized, time-varying form):

\[ \mathbf{W}_o(N) = \sum_{k=0}^{N-1} \boldsymbol{\Phi}_{k,0}^T \left(\sum_{i\in\mathcal{S}} (\mathbf{H}_k^{(i)})^T (\mathbf{R}_k^{(i)})^{-1} \mathbf{H}_k^{(i)} \right) \boldsymbol{\Phi}_{k,0} \]

where \( \boldsymbol{\Phi}_{k,0} \) is the state transition matrix from time 0 to k. If \( \mathbf{W}_o(N) \) is rank deficient, some state directions are weakly observable or unobservable under the chosen suite and trajectory.

A practical scalar score is the D-optimal information metric:

\[ I(\mathbf{z}) = \log \det \left( \mathbf{P}_{k|k}^{-1} \right) \]

where \( \mathbf{P}_{k|k} \) is the posterior covariance after the selected measurements are incorporated. Larger values imply tighter uncertainty ellipsoids.

Key proposition (additivity under conditional independence): if sensor noises are conditionally independent given the state, then the per-step information contribution is additive:

\[ \mathbf{Y}_k^{+} = \mathbf{Y}_k^{-} + \sum_{i\in\mathcal{S}} (\mathbf{H}_k^{(i)})^T (\mathbf{R}_k^{(i)})^{-1} \mathbf{H}_k^{(i)}, \qquad \mathbf{Y} = \mathbf{P}^{-1}. \]

Proof sketch: The joint likelihood of independent measurements factorizes into a product of Gaussian terms. Taking the negative log-likelihood yields a sum of quadratic forms. The Hessian of that sum with respect to the state estimate equals the sum of individual Hessians, which is precisely the additive information term above.

From the same formula, adding a sensor can only improve (or preserve) information because each term \( (\mathbf{H}^T \mathbf{R}^{-1} \mathbf{H}) \) is positive semidefinite. Hence:

\[ \mathbf{Y}_{\text{with new sensor}} - \mathbf{Y}_{\text{old}} \succeq \mathbf{0} \quad \Rightarrow \quad \mathbf{P}_{\text{with new sensor}} \preceq \mathbf{P}_{\text{old}} \]

in the Loewner ordering, assuming exact models and no numerical instability. This monotonicity is the mathematical basis for resource-constrained subset search.

In capstone deployments, a high-accuracy sensor can still degrade the stack if its timestamps are delayed or inconsistent. Let \( \delta t_i \) be the effective timing error (sampling jitter + middleware delay + queueing). A first-order measurement perturbation is:

\[ \Delta \mathbf{y}^{(i)} \approx \frac{\partial h_i}{\partial t} \delta t_i = \frac{\partial h_i}{\partial \mathbf{x}} \dot{\mathbf{x}} \; \delta t_i. \]

Therefore, the induced covariance inflation can be approximated by

\[ \mathbf{R}_{\text{eff}}^{(i)} \approx \mathbf{R}^{(i)} + \mathbf{J}_{t}^{(i)} \sigma_{t,i}^2 (\mathbf{J}_{t}^{(i)})^T, \qquad \mathbf{J}_{t}^{(i)} = \frac{\partial h_i}{\partial \mathbf{x}} \dot{\mathbf{x}}. \]

This is why sensor selection and scheduler design must be co-designed: the effective noise matrix depends on runtime timing quality, not only on the data sheet.

A useful capstone-level bound for planar localization is \( \|\Delta \mathbf{p}\| \leq \|\mathbf{v}\| \; |\delta t| \), so at \( 1.5\,\text{m/s} \) and \( 40\,\text{ms} \) delay, the temporal misalignment alone can induce about \( 0.06\,\text{m} \) spatial error.

Given hardware does not imply fixed usefulness: mounting geometry and calibration quality can change the effective measurement model. If a sensor pose relative to the robot frame is \( \mathbf{T}_{bs} \) and the calibration error is a small perturbation \( \delta \boldsymbol{\xi}_{bs} \), then the measurement residual changes as

\[ \Delta \mathbf{r} \approx \frac{\partial \mathbf{r}}{\partial \boldsymbol{\xi}_{bs}} \delta \boldsymbol{\xi}_{bs} \]

which means extrinsic uncertainty contributes an additional error term:

\[ \mathbf{R}_{\text{total}} \approx \mathbf{R}_{\text{sensor}} + \mathbf{J}_{\xi} \mathbf{\Sigma}_{\xi} \mathbf{J}_{\xi}^T. \]

For capstone design reviews, students should justify sensor placement by: (i) field-of-view coverage, (ii) vibration exposure, (iii) occlusion risk, and (iv) calibration repeatability. A mechanically unstable camera mount can nullify the theoretical information benefit of vision.

The recommended workflow below matches the mathematical formulation and can be executed before full stack deployment (Lesson 4). It combines observability screening, resource budgeting, and integration risk scoring.

flowchart TD
  S0["Start from mandatory sensors (encoder + IMU)"] --> S1["Enumerate feasible optional subsets"]
  S1 --> S2["Compute posterior covariance / information score"]
  S2 --> S3["Apply power + CPU + bandwidth + latency constraints"]
  S3 --> S4["Inflate R for timing and calibration uncertainty"]
  S4 --> S5["Rank feasible suites by weighted objective"]
  S5 --> S6["Choose mounting + synchronization plan"]
  S6 --> S7["Record justification for capstone report"]
        

This workflow is intentionally aligned with Chapters 5–19: it reuses odometry, probabilistic filtering, mapping/scan-matching, perception robustness, and benchmarking concepts without introducing unrelated theory.

For a linear Gaussian update with prior covariance \( \mathbf{P}^- \) and a stacked measurement matrix \( \mathbf{H} \) formed from the selected suite:

\[ \mathbf{P}^+ = \left( (\mathbf{P}^-)^{-1} + \mathbf{H}^T \mathbf{R}^{-1} \mathbf{H} \right)^{-1}. \]

If a new sensor contributes \( \Delta \mathbf{Y} = \mathbf{H}_n^T \mathbf{R}_n^{-1} \mathbf{H}_n \succeq \mathbf{0} \), then the updated information matrix becomes \( \mathbf{Y}^+_n = \mathbf{Y}^+ + \Delta \mathbf{Y} \). For any nonzero vector \( \mathbf{u} \):

\[ \mathbf{u}^T \mathbf{Y}^+_n \mathbf{u} = \mathbf{u}^T \mathbf{Y}^+ \mathbf{u} + \mathbf{u}^T \Delta \mathbf{Y} \mathbf{u} \geq \mathbf{u}^T \mathbf{Y}^+ \mathbf{u}. \]

Thus \( \mathbf{Y}^+_n \succeq \mathbf{Y}^+ \), and by matrix inversion order reversal on positive definite matrices:

\[ (\mathbf{Y}^+_n)^{-1} \preceq (\mathbf{Y}^+)^{-1} \quad \Rightarrow \quad \mathbf{P}^+_n \preceq \mathbf{P}^+. \]

This proof clarifies an important engineering detail: if adding a sensor worsens field performance, the cause is usually model mismatch, timing inconsistency, calibration drift, or outliers—not the idealized information theory itself.

The following Python code enumerates feasible sensor subsets from a given hardware pool, applies an information-form covariance update proxy, and ranks feasible suites using a weighted multi-objective score. It is suitable for capstone design-space exploration before ROS/real-time integration.

Chapter20_Lesson2.py

# Chapter20_Lesson2.py
# Sensor Suite Selection (given hardware) - Capstone AMR
# Python reference implementation: constrained subset search using an information-form proxy.

import itertools
import math
from dataclasses import dataclass
from typing import Dict, List, Tuple

import numpy as np

np.set_printoptions(precision=4, suppress=True)

@dataclass(frozen=True)
class SensorSpec:
    name: str
    rate_hz: float
    power_w: float
    bandwidth_kbps: float
    cpu_ms: float                 # processing time per measurement
    sigma: Tuple[float, ...]      # std dev per measurement channel
    H: np.ndarray                 # linearized measurement Jacobian
    mandatory: bool = False

def info_update(P: np.ndarray, H: np.ndarray, R: np.ndarray) -> np.ndarray:
    """Information-form covariance update for linear measurement y = Hx + v."""
    I_prior = np.linalg.inv(P)
    I_meas = H.T @ np.linalg.inv(R) @ H
    return np.linalg.inv(I_prior + I_meas)

def suite_metrics(suite: List[SensorSpec], P0: np.ndarray, dt: float = 0.1) -> Dict[str, float]:
    # Process model proxy (drift per cycle) for state [x, y, theta, v, gyro_bias]
    Q = np.diag([0.03, 0.03, 0.01, 0.05, 0.002]) * dt
    P_pred = P0 + Q

    P = P_pred.copy()
    total_power = 0.0
    total_bw = 0.0
    cpu_load_ratio = 0.0
    latency_ms = 0.0

    for s in suite:
        H = s.H
        R = np.diag(np.array(s.sigma) ** 2)
        P = info_update(P, H, R)
        total_power += s.power_w
        total_bw += s.bandwidth_kbps
        cpu_load_ratio += (s.cpu_ms * s.rate_hz) / 1000.0
        latency_ms += (1000.0 / s.rate_hz) + s.cpu_ms  # sample period + processing

    # Scalar proxies
    trP = float(np.trace(P))
    logdet_info = float(np.log(np.linalg.det(np.linalg.inv(P))))
    mean_latency_ms = latency_ms / max(1, len(suite))
    return {
        "traceP": trP,
        "logdetInfo": logdet_info,
        "powerW": total_power,
        "bandwidthKbps": total_bw,
        "cpuLoad": cpu_load_ratio,
        "meanLatencyMs": mean_latency_ms,
    }

def score(metrics: Dict[str, float]) -> float:
    # Multi-objective weighted score (maximize)
    # Higher logdet information is better; lower resource use and latency are better.
    return (
        1.0 * metrics["logdetInfo"]
        - 0.35 * metrics["traceP"]
        - 0.03 * metrics["powerW"]
        - 0.0008 * metrics["bandwidthKbps"]
        - 1.8 * metrics["cpuLoad"]
        - 0.01 * metrics["meanLatencyMs"]
    )

def build_sensors() -> List[SensorSpec]:
    # State: x = [x, y, theta, v, b_g]^T
    # Measurement models are linearized proxies for selection analysis.
    sensors = [
        SensorSpec(
            name="wheel_encoder",
            rate_hz=50.0,
            power_w=0.5,
            bandwidth_kbps=20.0,
            cpu_ms=0.3,
            sigma=(0.03, 0.01),  # v, yaw-rate-like proxy
            H=np.array([
                [0, 0, 0, 1, 0],   # velocity
                [0, 0, 1, 0, 1],   # theta + gyro bias proxy
            ], dtype=float),
            mandatory=True
        ),
        SensorSpec(
            name="imu",
            rate_hz=200.0,
            power_w=0.8,
            bandwidth_kbps=120.0,
            cpu_ms=0.4,
            sigma=(0.015, 0.01),  # yaw-rate, accel/velocity proxy
            H=np.array([
                [0, 0, 1, 0, 1],   # yaw + bias
                [0, 0, 0, 1, 0],   # velocity
            ], dtype=float),
            mandatory=True
        ),
        SensorSpec(
            name="2d_lidar",
            rate_hz=10.0,
            power_w=8.0,
            bandwidth_kbps=1500.0,
            cpu_ms=12.0,
            sigma=(0.05, 0.05, 0.02),  # x, y, theta corrections from scan matching
            H=np.array([
                [1, 0, 0, 0, 0],
                [0, 1, 0, 0, 0],
                [0, 0, 1, 0, 0],
            ], dtype=float),
        ),
        SensorSpec(
            name="mono_camera",
            rate_hz=30.0,
            power_w=2.5,
            bandwidth_kbps=2500.0,
            cpu_ms=18.0,
            sigma=(0.08, 0.08, 0.03),  # VO pose increment proxy
            H=np.array([
                [1, 0, 0, 0, 0],
                [0, 1, 0, 0, 0],
                [0, 0, 1, 0, 0],
            ], dtype=float),
        ),
        SensorSpec(
            name="depth_camera",
            rate_hz=15.0,
            power_w=4.5,
            bandwidth_kbps=5000.0,
            cpu_ms=20.0,
            sigma=(0.06, 0.06, 0.03, 0.08),  # x, y, theta, v
            H=np.array([
                [1, 0, 0, 0, 0],
                [0, 1, 0, 0, 0],
                [0, 0, 1, 0, 0],
                [0, 0, 0, 1, 0],
            ], dtype=float),
        ),
        SensorSpec(
            name="gnss_rtk",
            rate_hz=5.0,
            power_w=2.0,
            bandwidth_kbps=40.0,
            cpu_ms=1.5,
            sigma=(0.03, 0.03),  # x, y absolute
            H=np.array([
                [1, 0, 0, 0, 0],
                [0, 1, 0, 0, 0],
            ], dtype=float),
        ),
    ]
    return sensors

def select_suite():
    sensors = build_sensors()
    P0 = np.diag([1.5, 1.5, 0.5, 0.8, 0.2])  # prior covariance at startup

    power_budget = 14.0
    bw_budget = 7000.0
    cpu_budget = 0.80   # 80% of one CPU core equivalent
    latency_budget_ms = 120.0

    mandatory = [s for s in sensors if s.mandatory]
    optional = [s for s in sensors if not s.mandatory]

    best = None
    evaluated = []

    for r in range(len(optional) + 1):
        for combo in itertools.combinations(optional, r):
            suite = mandatory + list(combo)
            metrics = suite_metrics(suite, P0)
            feasible = (
                metrics["powerW"] \leq power_budget and
                metrics["bandwidthKbps"] \leq bw_budget and
                metrics["cpuLoad"] \leq cpu_budget and
                metrics["meanLatencyMs"] \leq latency_budget_ms
            )
            if not feasible:
                continue
            J = score(metrics)
            item = (J, suite, metrics)
            evaluated.append(item)
            if best is None or J > best[0]:
                best = item

    if best is None:
        raise RuntimeError("No feasible sensor suite under the provided budgets.")

    # Sort top feasible suites
    evaluated.sort(key=lambda x: x[0], reverse=True)
    print("Top feasible suites:")
    for rank, (J, suite, metrics) in enumerate(evaluated[:8], start=1):
        names = [s.name for s in suite]
        print(f"{rank:2d}. score={J:.3f}  suite={names}")
        print("    ", {k: round(v, 4) for k, v in metrics.items()})

    print("\nBest suite recommendation:")
    J, suite, metrics = best
    print("Sensors:", [s.name for s in suite])
    print("Score:", round(J, 4))
    print("Metrics:", {k: round(v, 4) for k, v in metrics.items()})

if __name__ == "__main__":
    select_suite()

      

The C++ version mirrors the same logic and is appropriate for performance-oriented evaluation tools or offline benchmarking utilities. It uses Eigen matrices to compute the covariance updates and select the best feasible suite.

Chapter20_Lesson2.cpp

// Chapter20_Lesson2.cpp
// Sensor Suite Selection (given hardware) - C++/Eigen implementation
// Compile (Linux): g++ -std=c++17 Chapter20_Lesson2.cpp -O2 -I /usr/include/eigen3 -o sensor_select

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <string>
#include <iomanip>
#include <cmath>

struct SensorSpec {
    std::string name;
    double rateHz;
    double powerW;
    double bandwidthKbps;
    double cpuMs;
    Eigen::MatrixXd H;
    Eigen::MatrixXd R;
    bool mandatory;
};

Eigen::MatrixXd infoUpdate(const Eigen::MatrixXd& P, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R) {
    Eigen::MatrixXd Iprior = P.inverse();
    Eigen::MatrixXd Imeas  = H.transpose() * R.inverse() * H;
    return (Iprior + Imeas).inverse();
}

struct Metrics {
    double traceP;
    double logdetInfo;
    double powerW;
    double bandwidthKbps;
    double cpuLoad;
    double meanLatencyMs;
};

Metrics evaluateSuite(const std::vector<SensorSpec>& suite, const Eigen::MatrixXd& P0, double dt = 0.1) {
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(5, 5);
    Q.diagonal() << 0.03 * dt, 0.03 * dt, 0.01 * dt, 0.05 * dt, 0.002 * dt;
    Eigen::MatrixXd P = P0 + Q;

    double totalPower = 0.0, totalBw = 0.0, cpuLoad = 0.0, latencyMs = 0.0;
    for (const auto& s : suite) {
        P = infoUpdate(P, s.H, s.R);
        totalPower += s.powerW;
        totalBw += s.bandwidthKbps;
        cpuLoad += (s.cpuMs * s.rateHz) / 1000.0;
        latencyMs += 1000.0 / s.rateHz + s.cpuMs;
    }

    double sign = 0.0, logdetP = 0.0;
    Eigen::LLT<Eigen::MatrixXd> llt(P);
    if (llt.info() == Eigen::Success) {
        const auto L = llt.matrixL();
        for (int i = 0; i < L.rows(); ++i) { logdetP += 2.0 * std::log(L(i, i)); }
    } else {
        logdetP = std::log(P.determinant());
    }

    Metrics m;
    m.traceP = P.trace();
    m.logdetInfo = -logdetP;   // log(det(P^{-1}))
    m.powerW = totalPower;
    m.bandwidthKbps = totalBw;
    m.cpuLoad = cpuLoad;
    m.meanLatencyMs = latencyMs / std::max<size_t>(1, suite.size());
    return m;
}

double score(const Metrics& m) {
    return 1.0 * m.logdetInfo
         - 0.35 * m.traceP
         - 0.03 * m.powerW
         - 0.0008 * m.bandwidthKbps
         - 1.8 * m.cpuLoad
         - 0.01 * m.meanLatencyMs;
}

SensorSpec makeSensor(
    const std::string& name, double rateHz, double powerW, double bw, double cpuMs,
    const Eigen::MatrixXd& H, const Eigen::VectorXd& sigma, bool mandatory = false
) {
    SensorSpec s;
    s.name = name; s.rateHz = rateHz; s.powerW = powerW; s.bandwidthKbps = bw; s.cpuMs = cpuMs; s.H = H;
    s.R = sigma.array().square().matrix().asDiagonal();
    s.mandatory = mandatory;
    return s;
}

int main() {
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    std::vector<SensorSpec> sensors;

    MatrixXd Henc(2, 5); Henc << 0,0,0,1,0,  0,0,1,0,1;
    VectorXd senc(2); senc << 0.03, 0.01;
    sensors.push_back(makeSensor("wheel_encoder", 50.0, 0.5, 20.0, 0.3, Henc, senc, true));

    MatrixXd Himu(2, 5); Himu << 0,0,1,0,1,  0,0,0,1,0;
    VectorXd simu(2); simu << 0.015, 0.01;
    sensors.push_back(makeSensor("imu", 200.0, 0.8, 120.0, 0.4, Himu, simu, true));

    MatrixXd Hlidar(3, 5); Hlidar << 1,0,0,0,0,  0,1,0,0,0,  0,0,1,0,0;
    VectorXd slidar(3); slidar << 0.05, 0.05, 0.02;
    sensors.push_back(makeSensor("2d_lidar", 10.0, 8.0, 1500.0, 12.0, Hlidar, slidar));

    MatrixXd Hcam(3, 5); Hcam << 1,0,0,0,0,  0,1,0,0,0,  0,0,1,0,0;
    VectorXd scam(3); scam << 0.08, 0.08, 0.03;
    sensors.push_back(makeSensor("mono_camera", 30.0, 2.5, 2500.0, 18.0, Hcam, scam));

    MatrixXd Hdepth(4, 5); Hdepth << 1,0,0,0,0,  0,1,0,0,0,  0,0,1,0,0,  0,0,0,1,0;
    VectorXd sdepth(4); sdepth << 0.06, 0.06, 0.03, 0.08;
    sensors.push_back(makeSensor("depth_camera", 15.0, 4.5, 5000.0, 20.0, Hdepth, sdepth));

    MatrixXd Hgnss(2, 5); Hgnss << 1,0,0,0,0,  0,1,0,0,0;
    VectorXd sgnss(2); sgnss << 0.03, 0.03;
    sensors.push_back(makeSensor("gnss_rtk", 5.0, 2.0, 40.0, 1.5, Hgnss, sgnss));

    std::vector<int> mandatoryIdx, optionalIdx;
    for (int i = 0; i < static_cast<int>(sensors.size()); ++i) {
        if (sensors[i].mandatory) mandatoryIdx.push_back(i);
        else optionalIdx.push_back(i);
    }

    MatrixXd P0 = MatrixXd::Zero(5, 5);
    P0.diagonal() << 1.5, 1.5, 0.5, 0.8, 0.2;

    const double PWR_BUDGET = 14.0;
    const double BW_BUDGET = 7000.0;
    const double CPU_BUDGET = 0.80;
    const double LAT_BUDGET = 120.0;

    double bestScore = -1e18;
    std::vector<std::string> bestNames;
    Metrics bestMetrics{};

    const int M = static_cast<int>(optionalIdx.size());
    for (int mask = 0; mask < (1 << M); ++mask) {
        std::vector<SensorSpec> suite;
        for (int idx : mandatoryIdx) suite.push_back(sensors[idx]);
        for (int j = 0; j < M; ++j) {
            if (mask & (1 << j)) suite.push_back(sensors[optionalIdx[j]]);
        }

        Metrics m = evaluateSuite(suite, P0);
        bool feasible = (m.powerW \leq PWR_BUDGET) &&
                        (m.bandwidthKbps \leq BW_BUDGET) &&
                        (m.cpuLoad \leq CPU_BUDGET) &&
                        (m.meanLatencyMs \leq LAT_BUDGET);
        if (!feasible) continue;

        double J = score(m);
        if (J > bestScore) {
            bestScore = J;
            bestMetrics = m;
            bestNames.clear();
            for (const auto& s : suite) bestNames.push_back(s.name);
        }
    }

    std::cout << std::fixed << std::setprecision(4);
    std::cout << "Best feasible suite\\n";
    for (const auto& n : bestNames) std::cout << "  - " << n << "\\n";
    std::cout << "score = " << bestScore << "\\n";
    std::cout << "trace(P+) = " << bestMetrics.traceP << "\\n";
    std::cout << "logdetInfo = " << bestMetrics.logdetInfo << "\\n";
    std::cout << "powerW = " << bestMetrics.powerW << "\\n";
    std::cout << "bandwidthKbps = " << bestMetrics.bandwidthKbps << "\\n";
    std::cout << "cpuLoad = " << bestMetrics.cpuLoad << "\\n";
    std::cout << "meanLatencyMs = " << bestMetrics.meanLatencyMs << "\\n";
    return 0;
}

      

In many student capstones, middleware timing and queueing dominate field failures. The Java example below simulates asynchronous sensor events, queueing delay, jitter, and dropout rate, then computes a simple QoS score for timing health of the chosen suite.

Chapter20_Lesson2.java

// Chapter20_Lesson2.java
// Java implementation: sensor timing and middleware-load simulation for a selected sensor suite.
// Run: javac Chapter20_Lesson2.java && java Chapter20_Lesson2

import java.util.*;

public class Chapter20_Lesson2 {
    static class Sensor {
        String name;
        double rateHz;
        double processingMs;
        double jitterStdMs;
        double dropProb;
        int priority;

        Sensor(String name, double rateHz, double processingMs, double jitterStdMs, double dropProb, int priority) {
            this.name = name;
            this.rateHz = rateHz;
            this.processingMs = processingMs;
            this.jitterStdMs = jitterStdMs;
            this.dropProb = dropProb;
            this.priority = priority;
        }

        double periodMs() { return 1000.0 / rateHz; }
    }

    static class Event {
        Sensor sensor;
        double idealTimeMs;
        double actualTimeMs;
        boolean dropped;

        Event(Sensor sensor, double idealTimeMs, double actualTimeMs, boolean dropped) {
            this.sensor = sensor;
            this.idealTimeMs = idealTimeMs;
            this.actualTimeMs = actualTimeMs;
            this.dropped = dropped;
        }
    }

    static class Stats {
        int total = 0;
        int dropped = 0;
        double sumAbsJitter = 0.0;
        double maxAbsJitter = 0.0;
        double cpuBusyMs = 0.0;
    }

    public static void main(String[] args) {
        // Example suite chosen after a selection phase:
        // wheel_encoder + imu + 2d_lidar + gnss_rtk
        List<Sensor> sensors = Arrays.asList(
            new Sensor("wheel_encoder", 50.0, 0.3, 0.10, 0.001, 1),
            new Sensor("imu",          200.0, 0.4, 0.05, 0.0005, 0),
            new Sensor("2d_lidar",      10.0, 12.0, 1.20, 0.005, 2),
            new Sensor("gnss_rtk",       5.0, 1.5, 0.70, 0.002, 3)
        );

        double horizonMs = 60000.0; // 60 s
        Map<String, Stats> map = new LinkedHashMap<>();
        for (Sensor s : sensors) map.put(s.name, new Stats());

        PriorityQueue<Event> q = new PriorityQueue<>(
            Comparator.<Event>comparingDouble(e -> e.actualTimeMs)
                      .thenComparingInt(e -> e.sensor.priority)
        );

        Random rng = new Random(7);

        // Schedule all events
        for (Sensor s : sensors) {
            for (double t = 0.0; t \leq horizonMs + 1e-9; t += s.periodMs()) {
                double jitter = rng.nextGaussian() * s.jitterStdMs;
                boolean dropped = rng.nextDouble() < s.dropProb;
                q.add(new Event(s, t, Math.max(0.0, t + jitter), dropped));
            }
        }

        double cpuAvailableAt = 0.0;
        while (!q.isEmpty()) {
            Event e = q.poll();
            Stats st = map.get(e.sensor.name);
            st.total += 1;

            if (e.dropped) {
                st.dropped += 1;
                continue;
            }

            double start = Math.max(e.actualTimeMs, cpuAvailableAt);
            double finish = start + e.sensor.processingMs;
            cpuAvailableAt = finish;

            double effectiveJitter = start - e.idealTimeMs; // includes queueing delay
            double absJ = Math.abs(effectiveJitter);

            st.sumAbsJitter += absJ;
            st.maxAbsJitter = Math.max(st.maxAbsJitter, absJ);
            st.cpuBusyMs += e.sensor.processingMs;
        }

        System.out.println("Sensor timing diagnostics (60 s horizon)");
        System.out.println("-----------------------------------------");
        double totalCpuBusy = 0.0;
        for (Sensor s : sensors) {
            Stats st = map.get(s.name);
            totalCpuBusy += st.cpuBusyMs;
            int valid = st.total - st.dropped;
            double meanAbsJitter = valid > 0 ? st.sumAbsJitter / valid : 0.0;
            double dropoutRate = st.total > 0 ? ((double) st.dropped / st.total) : 0.0;

            // A simple QoS score: penalize jitter, dropouts, and CPU time
            double qos = 100.0
                - 2.0 * meanAbsJitter
                - 0.8 * st.maxAbsJitter
                - 800.0 * dropoutRate
                - 0.005 * st.cpuBusyMs;

            System.out.printf(
                Locale.US,
                "%-12s total=%5d drop=%4d (%.3f) mean|j|=%.3f ms max|j|=%.3f ms cpu=%.1f ms QoS=%.2f%n",
                s.name, st.total, st.dropped, dropoutRate, meanAbsJitter, st.maxAbsJitter, st.cpuBusyMs, qos
            );
        }

        double cpuUtil = totalCpuBusy / horizonMs;
        System.out.printf(Locale.US, "%nAggregate CPU utilization = %.3f%n", cpuUtil);
        if (cpuUtil > 0.8) {
            System.out.println("Warning: CPU utilization exceeds recommended planning threshold (0.8).");
        } else {
            System.out.println("CPU utilization is within the planning threshold.");
        }
    }
}

      

The MATLAB script below compares candidate suites using asynchronous measurement updates and Joseph-form covariance updates. It also prints sample-time and noise parameters that can be mapped directly into Simulink sensor and rate-transition blocks for a capstone integration model.

Chapter20_Lesson2.m

% Chapter20_Lesson2.m
% Sensor Suite Selection (given hardware) - MATLAB / Simulink support script
% This script compares candidate suites using covariance propagation and asynchronous updates.
% It also prints parameters that can be copied into Simulink blocks (sample times, R matrices).

clear; clc;

% State x = [x; y; theta; v; b_g]
n = 5;
dt = 0.01;           % base simulation step (100 Hz)
T  = 30.0;           % seconds
N  = round(T / dt);

% Constant-velocity planar linearized model (local frame approximation)
F = eye(n);
F(1,4) = dt;         % x <- x + v*dt
F(3,5) = -dt;        % theta affected by gyro bias (proxy)
Q = diag([0.03, 0.03, 0.01, 0.05, 0.002]) * dt;

% Candidate sensors (linearized H, R, and update periods)
sensors = struct([]);

sensors(1).name = 'wheel_encoder';
sensors(1).H = [0 0 0 1 0; 0 0 1 0 1];
sensors(1).R = diag([0.03^2, 0.01^2]);
sensors(1).Ts = 1/50;

sensors(2).name = 'imu';
sensors(2).H = [0 0 1 0 1; 0 0 0 1 0];
sensors(2).R = diag([0.015^2, 0.01^2]);
sensors(2).Ts = 1/200;

sensors(3).name = '2d_lidar';
sensors(3).H = [1 0 0 0 0; 0 1 0 0 0; 0 0 1 0 0];
sensors(3).R = diag([0.05^2, 0.05^2, 0.02^2]);
sensors(3).Ts = 1/10;

sensors(4).name = 'mono_camera';
sensors(4).H = [1 0 0 0 0; 0 1 0 0 0; 0 0 1 0 0];
sensors(4).R = diag([0.08^2, 0.08^2, 0.03^2]);
sensors(4).Ts = 1/30;

sensors(5).name = 'gnss_rtk';
sensors(5).H = [1 0 0 0 0; 0 1 0 0 0];
sensors(5).R = diag([0.03^2, 0.03^2]);
sensors(5).Ts = 1/5;

% Two example suites (given hardware, different choices)
suiteA = [1 2 3 5]; % wheel + imu + lidar + gnss
suiteB = [1 2 4 5]; % wheel + imu + camera + gnss

P0 = diag([1.5, 1.5, 0.5, 0.8, 0.2]);

[traceHistA, PfinalA] = run_suite(F, Q, sensors, suiteA, P0, dt, N);
[traceHistB, PfinalB] = run_suite(F, Q, sensors, suiteB, P0, dt, N);

fprintf('Final trace(P) suiteA = %.4f\n', trace(PfinalA));
fprintf('Final trace(P) suiteB = %.4f\n', trace(PfinalB));

% Plot covariance trace evolution
figure;
plot((0:N-1)*dt, traceHistA, 'LineWidth', 1.5); hold on;
plot((0:N-1)*dt, traceHistB, 'LineWidth', 1.5);
xlabel('Time [s]');
ylabel('trace(P)');
legend('Suite A: enc+imu+lidar+gnss', 'Suite B: enc+imu+camera+gnss', 'Location', 'northeast');
title('Covariance Trace Comparison for Candidate Sensor Suites');
grid on;

% Print Simulink-friendly configuration
fprintf('\nSimulink block parameters (copy into Rate Transition / Sensor blocks):\n');
for i = 1:numel(sensors)
    fprintf('  %s: Ts = %.4f s, R = %s\n', sensors(i).name, sensors(i).Ts, mat2str(diag(sensors(i).R)'));
end

% -------------------------
function [traceHist, P] = run_suite(F, Q, sensors, suite, P0, dt, N)
    n = size(F,1);
    P = P0;
    traceHist = zeros(N,1);

    % Next measurement times
    nextT = inf(1, numel(sensors));
    for k = suite
        nextT(k) = 0.0;
    end

    t = 0.0;
    for i = 1:N
        % Time update
        P = F * P * F' + Q;

        % Measurement updates (asynchronous)
        for k = suite
            if t + 1e-12 \geq nextT(k)
                H = sensors(k).H;
                R = sensors(k).R;
                S = H * P * H' + R;
                K = P * H' / S;
                P = (eye(n) - K * H) * P * (eye(n) - K * H)' + K * R * K'; % Joseph form
                nextT(k) = nextT(k) + sensors(k).Ts;
            end
        end

        traceHist(i) = trace(P);
        t = t + dt;
    end
end

      

This Mathematica code performs symbolic observability-rank checks and compares information determinants for candidate sensor additions. It is useful when students want a formal symbolic justification in the capstone report.

Chapter20_Lesson2.nb

(* Chapter20_Lesson2.nb *)
(* Wolfram Mathematica / Wolfram Language code for symbolic observability and information analysis *)

ClearAll["Global`*"];

(* State x = {x, y, theta, v, bg}; linearized discrete-time dynamics *)
dt = Symbol["dt"];
F = {
  {1, 0, 0, dt, 0},
  {0, 1, 0, 0,  0},
  {0, 0, 1, 0, -dt},
  {0, 0, 0, 1,  0},
  {0, 0, 0, 0,  1}
};

(* Candidate measurement Jacobians *)
Henc = {
  {0, 0, 0, 1, 0},
  {0, 0, 1, 0, 1}
};

Himu = {
  {0, 0, 1, 0, 1},
  {0, 0, 0, 1, 0}
};

Hlidar = {
  {1, 0, 0, 0, 0},
  {0, 1, 0, 0, 0},
  {0, 0, 1, 0, 0}
};

Hgnss = {
  {1, 0, 0, 0, 0},
  {0, 1, 0, 0, 0}
};

(* Finite-horizon observability matrix builder *)
ObsMat[F_, H_, L_Integer] := Join @@ Table[H.MatrixPower[F, k], {k, 0, L - 1}];

OimuEnc = ObsMat[F, Join[Henc, Himu], 3];
OlidarEncImu = ObsMat[F, Join[Henc, Himu, Hlidar], 2];

Print["Rank(encoder+imu, horizon=3) = ", MatrixRank[OimuEnc]];
Print["Rank(encoder+imu+lidar, horizon=2) = ", MatrixRank[OlidarEncImu]];

(* Information matrix accumulation *)
P0 = DiagonalMatrix[{px, py, pth, pv, pbg}];
Renc = DiagonalMatrix[{sev^2, seg^2}];
Rimu = DiagonalMatrix[{sigw^2, siva^2}];
Rlidar = DiagonalMatrix[{slx^2, sly^2, slt^2}];

InfoPrior = Inverse[P0];
InfoEnc = Transpose[Henc].Inverse[Renc].Henc;
InfoImu = Transpose[Himu].Inverse[Rimu].Himu;
InfoLidar = Transpose[Hlidar].Inverse[Rlidar].Hlidar;

InfoTotalNoLidar = Simplify[InfoPrior + InfoEnc + InfoImu];
InfoTotalWithLidar = Simplify[InfoPrior + InfoEnc + InfoImu + InfoLidar];

detNoLidar = FullSimplify[Det[InfoTotalNoLidar]];
detWithLidar = FullSimplify[Det[InfoTotalWithLidar]];

Print["det(I_total) without lidar = ", detNoLidar];
Print["det(I_total) with lidar    = ", detWithLidar];
Print["det gain ratio = ", FullSimplify[detWithLidar/detNoLidar]];

(* Weighted selection score as a symbolic expression *)
alpha1 = Symbol["alphaInfo"];
alpha2 = Symbol["alphaPower"];
alpha3 = Symbol["alphaBW"];

scoreLidar = alpha1*Log[detWithLidar] - alpha2*8.0 - alpha3*1500.0;
scoreGnss  = alpha1*Log[Det[InfoPrior + InfoEnc + InfoImu + Transpose[Hgnss].Hgnss]] - alpha2*2.0 - alpha3*40.0;

Print["Symbolic score (lidar branch) = ", scoreLidar];
Print["Symbolic score (gnss proxy branch) = ", scoreGnss];

(* Numeric substitution example *)
params = {
  dt -> 0.1,
  px -> 1.5, py -> 1.5, pth -> 0.5, pv -> 0.8, pbg -> 0.2,
  sev -> 0.03, seg -> 0.01, sigw -> 0.015, siva -> 0.01,
  slx -> 0.05, sly -> 0.05, slt -> 0.02,
  alphaInfo -> 1.0, alphaPower -> 0.03, alphaBW -> 0.0008
};

Print["Numeric lidar score = ", N[scoreLidar /. params]];
Print["Numeric det gain ratio = ", N[(detWithLidar/detNoLidar) /. params]];

      

Problem 1 (Feasible Subset Optimization): Let the optional sensors be LiDAR, camera, depth camera, and GNSS/RTK, with mandatory encoder and IMU. Formulate the constrained optimization problem that maximizes information while respecting power, bandwidth, and CPU budgets. Explain why exhaustive enumeration is acceptable in this lesson.

Solution: Define binary variables \( z_i \in \{0,1\} \) for the optional sensors. The objective is \( J(\mathbf{z})=w_I I(\mathbf{z})-w_P P(\mathbf{z})-w_B B(\mathbf{z})-w_C C(\mathbf{z})-w_L L(\mathbf{z}) \) with the constraints from Section 2 and fixed mandatory sensors. Exhaustive enumeration is reasonable because the optional set is small (typically \( m_{\text{optional}} \leq 6 \)), so only \( 2^m \) subsets are evaluated. For example, if \( m=4 \), only 16 subsets exist.

Problem 2 (Monotonic Covariance Reduction): Prove that adding a sensor with independent Gaussian noise cannot increase the posterior covariance in the ideal linear model.

Solution: Let the old information matrix be \( \mathbf{Y}_o \) and the new sensor contribution \( \Delta \mathbf{Y} = \mathbf{H}^T \mathbf{R}^{-1} \mathbf{H} \succeq \mathbf{0} \). Then \( \mathbf{Y}_n = \mathbf{Y}_o + \Delta \mathbf{Y} \succeq \mathbf{Y}_o \). Since matrix inversion reverses semidefinite order for positive definite matrices, \( \mathbf{Y}_n^{-1} \preceq \mathbf{Y}_o^{-1} \). Therefore the posterior covariance decreases (or remains equal) in every direction.

Problem 3 (Timing Error Bound): A robot travels at \( 1.2\,\text{m/s} \). A camera packet experiences an average timestamp mismatch of \( 35\,\text{ms} \). Compute a first-order upper bound on position error due solely to timing.

Solution: Using \( \|\Delta \mathbf{p}\| \leq \|\mathbf{v}\|\;|\delta t| \):

\[ \|\Delta \mathbf{p}\| \leq (1.2) (0.035) = 0.042\;\text{m} \]

so the timing mismatch alone can induce about \( 4.2\,\text{cm} \) error, which is large enough to distort scan-to-map or vision-to-map alignment in narrow corridors.

Problem 4 (Observability Complementarity): Explain why encoder + IMU may still leave position drift poorly constrained, and how LiDAR or GNSS changes the information matrix structure.

Solution: Encoder + IMU primarily constrain velocity, heading, and inertial increments, but they do not directly anchor global position. Consequently, the information matrix can remain weak in the \( x,y \) directions over long horizons. LiDAR scan matching adds geometric pose constraints (including \( x,y,\theta \)) relative to map structure, while GNSS/RTK directly contributes absolute position rows in \( \mathbf{H} \), increasing information in the position subspace and reducing drift.

Problem 5 (Capstone Review Checklist Design): Build a short decision procedure for a team that must justify sensor suite selection to an evaluation panel.

Solution (procedure):

1. State the mission environment and failure modes (indoors/outdoors, lighting, slip, occlusion).
2. Declare mandatory sensors and fixed hardware inventory.
3. Tabulate \( \mathbf{H}, \mathbf{R}, f_i, p_i, b_i, c_i \) for each sensor.
4. Compute observability/information metrics for candidate subsets.
5. Inflate noise models for synchronization and calibration uncertainty.
6. Reject infeasible subsets by budget constraints.
7. Select the best feasible suite and document mounting/calibration decisions.

Problem 6 (Weighted Objective Sensitivity): Show qualitatively how increasing \( w_B \) (bandwidth penalty) changes the preferred suite when LiDAR and depth camera are both available.

Solution: Since LiDAR and depth camera both improve geometric observability but the depth camera often has much larger bandwidth, a larger \( w_B \) shifts the optimum toward lower-bandwidth choices (e.g., LiDAR + GNSS or LiDAR only, depending on the environment). The selected subset can change even if the information term is larger for the depth camera, because the optimization is constrained and multi-objective, not accuracy-only.

Sensor suite selection in a capstone AMR project is a constrained estimation-design problem. The correct workflow is to model measurement structure, quantify observability and posterior uncertainty reduction, account for timing/calibration effects, and then optimize under real compute and power budgets. This lesson provides both the mathematical foundation and implementation templates needed for the integration work in Lessons 3 and 4.