In Chapters 7–13, we studied estimators (EKF/UKF, particle filters, scan matching, filter-based SLAM, graph-based SLAM, and visual/VIO pipelines). In Chapter 19 we switch from algorithm construction to algorithm evaluation. A localization or SLAM pipeline is only scientifically meaningful if we define: (i) what is being measured, (ii) under which experimental conditions, and (iii) how uncertainty and variability are reported.

Let the ground-truth pose sequence be \( X_1,\dots,X_N \) and the estimated sequence be \( \hat{X}_1,\dots,\hat{X}_N \), where each pose is in \( SE(2) \) for planar AMR (or \( SE(3) \) for 3D sensors). A metric is a mapping

\[ M: \big(\{X_k\}_{k=1}^N,\{\hat{X}_k\}_{k=1}^N, \text{metadata}\big) → \mathbb{R}^m \]

that outputs one or more scalar values (e.g., RMSE, median drift, precision, runtime). Good metrics should satisfy: interpretability, repeatability, and alignment with downstream AMR behavior (e.g., whether localization error actually destabilizes navigation).

flowchart TD
  A["Sensor logs and ground truth"] --> B["Time synchronization"]
  B --> C["Frame alignment"]
  C --> D["Trajectory metrics"]
  C --> E["Map metrics"]
  C --> F["Consistency metrics"]
  D --> G["Statistical comparison"]
  E --> G
  F --> G
  G --> H["Benchmark report and conclusions"]
        

A complete benchmark should report multiple metric families, because no single scalar captures all failure modes:

• Trajectory accuracy: absolute trajectory error (ATE), relative pose error (RPE), drift per meter.
• Estimator consistency: NEES/NIS statistics for probabilistic filters.
• Map quality: occupancy cross-entropy, IoU/F1, geometric distances for landmark/point maps.
• Loop closure and data association quality: precision-recall, false-positive rate.
• Computational performance: latency, throughput, memory, CPU/GPU utilization, energy.

For outdoor AMR and long-range missions, additional metrics such as drift normalized by traveled distance and time-to-relocalize after track loss are especially important.

\[ \text{DriftRate} = \frac{\text{ATE} }{L_{\text{path} } }, \qquad L_{\text{path} } = \sum_{k=1}^{N-1} \|\mathbf{p}_{k+1}-\mathbf{p}_k\|_2 \]

The normalization by path length is useful when comparing datasets with different sizes. A system with \( 0.5\,\text{m} \) ATE on a 10 m path is very different from the same ATE on a 2 km path.

Before computing trajectory error, we usually align the estimated trajectory to the ground-truth trajectory by an optimal rigid transform (no scaling for wheel/LiDAR/GNSS localization; optional similarity alignment for monocular VO if scale is ambiguous). In planar AMR, write poses by position \( \mathbf{p}_k \in \mathbb{R}^2 \) and heading \( \psi_k \). The alignment problem is:

\[ \min_{\mathbf{R}\in SO(2),\,\mathbf{t}\in\mathbb{R}^2} \sum_{k=1}^N \left\| \mathbf{p}_k - (\mathbf{R}\hat{\mathbf{p} }_k + \mathbf{t}) \right\|_2^2 \]

Proposition (optimal translation for fixed rotation): If \( \mathbf{R} \) is fixed, the minimizing translation is

\[ \mathbf{t}^*(\mathbf{R}) = \bar{\mathbf{p} } - \mathbf{R}\bar{\hat{\mathbf{p} } }, \quad \bar{\mathbf{p} } = \frac{1}{N}\sum_{k=1}^N \mathbf{p}_k, \quad \bar{\hat{\mathbf{p} } } = \frac{1}{N}\sum_{k=1}^N \hat{\mathbf{p} }_k \]

Proof: Differentiate the quadratic objective with respect to \( \mathbf{t} \), set gradient to zero, and solve the linear system. Since the Hessian is \( 2N\mathbf{I} \), the minimizer is unique.

In 2D, after centering the trajectories, the optimal rotation angle has a closed form:

\[ \theta^* = \operatorname{atan2}\!\left( \sum_{k=1}^N (x'_k y_k - y'_k x_k), \sum_{k=1}^N (x'_k x_k + y'_k y_k) \right), \quad \mathbf{R}^* = \begin{bmatrix} \cos\theta^* & -\sin\theta^* \\ \sin\theta^* & \cos\theta^* \end{bmatrix} \]

where \( (x'_k,y'_k) \) are centered estimated points and \( (x_k,y_k) \) are centered ground-truth points.

After alignment, the absolute trajectory error (ATE) RMSE is

\[ \text{ATE}_{\text{RMSE} } = \sqrt{\frac{1}{N} \sum_{k=1}^N \left\| \mathbf{p}_k - \mathbf{p}_k^{\text{(aligned)} } \right\|_2^2 } \]

The relative pose error (RPE) for lag \( \Delta \) measures local drift. Using pose composition in \( SE(2) \):

\[ E_k^{(\Delta)} = \left(X_k^{-1}X_{k+\Delta}\right)^{-1} \left(\hat{X}_k^{-1}\hat{X}_{k+\Delta}\right) \]

\[ \text{RPE}_{\text{trans} } = \sqrt{\frac{1}{N-\Delta}\sum_{k=1}^{N-\Delta} \|\operatorname{trans}(E_k^{(\Delta)})\|_2^2}, \qquad \text{RPE}_{\text{rot} } = \sqrt{\frac{1}{N-\Delta}\sum_{k=1}^{N-\Delta} \operatorname{ang}(E_k^{(\Delta)})^2} \]

Invariance note: RPE is invariant to a common global rigid transform applied to both trajectories, while ATE is not invariant unless alignment is performed first. This is one reason RPE is useful for evaluating odometry quality independent of global frame choice.

For Bayesian filters (EKF/UKF/graph posteriors), mean error alone is insufficient. We must test whether reported covariance matches actual error. Let \( \mathbf{e}_k = \mathbf{x}_k - \hat{\mathbf{x} }_k \) and covariance \( \mathbf{P}_k \). The normalized estimation error squared is

\[ \epsilon_k = \mathbf{e}_k^\top \mathbf{P}_k^{-1} \mathbf{e}_k \]

If the estimator is unbiased and consistent with Gaussian error, \( \mathbf{e}_k \sim \mathcal{N}(\mathbf{0},\mathbf{P}_k) \), then \( \epsilon_k \sim \chi^2_n \), where \( n \) is the state dimension.

Proof sketch: define the whitened error \( \mathbf{z}_k = \mathbf{P}_k^{-1/2}\mathbf{e}_k \). Then \( \mathbf{z}_k \sim \mathcal{N}(\mathbf{0},\mathbf{I}) \) and

\[ \epsilon_k = \mathbf{z}_k^\top \mathbf{z}_k = \sum_{i=1}^n z_{k,i}^2 \sim \chi_n^2 \]

Therefore, \( \mathbb{E}[\epsilon_k] = n \). The average NEES over \( N \) independent trials is

\[ \bar{\epsilon} = \frac{1}{N}\sum_{k=1}^N \epsilon_k, \qquad N\bar{\epsilon} \sim \chi_{Nn}^2 \]

A practical consistency test at confidence level \( 1-\alpha \) is

\[ \chi^2_{Nn,\alpha/2} < N\bar{\epsilon} < \chi^2_{Nn,1-\alpha/2} \]

Likewise, for sensor innovations \( \boldsymbol{\nu}_k \) and innovation covariance \( \mathbf{S}_k \), the normalized innovation squared is

\[ \eta_k = \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1} \boldsymbol{\nu}_k \sim \chi_m^2 \]

where \( m \) is the measurement dimension. NEES/NIS are essential because two estimators may have similar ATE but one can be dangerously overconfident (underestimated covariance).

SLAM quality is not only trajectory quality. A trajectory can look accurate while the map is topologically inconsistent or geometrically warped. The metric depends on map representation.

Occupancy grid maps: let \( q_i \in \{0,1\} \) be the ground-truth occupancy of cell \( i \) and \( p_i \in (0,1) \) the predicted occupancy probability. A proper probabilistic scoring rule is the Bernoulli cross-entropy:

\[ \mathcal{L}_{\text{CE} } = -\frac{1}{M}\sum_{i=1}^M \left[q_i\log p_i + (1-q_i)\log(1-p_i)\right] \]

A calibration-oriented alternative is the Brier score:

\[ \mathcal{L}_{\text{Brier} } = \frac{1}{M}\sum_{i=1}^M (p_i-q_i)^2 \]

If a binary threshold map is required (e.g., for navigation), define \( \hat{q}_i = \mathbf{1}[p_i \ge \gamma] \) and report precision/recall/F1/IoU:

\[ \text{Precision} = \frac{TP}{TP+FP}, \quad \text{Recall} = \frac{TP}{TP+FN}, \quad F_1 = \frac{2\,\text{Precision}\,\text{Recall} }{\text{Precision}+\text{Recall} }, \\ \text{IoU} = \frac{TP}{TP+FP+FN} \]

Landmark or point-set maps: compare point sets \( \mathcal{P} \) and \( \mathcal{Q} \) using Chamfer or Hausdorff distance. Chamfer distance is robust for dense maps:

\[ d_{\text{Chamfer} }(\mathcal{P},\mathcal{Q}) = \frac{1}{|\mathcal{P}|}\sum_{\mathbf{p}\in\mathcal{P} } \min_{\mathbf{q}\in\mathcal{Q} } \|\mathbf{p}-\mathbf{q}\|_2 + \frac{1}{|\mathcal{Q}|}\sum_{\mathbf{q}\in\mathcal{Q} } \min_{\mathbf{p}\in\mathcal{P} } \|\mathbf{q}-\mathbf{p}\|_2 \]

Pose-graph optimization diagnostics: report residual statistics, robust-kernel inlier ratio, and loop-closure precision-recall. For each candidate loop edge score threshold \( \lambda \), compute:

\[ \text{PR}(\lambda) = \big(\text{Precision}(\lambda),\text{Recall}(\lambda)\big) \]

This is critical because a few false loop closures can catastrophically corrupt a map even when ATE is low on short segments.

A metric is only as good as the protocol around it. For AMR benchmarking, always specify: dataset splits, calibration source, synchronization method, alignment convention, failure handling (e.g., lost track), and aggregation rules (mean/median/percentiles).

Suppose two systems A and B are tested on the same \( N \) runs and the scalar metric of interest (e.g., ATE RMSE) is \( m_i^A, m_i^B \). A paired comparison should be based on differences \( d_i = m_i^A - m_i^B \). The sample mean and variance are

\[ \bar{d} = \frac{1}{N}\sum_{i=1}^N d_i, \qquad s_d^2 = \frac{1}{N-1}\sum_{i=1}^N (d_i-\bar{d})^2 \]

For moderate \( N \), a confidence interval for the mean difference is

\[ \bar{d} \pm t_{N-1,1-\alpha/2}\frac{s_d}{\sqrt{N} } \]

If the interval excludes zero, the performance difference is statistically significant under the paired t-model assumptions. For heavy-tailed failures, use bootstrap confidence intervals.

Runtime reporting should include percentile latency, not only average runtime. For a sequence of frame times \( \ell_1,\dots,\ell_T \), report \( \operatorname{median}(\ell) \), \( P95(\ell) \), and \( P99(\ell) \). Tail latency is often what breaks real-time navigation.

flowchart TD
  A["Define scenario matrix"] --> B["Run repeated trials with fixed seeds"]
  B --> C["Log poses, covariances, loop closures, runtime"]
  C --> D["Compute ATE/RPE and drift"]
  C --> E["Compute NEES/NIS"]
  C --> F["Compute map metrics and PR curves"]
  D --> G["Aggregate mean/median/p95"]
  E --> G
  F --> G
  G --> H["Paired significance tests and confidence intervals"]
  H --> I["Publish benchmark protocol and raw logs"]
        

In Python, common evaluation stacks include numpy, scipy, pandas, and robotics-specific tools such as evo (trajectory evaluation), rosbag/rosbags for log parsing, and open3d for point-map processing. The following script implements: SE(2) alignment, ATE, RPE, ANEES/NIS, and occupancy-grid metrics.

Code: Chapter19_Lesson1.py

# Chapter19_Lesson1.py
# Metrics for Localization and SLAM (Autonomous Mobile Robots)
# Python reference implementation: SE(2) alignment, ATE, RPE, NEES/NIS, occupancy metrics

import numpy as np

def wrap_to_pi(angle):
    return (angle + np.pi) % (2.0 * np.pi) - np.pi

def align_se2(gt_xy, est_xy):
    """Least-squares rigid alignment in 2D (rotation + translation, no scale)."""
    gt_xy = np.asarray(gt_xy, dtype=float)
    est_xy = np.asarray(est_xy, dtype=float)
    c_gt = gt_xy.mean(axis=0)
    c_est = est_xy.mean(axis=0)

    X = est_xy - c_est
    Y = gt_xy - c_gt

    # 2D orthogonal Procrustes via closed-form angle
    s = np.sum(X[:, 0] * Y[:, 1] - X[:, 1] * Y[:, 0])
    c = np.sum(X[:, 0] * Y[:, 0] + X[:, 1] * Y[:, 1])
    theta = np.arctan2(s, c)

    R = np.array([[np.cos(theta), -np.sin(theta)],
                  [np.sin(theta),  np.cos(theta)]])
    t = c_gt - R @ c_est
    return R, t, theta

def apply_se2(xy, yaw, R, t, theta):
    xy = np.asarray(xy, dtype=float)
    yaw = np.asarray(yaw, dtype=float)
    xy_aligned = (R @ xy.T).T + t
    yaw_aligned = wrap_to_pi(yaw + theta)
    return xy_aligned, yaw_aligned

def ate_rmse(gt_xy, est_xy_aligned):
    e = np.asarray(gt_xy) - np.asarray(est_xy_aligned)
    return float(np.sqrt(np.mean(np.sum(e * e, axis=1))))

def rpe(gt_xy, gt_yaw, est_xy, est_yaw, delta=1):
    gt_xy = np.asarray(gt_xy); est_xy = np.asarray(est_xy)
    gt_yaw = np.asarray(gt_yaw); est_yaw = np.asarray(est_yaw)
    trans_err = []
    rot_err = []
    for k in range(len(gt_xy) - delta):
        dg = gt_xy[k + delta] - gt_xy[k]
        de = est_xy[k + delta] - est_xy[k]
        # rotate into local frame not necessary for norm metric; norm is invariant
        trans_err.append(np.linalg.norm(dg - de))
        dpsi_g = wrap_to_pi(gt_yaw[k + delta] - gt_yaw[k])
        dpsi_e = wrap_to_pi(est_yaw[k + delta] - est_yaw[k])
        rot_err.append(abs(wrap_to_pi(dpsi_g - dpsi_e)))
    trans_err = np.array(trans_err)
    rot_err = np.array(rot_err)
    return { {
        "rpe_trans_rmse": float(np.sqrt(np.mean(trans_err ** 2))),
        "rpe_rot_rmse_rad": float(np.sqrt(np.mean(rot_err ** 2))),
        "rpe_trans_series": trans_err,
        "rpe_rot_series": rot_err,
    } }

def anees(errors, covariances):
    """Average normalized estimation error squared.
    errors: (N, n), covariances: (N, n, n)
    """
    errors = np.asarray(errors, dtype=float)
    covariances = np.asarray(covariances, dtype=float)
    vals = []
    for e, P in zip(errors, covariances):
        vals.append(float(e.T @ np.linalg.inv(P) @ e))
    vals = np.array(vals)
    return float(np.mean(vals)), vals

def nis(innovations, S_mats):
    innovations = np.asarray(innovations, dtype=float)
    S_mats = np.asarray(S_mats, dtype=float)
    vals = []
    for nu, S in zip(innovations, S_mats):
        vals.append(float(nu.T @ np.linalg.inv(S) @ nu))
    vals = np.array(vals)
    return float(np.mean(vals)), vals

def occupancy_metrics(prob_map, gt_binary, thr=0.5, eps=1e-9):
    """prob_map in [0,1], gt_binary in { {0,1} }"""
    p = np.clip(np.asarray(prob_map, dtype=float), eps, 1 - eps)
    q = np.asarray(gt_binary, dtype=float)
    ce = -np.mean(q * np.log(p) + (1 - q) * np.log(1 - p))
    brier = np.mean((p - q) ** 2)

    pred = (p >= thr).astype(int)
    tp = int(np.sum((pred == 1) & (q == 1)))
    fp = int(np.sum((pred == 1) & (q == 0)))
    fn = int(np.sum((pred == 0) & (q == 1)))
    tn = int(np.sum((pred == 0) & (q == 0)))

    precision = tp / (tp + fp + 1e-12)
    recall = tp / (tp + fn + 1e-12)
    f1 = 2 * precision * recall / (precision + recall + 1e-12)
    iou = tp / (tp + fp + fn + 1e-12)

    return { {
        "cross_entropy": float(ce),
        "brier": float(brier),
        "tp": tp, "fp": fp, "fn": fn, "tn": tn,
        "precision": float(precision),
        "recall": float(recall),
        "f1": float(f1),
        "iou": float(iou),
    } }

def demo():
    np.random.seed(19)
    N = 200
    t = np.linspace(0, 20, N)
    gt_xy = np.column_stack([0.5 * t, np.sin(0.4 * t) * 2.0])
    gt_yaw = np.arctan2(np.gradient(gt_xy[:,1]), np.gradient(gt_xy[:,0]))

    # Estimated trajectory: global transform + local drift + noise
    theta0 = np.deg2rad(7.0)
    R0 = np.array([[np.cos(theta0), -np.sin(theta0)],
                   [np.sin(theta0),  np.cos(theta0)]])
    tr0 = np.array([1.5, -0.8])
    drift = np.column_stack([0.01 * t, -0.004 * t])
    est_xy = (R0.T @ (gt_xy - tr0).T).T + drift + 0.03 * np.random.randn(N, 2)
    est_yaw = wrap_to_pi(gt_yaw - theta0 + 0.01 * np.sin(0.2 * t) + 0.01 * np.random.randn(N))

    # Align estimated to ground truth
    R, tt, theta = align_se2(gt_xy, est_xy)
    est_xy_a, est_yaw_a = apply_se2(est_xy, est_yaw, R, tt, theta)

    ate = ate_rmse(gt_xy, est_xy_a)
    rpe_stats = rpe(gt_xy, gt_yaw, est_xy_a, est_yaw_a, delta=5)

    # Simulated filter consistency (pose error x,y,psi)
    n = 3
    errs = np.column_stack([
        0.05 * np.random.randn(N),
        0.05 * np.random.randn(N),
        np.deg2rad(1.0) * np.random.randn(N)
    ])
    Pk = np.zeros((N, n, n))
    for k in range(N):
        Pk[k] = np.diag([0.05**2, 0.05**2, np.deg2rad(1.1)**2])
    anees_mean, anees_vals = anees(errs, Pk)

    # Simulated innovation consistency (range-bearing sensor)
    m = 2
    nu = np.column_stack([0.08 * np.random.randn(N), np.deg2rad(2.0) * np.random.randn(N)])
    Sk = np.zeros((N, m, m))
    for k in range(N):
        Sk[k] = np.diag([0.1**2, np.deg2rad(2.5)**2])
    nis_mean, nis_vals = nis(nu, Sk)

    # Occupancy grid metrics
    H, W = 80, 100
    yy, xx = np.mgrid[0:H, 0:W]
    gt_occ = (((xx - 40)**2 + (yy - 35)**2) < 14**2).astype(int)
    gt_occ |= (((xx > 70) & (yy > 10) & (yy < 55))).astype(int)

    logits = -2.2 + 4.8 * gt_occ + 0.8 * np.random.randn(H, W)
    p_occ = 1.0 / (1.0 + np.exp(-logits))
    occ_stats = occupancy_metrics(p_occ, gt_occ, thr=0.5)

    print("=== Chapter19 Lesson1 Metrics Demo ===")
    print(f"ATE RMSE [m]: { {ate:.4f} }")
    print(f"RPE translational RMSE [m] (delta=5): { {rpe_stats['rpe_trans_rmse']:.4f} }")
    print(f"RPE rotational RMSE [rad] (delta=5): { {rpe_stats['rpe_rot_rmse_rad']:.6f} }")
    print(f"ANEES mean (n=3 expected near 3): { {anees_mean:.4f} }")
    print(f"NIS mean (m=2 expected near 2): { {nis_mean:.4f} }")
    print("Occupancy metrics:", occ_stats)

if __name__ == "__main__":
    demo()

      

In production AMR stacks, C++ is typical for real-time evaluation and integration with SLAM libraries such as Eigen, Sophus, GTSAM, g2o, Ceres, and PCL. The example below uses Eigen for matrix operations and implements SE(2) alignment, ATE, and RPE.

Code: Chapter19_Lesson1.cpp

// Chapter19_Lesson1.cpp
// Metrics for Localization and SLAM (Autonomous Mobile Robots)
// C++ reference implementation using Eigen (SE(2) alignment, ATE, RPE)

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

struct Pose2 { {
    double x;
    double y;
    double yaw;
} };

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

struct SE2AlignResult { {
    Eigen::Matrix2d R;
    Eigen::Vector2d t;
    double theta;
} };

SE2AlignResult alignSE2(const std::vector<Pose2>& gt, const std::vector<Pose2>& est) { {
    const int N = static_cast<int>(gt.size());
    Eigen::Vector2d c_gt(0, 0), c_est(0, 0);

    for (int i = 0; i < N; ++i) { {
        c_gt += Eigen::Vector2d(gt[i].x, gt[i].y);
        c_est += Eigen::Vector2d(est[i].x, est[i].y);
    } }
    c_gt /= N;
    c_est /= N;

    double s = 0.0, c = 0.0;
    for (int i = 0; i < N; ++i) { {
        Eigen::Vector2d X(est[i].x, est[i].y); X -= c_est;
        Eigen::Vector2d Y(gt[i].x, gt[i].y);   Y -= c_gt;
        s += X.x() * Y.y() - X.y() * Y.x();
        c += X.x() * Y.x() + X.y() * Y.y();
    } }
    double theta = std::atan2(s, c);

    Eigen::Matrix2d R;
    R << std::cos(theta), -std::sin(theta),
         std::sin(theta),  std::cos(theta);

    Eigen::Vector2d t = c_gt - R * c_est;
    return { {R, t, theta} };
} }

std::vector<Pose2> applySE2(const std::vector<Pose2>& est, const SE2AlignResult& A) { {
    std::vector<Pose2> out = est;
    for (size_t i = 0; i < est.size(); ++i) { {
        Eigen::Vector2d p(est[i].x, est[i].y);
        Eigen::Vector2d q = A.R * p + A.t;
        out[i].x = q.x();
        out[i].y = q.y();
        out[i].yaw = wrapToPi(est[i].yaw + A.theta);
    } }
    return out;
} }

double ateRMSE(const std::vector<Pose2>& gt, const std::vector<Pose2>& est) { {
    double s = 0.0;
    for (size_t i = 0; i < gt.size(); ++i) { {
        double dx = gt[i].x - est[i].x;
        double dy = gt[i].y - est[i].y;
        s += dx * dx + dy * dy;
    } }
    return std::sqrt(s / gt.size());
} }

std::pair<double,double> rpeRMSE(const std::vector<Pose2>& gt, const std::vector<Pose2>& est, int delta) { {
    std::vector<double> et, er;
    for (int k = 0; k + delta < static_cast<int>(gt.size()); ++k) { {
        double dgx = gt[k + delta].x - gt[k].x;
        double dgy = gt[k + delta].y - gt[k].y;
        double dex = est[k + delta].x - est[k].x;
        double dey = est[k + delta].y - est[k].y;

        double dt = std::hypot(dgx - dex, dgy - dey);
        et.push_back(dt);

        double dpg = wrapToPi(gt[k + delta].yaw - gt[k].yaw);
        double dpe = wrapToPi(est[k + delta].yaw - est[k].yaw);
        er.push_back(std::fabs(wrapToPi(dpg - dpe)));
    } }

    auto rmse = [](const std::vector<double>& v) { {
        double s = 0.0;
        for (double x : v) s += x * x;
        return std::sqrt(s / v.size());
    } };
    return { {rmse(et), rmse(er)} };
} }

int main() { {
    std::mt19937 gen(19);
    std::normal_distribution<double> nxy(0.0, 0.03);
    std::normal_distribution<double> nyaw(0.0, 0.01);

    const int N = 200;
    std::vector<Pose2> gt(N), est(N);

    double theta0 = 7.0 * M_PI / 180.0;
    Eigen::Matrix2d R0;
    R0 << std::cos(theta0), -std::sin(theta0),
          std::sin(theta0),  std::cos(theta0);
    Eigen::Vector2d t0(1.5, -0.8);

    std::vector<double> tx(N), ty(N);
    for (int i = 0; i < N; ++i) { {
        double t = 20.0 * i / (N - 1.0);
        tx[i] = 0.5 * t;
        ty[i] = 2.0 * std::sin(0.4 * t);
    } }
    for (int i = 0; i < N; ++i) { {
        double dx = (i == 0 ? tx[1] - tx[0] : tx[i] - tx[i - 1]);
        double dy = (i == 0 ? ty[1] - ty[0] : ty[i] - ty[i - 1]);
        gt[i].x = tx[i];
        gt[i].y = ty[i];
        gt[i].yaw = std::atan2(dy, dx);

        Eigen::Vector2d p_gt(gt[i].x, gt[i].y);
        Eigen::Vector2d drift(0.01 * (20.0 * i / (N - 1.0)), -0.004 * (20.0 * i / (N - 1.0)));
        Eigen::Vector2d p_est = R0.transpose() * (p_gt - t0) + drift + Eigen::Vector2d(nxy(gen), nxy(gen));

        est[i].x = p_est.x();
        est[i].y = p_est.y();
        est[i].yaw = wrapToPi(gt[i].yaw - theta0 + 0.01 * std::sin(0.2 * (20.0 * i / (N - 1.0))) + nyaw(gen));
    } }

    SE2AlignResult A = alignSE2(gt, est);
    auto estAligned = applySE2(est, A);

    double ate = ateRMSE(gt, estAligned);
    auto rpe = rpeRMSE(gt, estAligned, 5);

    std::cout << "=== Chapter19 Lesson1 Metrics Demo (C++) ===\\n";
    std::cout << "ATE RMSE [m]: " << ate << "\\n";
    std::cout << "RPE translational RMSE [m] (delta=5): " << rpe.first << "\\n";
    std::cout << "RPE rotational RMSE [rad] (delta=5): " << rpe.second << "\\n";
    std::cout << "Estimated global alignment theta [deg]: " << (A.theta * 180.0 / M_PI) << "\\n";
    return 0;
} }

      

Java is less common for core SLAM research, but it is useful for tooling, analytics dashboards, educational simulators, and integration with enterprise systems. For robotics applications, teams often use EJML or Apache Commons Math for linear algebra and rosjava for ROS integration. The code below keeps dependencies minimal and implements SE(2)-based trajectory metrics directly.

Code: Chapter19_Lesson1.java

// Chapter19_Lesson1.java
// Metrics for Localization and SLAM (Autonomous Mobile Robots)
// Java reference implementation: SE(2) alignment, ATE, RPE (2D)

import java.util.Random;

public class Chapter19_Lesson1 { {

    static class Pose2 { {
        double x, y, yaw;
        Pose2(double x, double y, double yaw) { {
            this.x = x; this.y = y; this.yaw = yaw;
        } }
    } }

    static class AlignResult { {
        double[][] R;   // 2x2
        double[] t;     // 2x1
        double theta;
        AlignResult(double[][] R, double[] t, double theta) { {
            this.R = R; this.t = t; this.theta = theta;
        } }
    } }

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

    static AlignResult alignSE2(Pose2[] gt, Pose2[] est) { {
        int N = gt.length;
        double cgx = 0, cgy = 0, cex = 0, cey = 0;
        for (int i = 0; i < N; i++) { {
            cgx += gt[i].x; cgy += gt[i].y;
            cex += est[i].x; cey += est[i].y;
        } }
        cgx /= N; cgy /= N; cex /= N; cey /= N;

        double s = 0.0, c = 0.0;
        for (int i = 0; i < N; i++) { {
            double Xx = est[i].x - cex, Xy = est[i].y - cey;
            double Yx = gt[i].x - cgx,  Yy = gt[i].y - cgy;
            s += Xx * Yy - Xy * Yx;
            c += Xx * Yx + Xy * Yy;
        } }
        double theta = Math.atan2(s, c);
        double ct = Math.cos(theta), st = Math.sin(theta);

        double[][] R = new double[][] { {
            { { ct, -st } },
            { { st,  ct } }
        } };
        double[] t = new double[] { {
            cgx - (R[0][0] * cex + R[0][1] * cey),
            cgy - (R[1][0] * cex + R[1][1] * cey)
        } };
        return new AlignResult(R, t, theta);
    } }

    static Pose2[] applySE2(Pose2[] est, AlignResult A) { {
        Pose2[] out = new Pose2[est.length];
        for (int i = 0; i < est.length; i++) { {
            double x = est[i].x, y = est[i].y;
            double xa = A.R[0][0] * x + A.R[0][1] * y + A.t[0];
            double ya = A.R[1][0] * x + A.R[1][1] * y + A.t[1];
            out[i] = new Pose2(xa, ya, wrapToPi(est[i].yaw + A.theta));
        } }
        return out;
    } }

    static double ateRMSE(Pose2[] gt, Pose2[] est) { {
        double s = 0.0;
        for (int i = 0; i < gt.length; i++) { {
            double dx = gt[i].x - est[i].x;
            double dy = gt[i].y - est[i].y;
            s += dx * dx + dy * dy;
        } }
        return Math.sqrt(s / gt.length);
    } }

    static double[] rpeRMSE(Pose2[] gt, Pose2[] est, int delta) { {
        int M = gt.length - delta;
        double sT = 0.0, sR = 0.0;
        for (int k = 0; k < M; k++) { {
            double dgx = gt[k + delta].x - gt[k].x;
            double dgy = gt[k + delta].y - gt[k].y;
            double dex = est[k + delta].x - est[k].x;
            double dey = est[k + delta].y - est[k].y;
            double dt = Math.hypot(dgx - dex, dgy - dey);
            sT += dt * dt;

            double dyg = wrapToPi(gt[k + delta].yaw - gt[k].yaw);
            double dye = wrapToPi(est[k + delta].yaw - est[k].yaw);
            double dr = Math.abs(wrapToPi(dyg - dye));
            sR += dr * dr;
        } }
        return new double[] { { Math.sqrt(sT / M), Math.sqrt(sR / M) } };
    } }

    public static void main(String[] args) { {
        Random rng = new Random(19);
        int N = 200;
        Pose2[] gt = new Pose2[N];
        Pose2[] est = new Pose2[N];

        double theta0 = Math.toRadians(7.0);
        double c0 = Math.cos(theta0), s0 = Math.sin(theta0);

        double[] tx = new double[N];
        double[] ty = new double[N];

        for (int i = 0; i < N; i++) { {
            double tt = 20.0 * i / (N - 1.0);
            tx[i] = 0.5 * tt;
            ty[i] = 2.0 * Math.sin(0.4 * tt);
        } }

        for (int i = 0; i < N; i++) { {
            double dx = (i == 0) ? tx[1] - tx[0] : tx[i] - tx[i - 1];
            double dy = (i == 0) ? ty[1] - ty[0] : ty[i] - ty[i - 1];
            double yaw = Math.atan2(dy, dx);
            gt[i] = new Pose2(tx[i], ty[i], yaw);

            // inverse global transform + drift + noise
            double gx = gt[i].x - 1.5;
            double gy = gt[i].y + 0.8;
            double ex0 =  c0 * gx + s0 * gy;
            double ey0 = -s0 * gx + c0 * gy;

            double tt = 20.0 * i / (N - 1.0);
            double ex = ex0 + 0.01 * tt + 0.03 * rng.nextGaussian();
            double ey = ey0 - 0.004 * tt + 0.03 * rng.nextGaussian();
            double eyaw = wrapToPi(yaw - theta0 + 0.01 * Math.sin(0.2 * tt) + 0.01 * rng.nextGaussian());
            est[i] = new Pose2(ex, ey, eyaw);
        } }

        AlignResult A = alignSE2(gt, est);
        Pose2[] estA = applySE2(est, A);
        double ate = ateRMSE(gt, estA);
        double[] rpe = rpeRMSE(gt, estA, 5);

        System.out.println("=== Chapter19 Lesson1 Metrics Demo (Java) ===");
        System.out.printf("ATE RMSE [m]: %.6f%n", ate);
        System.out.printf("RPE translational RMSE [m] (delta=5): %.6f%n", rpe[0]);
        System.out.printf("RPE rotational RMSE [rad] (delta=5): %.6f%n", rpe[1]);
        System.out.printf("Estimated global alignment theta [deg]: %.4f%n", Math.toDegrees(A.theta));
    } }
} }

      

MATLAB is widely used in university AMR labs for rapid analysis and visualization. In practice, Robotics System Toolbox and Navigation Toolbox provide ready-made utilities, but it is important for students to implement core metrics from scratch at least once. The script below computes SE(2) alignment, ATE, RPE, and consistency metrics. In Simulink, the same functions can be placed inside a MATLAB Function block to evaluate logs online or after simulation.

Code: Chapter19_Lesson1.m

% Chapter19_Lesson1.m
% Metrics for Localization and SLAM (Autonomous Mobile Robots)
% MATLAB reference implementation: SE(2) alignment, ATE, RPE, ANEES/NIS

clear; clc; rng(19);

N = 200;
tt = linspace(0,20,N)';
gt_xy = [0.5*tt, 2*sin(0.4*tt)];
dxy = [diff(gt_xy); gt_xy(end,:) - gt_xy(end-1,:)];
gt_yaw = atan2(dxy(:,2), dxy(:,1));

theta0 = deg2rad(7.0);
R0 = [cos(theta0) -sin(theta0); sin(theta0) cos(theta0)];
t0 = [1.5; -0.8];
drift = [0.01*tt, -0.004*tt];

est_xy = (R0' * (gt_xy' - t0))' + drift + 0.03*randn(N,2);
est_yaw = wrapToPi(gt_yaw - theta0 + 0.01*sin(0.2*tt) + 0.01*randn(N,1));

[R, t, theta] = alignSE2(gt_xy, est_xy);
est_xy_a = (R * est_xy')' + t';
est_yaw_a = wrapToPi(est_yaw + theta);

ate = sqrt(mean(sum((gt_xy - est_xy_a).^2, 2)));
[rpe_t, rpe_r] = rpeMetrics(gt_xy, gt_yaw, est_xy_a, est_yaw_a, 5);

% ANEES demo (state dimension n=3)
n = 3;
errs = [0.05*randn(N,1), 0.05*randn(N,1), deg2rad(1.0)*randn(N,1)];
Pk = repmat(diag([0.05^2, 0.05^2, deg2rad(1.1)^2]), 1, 1, N);
nees_vals = zeros(N,1);
for k = 1:N
    e = errs(k,:)';
    P = Pk(:,:,k);
    nees_vals(k) = e' * (P \ e);
end
anees_val = mean(nees_vals);

% NIS demo (measurement dimension m=2)
m = 2;
nu = [0.08*randn(N,1), deg2rad(2.0)*randn(N,1)];
Sk = repmat(diag([0.1^2, deg2rad(2.5)^2]), 1, 1, N);
nis_vals = zeros(N,1);
for k = 1:N
    v = nu(k,:)';
    S = Sk(:,:,k);
    nis_vals(k) = v' * (S \ v);
end
anis_val = mean(nis_vals);

fprintf('=== Chapter19 Lesson1 Metrics Demo (MATLAB) ===\n');
fprintf('ATE RMSE [m]: %.6f\n', ate);
fprintf('RPE translational RMSE [m] (delta=5): %.6f\n', rpe_t);
fprintf('RPE rotational RMSE [rad] (delta=5): %.6f\n', rpe_r);
fprintf('ANEES mean (n=3 expected near 3): %.6f\n', anees_val);
fprintf('NIS mean (m=2 expected near 2): %.6f\n', anis_val);
fprintf('Estimated global alignment theta [deg]: %.4f\n', rad2deg(theta));

function [R, t, theta] = alignSE2(gt_xy, est_xy)
    c_gt = mean(gt_xy,1);
    c_est = mean(est_xy,1);

    X = est_xy - c_est;
    Y = gt_xy - c_gt;

    s = sum(X(:,1).*Y(:,2) - X(:,2).*Y(:,1));
    c = sum(X(:,1).*Y(:,1) + X(:,2).*Y(:,2));
    theta = atan2(s, c);

    R = [cos(theta) -sin(theta); sin(theta) cos(theta)];
    t = c_gt' - R * c_est';
end

function [rpe_t_rmse, rpe_r_rmse] = rpeMetrics(gt_xy, gt_yaw, est_xy, est_yaw, delta)
    M = size(gt_xy,1) - delta;
    et = zeros(M,1);
    er = zeros(M,1);
    for k = 1:M
        dg = gt_xy(k+delta,:) - gt_xy(k,:);
        de = est_xy(k+delta,:) - est_xy(k,:);
        et(k) = norm(dg - de, 2);

        dyg = wrapToPi(gt_yaw(k+delta) - gt_yaw(k));
        dye = wrapToPi(est_yaw(k+delta) - est_yaw(k));
        er(k) = abs(wrapToPi(dyg - dye));
    end
    rpe_t_rmse = sqrt(mean(et.^2));
    rpe_r_rmse = sqrt(mean(er.^2));
end

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

      

Wolfram Mathematica is excellent for symbolic derivations, analytic checks, and compact numerical prototypes. It is particularly useful for verifying derivations (e.g., optimal alignment or covariance identities) before porting to embedded code. The following notebook-style code computes SE(2) alignment, ATE, and RPE.

Code: Chapter19_Lesson1.nb

(* Chapter19_Lesson1.nb *)
(* Metrics for Localization and SLAM (Autonomous Mobile Robots) *)
(* Wolfram Language implementation: SE(2) alignment, ATE, RPE *)

ClearAll["Global`*"];
SeedRandom[19];

wrapToPi[a_] := Mod[a + Pi, 2 Pi] - Pi;

alignSE2[gtXY_, estXY_] := Module[
  { {cgt, cest, X, Y, s, c, th, R, t} },
  cgt = Mean[gtXY];
  cest = Mean[estXY];
  X = estXY - cest;
  Y = gtXY - cgt;
  s = Total[X[[All, 1]]*Y[[All, 2]] - X[[All, 2]]*Y[[All, 1]]];
  c = Total[X[[All, 1]]*Y[[All, 1]] + X[[All, 2]]*Y[[All, 2]]];
  th = ArcTan[c, s]; (* equals atan2(s, c) *)
  R = { { { {Cos[th], -Sin[th]} }, { {Sin[th], Cos[th]} } } } /. { { { {a_, b_} }, { {c1_, d_} } } } :> { {a, b}, {c1, d} };
  t = cgt - R.cest;
  <|"R" -> R, "t" -> t, "theta" -> th|>
];

applySE2[xy_, yaw_, A_] := Module[
  { {xyA, yawA} },
  xyA = (A["R"].# + A["t"]) & /@ xy;
  yawA = wrapToPi /@ (yaw + A["theta"]);
  <|"xy" -> xyA, "yaw" -> yawA|>
];

ateRMSE[gtXY_, estXY_] := Module[
  { {e} },
  e = gtXY - estXY;
  Sqrt[Mean[(Norm /@ e)^2]]
];

rpeRMSE[gtXY_, gtYaw_, estXY_, estYaw_, delta_Integer] := Module[
  { {M, et, er, dg, de, dyg, dye} },
  M = Length[gtXY] - delta;
  et = Table[
    dg = gtXY[[k + delta]] - gtXY[[k]];
    de = estXY[[k + delta]] - estXY[[k]];
    Norm[dg - de],
    { {k, 1, M} }
  ];
  er = Table[
    dyg = wrapToPi[gtYaw[[k + delta]] - gtYaw[[k]]];
    dye = wrapToPi[estYaw[[k + delta]] - estYaw[[k]]];
    Abs[wrapToPi[dyg - dye]],
    { {k, 1, M} }
  ];
  <|
    "RPETransRMSE" -> Sqrt[Mean[et^2]],
    "RPERotRMSE" -> Sqrt[Mean[er^2]]
  |>
];

Nn = 200;
tt = N@Subdivide[0, 20, Nn - 1];
gtXY = Transpose[{ {0.5 tt, 2 Sin[0.4 tt]} }] /. { {a_, b_} } :> {a, b};
dxy = Differences[gtXY];
dxy = Append[dxy, Last[dxy]];
gtYaw = MapThread[ArcTan[#1, #2] &, { {dxy[[All, 1]], dxy[[All, 2]]} }] /. { {vals_} } :> vals;

theta0 = 7 Degree;
R0 = { {Cos[theta0], -Sin[theta0]}, {Sin[theta0], Cos[theta0]} };
t0 = {1.5, -0.8};
drift = Transpose[{ {0.01 tt, -0.004 tt} }] /. { {a_, b_} } :> {a, b};

estXY = ((Transpose[R0].(# - t0)) & /@ gtXY) + drift + RandomVariate[NormalDistribution[0, 0.03], { {Nn, 2} }] /. { { { {arr_} } } } :> arr;
estYaw = wrapToPi /@ (gtYaw - theta0 + 0.01 Sin[0.2 tt] + RandomVariate[NormalDistribution[0, 0.01], Nn]);

A = alignSE2[gtXY, estXY];
aligned = applySE2[estXY, estYaw, A];

ate = ateRMSE[gtXY, aligned["xy"]];
rpe = rpeRMSE[gtXY, gtYaw, aligned["xy"], aligned["yaw"], 5];

Print["=== Chapter19 Lesson1 Metrics Demo (Mathematica) ==="];
Print["ATE RMSE [m]: ", NumberForm[ate, { {8, 6} }] /. { {a_, b_} } :> Sequence[a, b]];
Print["RPE translational RMSE [m] (delta=5): ", NumberForm[rpe["RPETransRMSE"], { {8, 6} }] /. { {a_, b_} } :> Sequence[a, b]];
Print["RPE rotational RMSE [rad] (delta=5): ", NumberForm[rpe["RPERotRMSE"], { {8, 6} }] /. { {a_, b_} } :> Sequence[a, b]];
Print["Estimated global alignment theta [deg]: ", NumberForm[A["theta"]/Degree, { {8, 4} }] /. { {a_, b_} } :> Sequence[a, b]];

      

Problem 1 (Optimal translation in trajectory alignment): Consider the planar alignment objective \( J(\mathbf{R},\mathbf{t})=\sum_{k=1}^N \|\mathbf{p}_k-(\mathbf{R}\hat{\mathbf{p} }_k+\mathbf{t})\|_2^2 \). Show that for fixed \( \mathbf{R} \), the unique minimizer in \( \mathbf{t} \) is \( \bar{\mathbf{p} }-\mathbf{R}\bar{\hat{\mathbf{p} } } \).

Solution: Expand the quadratic in \( \mathbf{t} \):

\[ J = \sum_{k=1}^N \left\| (\mathbf{p}_k-\mathbf{R}\hat{\mathbf{p} }_k)-\mathbf{t} \right\|_2^2 \]

Differentiate: \( \nabla_{\mathbf{t} } J = -2\sum_{k=1}^N (\mathbf{p}_k-\mathbf{R}\hat{\mathbf{p} }_k-\mathbf{t}) \). Setting the gradient to zero gives

\[ N\mathbf{t} = \sum_{k=1}^N \mathbf{p}_k - \mathbf{R}\sum_{k=1}^N \hat{\mathbf{p} }_k \quad \Longrightarrow \quad \mathbf{t}^* = \bar{\mathbf{p} } - \mathbf{R}\bar{\hat{\mathbf{p} } } \]

The Hessian with respect to \( \mathbf{t} \) is \( 2N\mathbf{I} \), which is positive definite, so the minimizer is unique.

Problem 2 (RPE invariance to global rigid transform): Let \( G \in SE(2) \) be a constant transform and define transformed trajectories \( X'_k = GX_k \), \( \hat{X}'_k = G\hat{X}_k \). Prove that the relative error element \( E_k^{(\Delta)} \) is unchanged.

Solution: Substitute into the RPE definition:

\[ (X'_k)^{-1}X'_{k+\Delta} = (GX_k)^{-1}(GX_{k+\Delta}) = X_k^{-1}G^{-1}GX_{k+\Delta} = X_k^{-1}X_{k+\Delta} \]

and similarly \( (\hat{X}'_k)^{-1}\hat{X}'_{k+\Delta} = \hat{X}_k^{-1}\hat{X}_{k+\Delta} \). Therefore,

\[ E_k^{(\Delta)\prime} = \left(\left(X_k^{\prime}\right)^{-1}X_{k+\Delta}^{\prime}\right)^{-1} \left(\left(\hat{X}_k^{\prime}\right)^{-1}\hat{X}_{k+\Delta}^{\prime}\right) = E_k^{(\Delta)} \]

Hence translational and rotational RPE are invariant to the common global frame choice.

Problem 3 (Expected NEES under consistency): Suppose \( \mathbf{e} \sim \mathcal{N}(\mathbf{0},\mathbf{P}) \) with \( \mathbf{P} \succ 0 \). Show that \( \epsilon = \mathbf{e}^\top\mathbf{P}^{-1}\mathbf{e} \) satisfies \( \mathbb{E}[\epsilon]=n \), where \( n \) is the dimension of \( \mathbf{e} \).

Solution: Let \( \mathbf{z}=\mathbf{P}^{-1/2}\mathbf{e} \). Then \( \mathbf{z}\sim\mathcal{N}(\mathbf{0},\mathbf{I}) \) and

\[ \epsilon = \mathbf{z}^\top\mathbf{z} = \sum_{i=1}^n z_i^2 \]

Since each \( z_i^2 \) has mean 1 and the components are independent,

\[ \mathbb{E}[\epsilon] = \sum_{i=1}^n \mathbb{E}[z_i^2] = n \]

This is the basis for the ANEES consistency test.

Problem 4 (Why cross-entropy is a proper occupancy metric): For one occupancy cell with true Bernoulli parameter \( q \in [0,1] \) and predicted probability \( p \in (0,1) \), the expected cross-entropy is

\[ \ell(p) = -\big(q\log p + (1-q)\log(1-p)\big) \]

Show that the unique minimizer is \( p=q \).

Solution: Differentiate:

\[ \ell'(p) = -\frac{q}{p} + \frac{1-q}{1-p} = \frac{p-q}{p(1-p)} \]

The stationary point is \( p=q \). The second derivative is

\[ \ell''(p) = \frac{q}{p^2} + \frac{1-q}{(1-p)^2} > 0 \quad \text{for } 0<p<1 \]

so the stationary point is the unique minimizer. Therefore cross-entropy is a proper scoring rule for occupancy probabilities.

Problem 5 (Paired comparison of two SLAM systems): A benchmark reports ATE RMSE on 8 matched runs. The paired differences \( d_i = m_i^A - m_i^B \) (meters) are \( [0.12, 0.08, 0.10, 0.03, 0.09, 0.05, 0.07, 0.11] \). Compute \( \bar{d} \) and interpret the sign.

Solution: Summing the values gives \( 0.65 \), so

\[ \bar{d} = \frac{0.65}{8} = 0.08125\,\text{m} \]

Because \( d_i = m_i^A - m_i^B \) is positive on average, system B has lower ATE (better trajectory accuracy) by about \( 8.1\,\text{cm} \) on matched runs. A full report should also include \( s_d \) and a confidence interval, not only the mean.

We established a university-level evaluation framework for localization and SLAM in AMR: rigidly aligned trajectory metrics (ATE), local drift metrics (RPE), probabilistic consistency metrics (NEES/NIS), map-quality metrics for occupancy and geometric maps, and statistically valid benchmark protocols. These metrics will be extended in the next lessons to full navigation robustness metrics and stress-testing procedures.