From Lessons 1–3, we already have: (i) a mobile robot state (pose and possibly velocities), and (ii) an environment representation (grid, features, or geometry). The remaining question is: how do we connect sensor signals to a navigation decision in a way that is mathematically well-posed and operationally robust?

We model the autonomy loop in discrete time \( k \in \mathbb{Z}_{\ge 0} \) with sampling interval \( \Delta t \gt 0 \). Let the robot state be \( \mathbf{x}_k \in \mathbb{R}^n \) (Lesson 2), control input \( \mathbf{u}_k \in \mathbb{R}^p \), and measurement vector \( \mathbf{y}_k \in \mathbb{R}^m \). A minimal pipeline model is:

\[ \begin{aligned} \text{Plant (motion):}\quad & \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{w}_k \\ \text{Sensors:}\quad & \mathbf{y}_k = \mathbf{h}(\mathbf{x}_k,\mathcal{E}) + \mathbf{v}_k \\ \text{Estimator:}\quad & \hat{\mathbf{x}}_k = \mathbf{g}(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k-1},\mathbf{y}_k) \\ \text{Navigation:}\quad & \mathbf{u}_k = \boldsymbol{\pi}(\hat{\mathbf{x}}_k,\mathcal{M},\mathbf{r}_k) \end{aligned} \]

Here \( \mathcal{E} \) denotes the physical environment, while \( \mathcal{M} \) is the internal environment representation (Lesson 3), and \( \mathbf{r}_k \) is the navigation objective (goal pose, waypoint, corridor, etc.). The additive terms \( \mathbf{w}_k \) and \( \mathbf{v}_k \) represent unmodeled disturbances and sensor noise.

flowchart TD
  S["Sensors: wheel encoders, IMU, LiDAR, camera"] --> P["Preprocess: sync, calibration, filtering"]
  P --> E["Estimation: x_hat + uncertainty"]
  E --> W["World model: map / costmap"]
  W --> N["Navigation: plan + command"]
  N --> A["Actuation: motors"]
  A -->|motion changes world view| S
        

Scope and assumptions for Chapter 1. In later chapters, you will implement full Bayesian filters and SLAM; in this chapter we adopt a controlled scope:

• State is low-dimensional planar motion (pose and optionally velocity).
• Map/model is treated as known or slowly varying (we focus on the interface).
• Estimator is expressed as optimization (weighted least squares) or as a generic observer.
• Navigation is an abstract policy that consumes estimates; advanced planners appear later.

Sensors measure the world through the robot’s pose. In planar mobile robotics, a common pose is \( \mathbf{x} = (x, y, \theta)^\top \) (Lesson 2). Let \( \{W\} \) be a world frame and \( \{B\} \) the robot body frame. The homogeneous transform from body to world is:

\[ \mathbf{T}_{WB}(x,y,\theta)= \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix}. \]

If a landmark point has coordinates \( \mathbf{p}^B \in \mathbb{R}^2 \) in body frame, then in world coordinates:

\[ \begin{bmatrix} \mathbf{p}^W \\ 1 \end{bmatrix} = \mathbf{T}_{WB} \begin{bmatrix} \mathbf{p}^B \\ 1 \end{bmatrix} \quad\Rightarrow\quad \mathbf{p}^W = \mathbf{R}(\theta)\,\mathbf{p}^B + \mathbf{t}, \]

with \( \mathbf{R}(\theta) \) the planar rotation and \( \mathbf{t}=(x,y)^\top \) the translation. This is the core bridge between the state and environment representations (Lesson 3): a grid cell index, a feature location, or a polygon edge becomes a predicted measurement through \( \mathbf{h}(\mathbf{x},\mathcal{E}) \).

Proposition (composition consistency). For any three frames \(A,B,C\):

\[ \mathbf{T}_{AC} = \mathbf{T}_{AB}\,\mathbf{T}_{BC}. \]

Proof. By definition, homogeneous transforms are affine maps: \( \tilde{\mathbf{p}}^A = \mathbf{T}_{AB}\tilde{\mathbf{p}}^B \) and \( \tilde{\mathbf{p}}^B = \mathbf{T}_{BC}\tilde{\mathbf{p}}^C \), where \( \tilde{\mathbf{p}} = [\mathbf{p}^\top\; 1]^\top \). Substituting gives \( \tilde{\mathbf{p}}^A = \mathbf{T}_{AB}\mathbf{T}_{BC}\tilde{\mathbf{p}}^C \). Since this holds for all points, the composed transform must be \( \mathbf{T}_{AC} = \mathbf{T}_{AB}\mathbf{T}_{BC} \). \( \square \)

This proposition is not an abstract detail: it is the mathematical mechanism behind sensor extrinsic calibration (sensor frame to body frame), map alignment (world frame choice), and multi-sensor fusion (consistent frame transformations).

The estimator’s goal is to compute \( \hat{\mathbf{x}} \) from measurements. In this lesson we adopt a classical control/estimation view: a deterministic state \( \mathbf{x} \) is inferred from noisy observations. Consider a measurement model: \( \mathbf{y} = \mathbf{h}(\mathbf{x}) + \mathbf{v} \), with \( \mathbf{v} \sim \mathcal{N}(\mathbf{0},\mathbf{R}) \).

Theorem (Gaussian ML equals WLS). Under the Gaussian noise assumption above, the maximum-likelihood estimate satisfies:

\[ \hat{\mathbf{x}}_{\mathrm{ML}} = \arg\min_{\mathbf{x}} \left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right)^\top \mathbf{R}^{-1} \left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right). \]

Proof. The likelihood is \( p(\mathbf{y}\mid\mathbf{x}) \propto \exp\left(-\tfrac{1}{2}(\mathbf{y}-\mathbf{h}(\mathbf{x}))^\top \mathbf{R}^{-1}(\mathbf{y}-\mathbf{h}(\mathbf{x}))\right) \). Maximizing the likelihood is equivalent to minimizing the negative log-likelihood; the constant terms drop, leaving exactly the weighted least-squares objective. \( \square \)

Adding a prior (odometry / prediction). In mobile robots, we nearly always have a prediction \( \mathbf{x}^{-} \) from dead-reckoning (Chapter 5) or from a process model. We encode it as a quadratic penalty (a “soft constraint”):

\[ \hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \underbrace{\left(\mathbf{x}-\mathbf{x}^{-}\right)^\top\mathbf{P}^{-1}\left(\mathbf{x}-\mathbf{x}^{-}\right)}_{\text{prior term}} + \underbrace{\left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right)^\top\mathbf{R}^{-1}\left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right)}_{\text{measurement term}}. \]

The matrix \( \mathbf{P} \) plays the role of a confidence/uncertainty matrix for the prior. Later chapters will interpret this probabilistically; here it is enough to view it as a weighting matrix with physical units.

Gauss–Newton step (local linearization). Linearize \( \mathbf{h}(\mathbf{x}) \approx \mathbf{h}(\mathbf{x}_0) + \mathbf{H}(\mathbf{x}_0)\,\delta \), where \( \mathbf{H} = \tfrac{\partial \mathbf{h}}{\partial \mathbf{x}} \) and \( \delta = \mathbf{x}-\mathbf{x}_0 \). The quadratic approximation yields normal equations:

\[ \left(\mathbf{P}^{-1} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{H}\right)\,\delta = \mathbf{H}^\top\mathbf{R}^{-1}\left(\mathbf{y}-\mathbf{h}(\mathbf{x}_0)\right) + \mathbf{P}^{-1}\left(\mathbf{x}^{-}-\mathbf{x}_0\right). \]

Solving for \( \delta \) and updating \( \mathbf{x}_0 \leftarrow \mathbf{x}_0 + \delta \) is the core numerical mechanism used in many modern estimators (including graph-based SLAM, later).

Navigation computes commands from \( \hat{\mathbf{x}} \), not from the true \( \mathbf{x} \). Define estimation error \( \tilde{\mathbf{x}}_k = \mathbf{x}_k - \hat{\mathbf{x}}_k \). Even if the controller is stabilizing for perfect state knowledge, estimation error behaves like a disturbance.

flowchart TD
  X["True state x"] -->|"y = h(x) + v"| Y["Measurements y"]
  Y --> EST["Estimator"]
  EST --> XH["Estimate x_hat"]
  XH -->|"u = pi(x_hat)"| U["Control u"]
  U --> PLANT["Robot dynamics"]
  PLANT --> X
        

To make this precise without using later probabilistic machinery, consider a simplified kinematic model in which the planar position \( \mathbf{p} \in \mathbb{R}^2 \) evolves as: \( \mathbf{p}_{k+1} = \mathbf{p}_k + \Delta t\,\mathbf{v}_k \). Suppose we command a “go-to-goal” velocity based on the estimated position: \( \mathbf{v}_k = -k\,(\hat{\mathbf{p}}_k-\mathbf{p}_g) \), with gain \( k \gt 0 \) and goal \( \mathbf{p}_g \).

Let the true position error be \( \mathbf{e}_k = \mathbf{p}_k-\mathbf{p}_g \), and the position estimation error be \( \tilde{\mathbf{p}}_k = \mathbf{p}_k-\hat{\mathbf{p}}_k \). Substituting the control law gives:

\[ \mathbf{e}_{k+1} = \mathbf{e}_k - k\Delta t\,(\mathbf{e}_k - \tilde{\mathbf{p}}_k) = (1-k\Delta t)\,\mathbf{e}_k + k\Delta t\,\tilde{\mathbf{p}}_k. \]

Proposition (ultimate bound). If \( 0 \lt k\Delta t \lt 1 \) and \( \|\tilde{\mathbf{p}}_k\| \le \varepsilon \) for all \( k \), then the tracking error satisfies:

\[ \limsup_{k\to\infty} \|\mathbf{e}_k\| \le \varepsilon. \]

Proof. Let \( a = 1-k\Delta t \), so \( |a| \lt 1 \). Unrolling the recursion yields:

\[ \mathbf{e}_k = a^k\mathbf{e}_0 + k\Delta t\sum_{i=0}^{k-1} a^{k-1-i}\tilde{\mathbf{p}}_i. \]

Taking norms and applying the bound \( \|\tilde{\mathbf{p}}_i\| \le \varepsilon \) gives \( \|\mathbf{e}_k\| \le |a|^k\|\mathbf{e}_0\| + k\Delta t\,\varepsilon\sum_{j=0}^{k-1}|a|^j \). Since the geometric sum is bounded and the transient term vanishes, we obtain \( \limsup_{k\to\infty}\|\mathbf{e}_k\| \le \varepsilon \). \( \square \)

Interpretation: a controller can only drive the robot to within the estimator’s accuracy. This motivates the “pipeline mindset”: if sensing quality drops (occlusion, slip), estimation error grows, and navigation must either slow down, re-localize, or switch modes (Chapter 14 recovery behaviors).

For early modeling and simulation (this chapter), we typically commit to a set of assumptions that make the pipeline tractable. Each assumption can later be relaxed, but doing so has mathematical consequences.

• Time synchronization: assume sensor timestamps can be aligned to the estimator time grid. Violations appear as effective noise/bias in \( \mathbf{v}_k \).
• Calibration: assume known transforms between sensor frames and body frame. If extrinsics are wrong, the measurement function \( \mathbf{h} \) is misspecified.
• Static or slowly varying world model: obstacles move slowly compared to reaction rate, or are handled by local sensing layers.
• Separation of layers: estimation outputs a state; navigation consumes the state and a map. This is an architectural assumption, not a law of nature.
• Bounded disturbances: for stability reasoning, assume \( \|\mathbf{w}_k\| \) and \( \|\mathbf{v}_k\| \) are bounded.

These assumptions are the “contract” between modules: sensing commits to providing measurements with known units and approximate error statistics; estimation commits to producing a best-effort state and a confidence measure; navigation commits to generating commands that remain safe under that confidence.

This lab implements the pipeline end-to-end in a self-contained 2D simulation: (i) a unicycle robot, (ii) noisy odometry, (iii) range-bearing measurements to known landmarks, (iv) an iterated weighted least-squares correction step, and (v) a simple go-to-goal controller. The estimator is written in optimization form (Section 3).

Chapter1_Lesson4.py

"""
Chapter1_Lesson4.py
Sensing–Estimation–Navigation pipeline demo (2D unicycle) using weighted least squares (WLS)
prior + range-bearing landmark measurements.
Dependencies: numpy, matplotlib
Optional robotics ecosystem pointers: ROS 2 (rclpy), nav2, GTSAM (not required for this demo).
"""

import numpy as np
import matplotlib.pyplot as plt

def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + np.pi) % (2.0 * np.pi) - np.pi

def unicycle_step(x: np.ndarray, v: float, w: float, dt: float) -> np.ndarray:
    """Discrete-time unicycle integration."""
    x_new = x.copy()
    th = x[2]
    x_new[0] = x[0] + v * dt * np.cos(th)
    x_new[1] = x[1] + v * dt * np.sin(th)
    x_new[2] = wrap_angle(x[2] + w * dt)
    return x_new

def meas_model(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Range-bearing measurement z = [r, b] to landmark in world coordinates."""
    dx = landmark[0] - x[0]
    dy = landmark[1] - x[1]
    r = np.hypot(dx, dy)
    b = wrap_angle(np.arctan2(dy, dx) - x[2])
    return np.array([r, b])

def meas_jacobian(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Jacobian H = d h / d x for range-bearing measurement."""
    dx = landmark[0] - x[0]
    dy = landmark[1] - x[1]
    q = dx*dx + dy*dy
    r = np.sqrt(q)

    # Avoid division by zero
    eps = 1e-9
    q = max(q, eps)
    r = max(r, eps)

    # h1 = r = sqrt((lx-x)^2 + (ly-y)^2)
    dr_dx = -(dx) / r
    dr_dy = -(dy) / r
    dr_dth = 0.0

    # h2 = b = atan2(dy, dx) - th
    db_dx =  dy / q
    db_dy = -dx / q
    db_dth = -1.0

    return np.array([[dr_dx, dr_dy, dr_dth],
                     [db_dx, db_dy, db_dth]])

def wls_update_iterated(x_prior: np.ndarray, P_prior: np.ndarray,
                        z_list: list, landmarks: np.ndarray,
                        R: np.ndarray, iters: int = 2) -> tuple[np.ndarray, np.ndarray]:
    """
    Iterated WLS (Gauss-Newton) for a prior + stacked measurements.
    Minimizes:
        (x-x_prior)^T P_prior^{-1} (x-x_prior) + sum_i (z_i - h_i(x))^T R^{-1} (z_i - h_i(x))
    """
    if len(z_list) == 0:
        return x_prior, P_prior

    # Stack measurements
    z = np.concatenate(z_list, axis=0)  # shape (2m,)
    m = len(z_list)

    # Block-diagonal measurement covariance
    R_big = np.kron(np.eye(m), R)  # (2m x 2m)
    Rinv = np.linalg.inv(R_big)

    Pinv = np.linalg.inv(P_prior)

    x = x_prior.copy()
    for _ in range(iters):
        # Build stacked residual and Jacobian
        h = []
        H = []
        for i in range(m):
            hi = meas_model(x, landmarks[i])
            h.append(hi)
            H.append(meas_jacobian(x, landmarks[i]))
        h = np.concatenate(h, axis=0)
        H = np.vstack(H)

        # Residual (bearing residual must be wrapped)
        res = z - h
        for i in range(m):
            res[2*i + 1] = wrap_angle(res[2*i + 1])

        # Solve normal equations for delta:
        # (P^{-1} + H^T R^{-1} H) delta = H^T R^{-1} res + P^{-1}(x_prior - x)
        A = Pinv + H.T @ Rinv @ H
        b = H.T @ Rinv @ res + Pinv @ (x_prior - x)
        delta = np.linalg.solve(A, b)

        x = x + delta
        x[2] = wrap_angle(x[2])

    # Posterior covariance at final linearization point
    h = []
    H = []
    for i in range(m):
        h.append(meas_model(x, landmarks[i]))
        H.append(meas_jacobian(x, landmarks[i]))
    H = np.vstack(H)
    P_post = np.linalg.inv(Pinv + H.T @ Rinv @ H)
    return x, P_post

def main():
    np.random.seed(7)

    # Map (known landmarks): choose 3 well-spread landmarks
    landmarks = np.array([[5.0, 0.0],
                          [6.0, 6.0],
                          [0.0, 6.0]])

    dt = 0.1
    N = 350

    # True state and estimate (x, y, theta)
    x_true = np.array([0.0, 0.0, 0.0])
    x_hat  = np.array([0.0, 0.0, 0.0])

    # Prior covariance
    P = np.diag([0.15**2, 0.15**2, (np.deg2rad(8.0))**2])

    # Noise levels
    sigma_v = 0.08          # m/s odometry noise
    sigma_w = np.deg2rad(3) # rad/s odometry noise
    sigma_r = 0.12          # m range noise
    sigma_b = np.deg2rad(2) # rad bearing noise

    # Navigation goal and controller gains (simple go-to-goal)
    goal = np.array([7.0, 7.0])
    k_heading = 1.6
    v_max = 0.9

    # Measurement covariance per landmark measurement
    R = np.diag([sigma_r**2, sigma_b**2])

    hist_true = np.zeros((N, 3))
    hist_hat  = np.zeros((N, 3))

    for k in range(N):
        # --- Navigation (policy uses estimate) ---
        dx = goal[0] - x_hat[0]
        dy = goal[1] - x_hat[1]
        dist = np.hypot(dx, dy)
        desired_heading = np.arctan2(dy, dx)
        heading_error = wrap_angle(desired_heading - x_hat[2])

        v_cmd = v_max * np.tanh(dist)      # smoothly saturating speed
        w_cmd = k_heading * heading_error  # proportional heading control

        # --- True motion (includes unmodeled disturbances) ---
        # Model a mild lateral slip disturbance as small random heading perturbation.
        slip = np.random.normal(0.0, np.deg2rad(0.35))
        x_true = unicycle_step(x_true, v_cmd, w_cmd + slip/dt, dt)

        # --- Sensing: odometry (control-derived) ---
        v_meas = v_cmd + np.random.normal(0.0, sigma_v)
        w_meas = w_cmd + np.random.normal(0.0, sigma_w)

        # --- Estimation: prediction from odometry ---
        x_prior = unicycle_step(x_hat, v_meas, w_meas, dt)

        th = x_hat[2]
        F = np.array([[1.0, 0.0, -v_meas*dt*np.sin(th)],
                      [0.0, 1.0,  v_meas*dt*np.cos(th)],
                      [0.0, 0.0,  1.0]])

        # Simple process noise model (odometry-induced)
        Q = np.diag([(sigma_v*dt)**2, (sigma_v*dt)**2, (sigma_w*dt)**2])
        P_prior = F @ P @ F.T + Q

        # --- Sensing: landmark measurements (range-bearing) ---
        z_list = []
        # Use all landmarks within range to emulate a perception module
        max_range = 8.0
        for lm in landmarks:
            z_true = meas_model(x_true, lm)
            if z_true[0] <= max_range:
                z_noisy = z_true + np.array([np.random.normal(0.0, sigma_r),
                                             np.random.normal(0.0, sigma_b)])
                z_noisy[1] = wrap_angle(z_noisy[1])
                z_list.append(z_noisy)

        # --- Estimation: iterated WLS correction (prior + measurements) ---
        x_hat, P = wls_update_iterated(x_prior, P_prior, z_list, landmarks[:len(z_list)], R, iters=2)

        hist_true[k] = x_true
        hist_hat[k]  = x_hat

    # --- Visualization ---
    plt.figure()
    plt.plot(hist_true[:,0], hist_true[:,1], label="true")
    plt.plot(hist_hat[:,0],  hist_hat[:,1],  label="estimated")
    plt.scatter(landmarks[:,0], landmarks[:,1], marker="x", label="landmarks")
    plt.scatter(goal[0], goal[1], marker="*", label="goal")
    plt.axis("equal")
    plt.grid(True)
    plt.legend()
    plt.title("Sensing–Estimation–Navigation loop (demo)")
    plt.show()

if __name__ == "__main__":
    main()
      

Robotics libraries to know (Python): ROS 2 (rclpy) for messaging/execution, tf2 for frame transforms, and factor-graph tools such as GTSAM for optimization-based estimation. We keep this lab ROS-free so it runs in a standard scientific Python stack.

C++ is common for real-time robot stacks (ROS 2 nodes, high-rate perception). This implementation mirrors the Python estimator and writes a CSV trajectory for inspection.

Chapter1_Lesson4.cpp

// Chapter1_Lesson4.cpp
// Sensing–Estimation–Navigation pipeline demo (2D unicycle) using prior + WLS correction.
// Dependencies: C++17. Optional: Eigen (header-only) for linear algebra.
// Build example (with Eigen in include path):
//   g++ -O2 -std=c++17 Chapter1_Lesson4.cpp -I /usr/include/eigen3 -o amr_lesson4
//
// Output: writes a CSV file "traj_ch1_l4.csv" with true and estimated pose per step.
// Typical robotics ecosystem pointers: ROS 2 (rclcpp), Eigen, GTSAM (not required for this demo).

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

static double wrapAngle(double a) {
    const double TWO_PI = 2.0 * M_PI;
    a = std::fmod(a + M_PI, TWO_PI);
    if (a < 0.0) a += TWO_PI;
    return a - M_PI;
}

static Eigen::Vector3d unicycleStep(const Eigen::Vector3d& x, double v, double w, double dt) {
    Eigen::Vector3d xn = x;
    double th = x(2);
    xn(0) = x(0) + v * dt * std::cos(th);
    xn(1) = x(1) + v * dt * std::sin(th);
    xn(2) = wrapAngle(x(2) + w * dt);
    return xn;
}

static Eigen::Vector2d measModel(const Eigen::Vector3d& x, const Eigen::Vector2d& lm) {
    double dx = lm(0) - x(0);
    double dy = lm(1) - x(1);
    double r  = std::hypot(dx, dy);
    double b  = wrapAngle(std::atan2(dy, dx) - x(2));
    return Eigen::Vector2d(r, b);
}

static Eigen::Matrix<double,2,3> measJacobian(const Eigen::Vector3d& x, const Eigen::Vector2d& lm) {
    double dx = lm(0) - x(0);
    double dy = lm(1) - x(1);
    double q = dx*dx + dy*dy;
    double r = std::sqrt(std::max(q, 1e-12));

    q = std::max(q, 1e-12);

    Eigen::Matrix<double,2,3> H;
    // range derivatives
    H(0,0) = -dx / r;
    H(0,1) = -dy / r;
    H(0,2) = 0.0;
    // bearing derivatives
    H(1,0) =  dy / q;
    H(1,1) = -dx / q;
    H(1,2) = -1.0;
    return H;
}

static void wlsUpdateIterated(
    const Eigen::Vector3d& x_prior, const Eigen::Matrix3d& P_prior,
    const std::vector<Eigen::Vector2d>& z_list, const std::vector<Eigen::Vector2d>& lm_list,
    const Eigen::Matrix2d& R, int iters,
    Eigen::Vector3d& x_post, Eigen::Matrix3d& P_post
) {
    if (z_list.empty()) {
        x_post = x_prior;
        P_post = P_prior;
        return;
    }

    int m = static_cast<int>(z_list.size());
    Eigen::VectorXd z(2*m);
    for (int i = 0; i < m; ++i) {
        z.segment<2>(2*i) = z_list[i];
    }

    Eigen::MatrixXd Rbig = Eigen::MatrixXd::Zero(2*m, 2*m);
    for (int i = 0; i < m; ++i) {
        Rbig.block<2,2>(2*i, 2*i) = R;
    }
    Eigen::MatrixXd Rinv = Rbig.inverse();
    Eigen::Matrix3d Pinv = P_prior.inverse();

    Eigen::Vector3d x = x_prior;

    for (int it = 0; it < iters; ++it) {
        Eigen::VectorXd h(2*m);
        Eigen::MatrixXd H(2*m, 3);

        for (int i = 0; i < m; ++i) {
            Eigen::Vector2d hi = measModel(x, lm_list[i]);
            h.segment<2>(2*i) = hi;
            H.block<2,3>(2*i, 0) = measJacobian(x, lm_list[i]);
        }

        Eigen::VectorXd res = z - h;
        for (int i = 0; i < m; ++i) {
            res(2*i + 1) = wrapAngle(res(2*i + 1));
        }

        Eigen::Matrix3d A = Pinv + H.transpose() * Rinv * H;
        Eigen::Vector3d b = H.transpose() * Rinv * res + Pinv * (x_prior - x);

        Eigen::Vector3d delta = A.ldlt().solve(b);
        x = x + delta;
        x(2) = wrapAngle(x(2));
    }

    // Final covariance
    Eigen::MatrixXd H(2*m, 3);
    for (int i = 0; i < m; ++i) {
        H.block<2,3>(2*i, 0) = measJacobian(x, lm_list[i]);
    }
    Eigen::Matrix3d Pinv_post = Pinv + H.transpose() * Rinv * H;
    P_post = Pinv_post.inverse();
    x_post = x;
}

int main() {
    std::mt19937 rng(7);
    std::normal_distribution<double> n01(0.0, 1.0);

    // Landmarks (known map)
    std::vector<Eigen::Vector2d> landmarks;
    landmarks.emplace_back(5.0, 0.0);
    landmarks.emplace_back(6.0, 6.0);
    landmarks.emplace_back(0.0, 6.0);

    const double dt = 0.1;
    const int N = 350;

    Eigen::Vector3d x_true(0.0, 0.0, 0.0);
    Eigen::Vector3d x_hat(0.0, 0.0, 0.0);

    Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
    P(0,0) = 0.15*0.15;
    P(1,1) = 0.15*0.15;
    P(2,2) = std::pow(8.0*M_PI/180.0, 2);

    const double sigma_v = 0.08;
    const double sigma_w = 3.0*M_PI/180.0;
    const double sigma_r = 0.12;
    const double sigma_b = 2.0*M_PI/180.0;

    Eigen::Vector2d goal(7.0, 7.0);
    const double k_heading = 1.6;
    const double v_max = 0.9;

    Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
    R(0,0) = sigma_r*sigma_r;
    R(1,1) = sigma_b*sigma_b;

    std::ofstream csv("traj_ch1_l4.csv");
    csv << "k,x_true,y_true,th_true,x_hat,y_hat,th_hat\n";

    for (int k = 0; k < N; ++k) {
        // Navigation uses estimate
        double dx = goal(0) - x_hat(0);
        double dy = goal(1) - x_hat(1);
        double dist = std::hypot(dx, dy);
        double desired = std::atan2(dy, dx);
        double heading_err = wrapAngle(desired - x_hat(2));

        double v_cmd = v_max * std::tanh(dist);
        double w_cmd = k_heading * heading_err;

        // True motion includes mild slip
        double slip = (0.35*M_PI/180.0) * n01(rng);
        x_true = unicycleStep(x_true, v_cmd, w_cmd + slip/dt, dt);

        // Odometry
        double v_meas = v_cmd + sigma_v * n01(rng);
        double w_meas = w_cmd + sigma_w * n01(rng);

        Eigen::Vector3d x_prior = unicycleStep(x_hat, v_meas, w_meas, dt);

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

        Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
        Q(0,0) = std::pow(sigma_v*dt, 2);
        Q(1,1) = std::pow(sigma_v*dt, 2);
        Q(2,2) = std::pow(sigma_w*dt, 2);

        Eigen::Matrix3d P_prior = F * P * F.transpose() + Q;

        // Landmark measurements
        std::vector<Eigen::Vector2d> z_list;
        std::vector<Eigen::Vector2d> lm_list;
        const double max_range = 8.0;

        for (const auto& lm : landmarks) {
            Eigen::Vector2d z_true = measModel(x_true, lm);
            if (z_true(0) <= max_range) {
                Eigen::Vector2d z_noisy = z_true;
                z_noisy(0) += sigma_r * n01(rng);
                z_noisy(1)  = wrapAngle(z_noisy(1) + sigma_b * n01(rng));
                z_list.push_back(z_noisy);
                lm_list.push_back(lm);
            }
        }

        // WLS correction
        Eigen::Vector3d x_post;
        Eigen::Matrix3d P_post;
        wlsUpdateIterated(x_prior, P_prior, z_list, lm_list, R, 2, x_post, P_post);

        x_hat = x_post;
        P = P_post;

        csv << k << "," << x_true(0) << "," << x_true(1) << "," << x_true(2) << ","
            << x_hat(0)  << "," << x_hat(1)  << "," << x_hat(2)  << "\n";
    }

    std::cout << "Wrote traj_ch1_l4.csv\n";
    return 0;
}
      

Robotics libraries to know (C++): ROS 2 (rclcpp), Eigen, and optimization/SLAM libraries such as Ceres Solver and GTSAM. Eigen is used here for compact linear algebra.

Java is less common on embedded robot controllers but appears in enterprise AMR systems and simulation tooling. This version is self-contained and avoids external dependencies by coding small matrix routines directly.

Chapter1_Lesson4.java

// Chapter1_Lesson4.java
// Sensing–Estimation–Navigation pipeline demo (2D unicycle) using prior + WLS correction.
// Build:
//   javac Chapter1_Lesson4.java
// Run:
//   java Chapter1_Lesson4
//
// Output: "traj_ch1_l4_java.csv" with true and estimated pose.
// Robotics ecosystem pointers: ROSJava (rosjava), EJML for matrices, JVX? (not required here).
// This demo uses small hand-coded linear algebra to stay self-contained.

import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;

public class Chapter1_Lesson4 {

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

    static double[] unicycleStep(double[] x, double v, double w, double dt) {
        double[] xn = x.clone();
        double th = x[2];
        xn[0] = x[0] + v * dt * Math.cos(th);
        xn[1] = x[1] + v * dt * Math.sin(th);
        xn[2] = wrapAngle(x[2] + w * dt);
        return xn;
    }

    static double[] measModel(double[] x, double[] lm) {
        double dx = lm[0] - x[0];
        double dy = lm[1] - x[1];
        double r = Math.hypot(dx, dy);
        double b = wrapAngle(Math.atan2(dy, dx) - x[2]);
        return new double[]{r, b};
    }

    static double[][] measJacobian(double[] x, double[] lm) {
        double dx = lm[0] - x[0];
        double dy = lm[1] - x[1];
        double q = dx*dx + dy*dy;
        q = Math.max(q, 1e-12);
        double r = Math.sqrt(q);
        r = Math.max(r, 1e-12);

        double[][] H = new double[2][3];
        // range
        H[0][0] = -dx / r;
        H[0][1] = -dy / r;
        H[0][2] = 0.0;
        // bearing
        H[1][0] =  dy / q;
        H[1][1] = -dx / q;
        H[1][2] = -1.0;
        return H;
    }

    // Small 3x3 linear algebra helpers
    static double[][] matAdd3(double[][] A, double[][] B) {
        double[][] C = new double[3][3];
        for (int i=0;i<3;i++) for (int j=0;j<3;j++) C[i][j] = A[i][j] + B[i][j];
        return C;
    }
    static double[][] matMul3(double[][] A, double[][] B) {
        double[][] C = new double[3][3];
        for (int i=0;i<3;i++) for (int j=0;j<3;j++) {
            double s=0.0;
            for (int k=0;k<3;k++) s += A[i][k]*B[k][j];
            C[i][j]=s;
        }
        return C;
    }
    static double[] matVec3(double[][] A, double[] x) {
        double[] y = new double[3];
        for (int i=0;i<3;i++) {
            y[i]=A[i][0]*x[0] + A[i][1]*x[1] + A[i][2]*x[2];
        }
        return y;
    }
    static double[][] transpose3(double[][] A) {
        double[][] T = new double[3][3];
        for (int i=0;i<3;i++) for (int j=0;j<3;j++) T[i][j] = A[j][i];
        return T;
    }

    // Inverse of 3x3 matrix via adjugate (sufficient for this small demo)
    static double[][] inv3(double[][] A) {
        double a=A[0][0], b=A[0][1], c=A[0][2];
        double d=A[1][0], e=A[1][1], f=A[1][2];
        double g=A[2][0], h=A[2][1], i=A[2][2];
        double det = a*(e*i - f*h) - b*(d*i - f*g) + c*(d*h - e*g);
        if (Math.abs(det) < 1e-12) det = (det >= 0 ? 1e-12 : -1e-12);

        double[][] inv = new double[3][3];
        inv[0][0] =  (e*i - f*h)/det;
        inv[0][1] = -(b*i - c*h)/det;
        inv[0][2] =  (b*f - c*e)/det;
        inv[1][0] = -(d*i - f*g)/det;
        inv[1][1] =  (a*i - c*g)/det;
        inv[1][2] = -(a*f - c*d)/det;
        inv[2][0] =  (d*h - e*g)/det;
        inv[2][1] = -(a*h - b*g)/det;
        inv[2][2] =  (a*e - b*d)/det;
        return inv;
    }

    // Solve 3x3 system A x = b (Gaussian elimination)
    static double[] solve3(double[][] A, double[] b) {
        double[][] M = new double[3][4];
        for (int r=0;r<3;r++) {
            System.arraycopy(A[r], 0, M[r], 0, 3);
            M[r][3] = b[r];
        }
        for (int col=0; col<3; col++) {
            // pivot
            int piv = col;
            for (int r=col+1;r<3;r++) if (Math.abs(M[r][col]) > Math.abs(M[piv][col])) piv = r;
            double[] tmp = M[col]; M[col] = M[piv]; M[piv] = tmp;

            double pivot = M[col][col];
            if (Math.abs(pivot) < 1e-12) pivot = (pivot>=0 ? 1e-12 : -1e-12);
            for (int c=col;c<4;c++) M[col][c] /= pivot;

            for (int r=0;r<3;r++) if (r != col) {
                double factor = M[r][col];
                for (int c=col;c<4;c++) M[r][c] -= factor * M[col][c];
            }
        }
        return new double[]{M[0][3], M[1][3], M[2][3]};
    }

    static void wlsUpdateIterated(
            double[] xPrior, double[][] PPrior,
            double[][] zList, double[][] lmList, double[][] R, int iters,
            double[] xPost, double[][] PPost) {

        int m = zList.length;
        if (m == 0) {
            System.arraycopy(xPrior, 0, xPost, 0, 3);
            for (int r=0;r<3;r++) System.arraycopy(PPrior[r], 0, PPost[r], 0, 3);
            return;
        }

        // Build R^{-1} for each measurement (2x2)
        double detR = R[0][0]*R[1][1] - R[0][1]*R[1][0];
        double[][] Rinv = new double[][] {
                { R[1][1]/detR, -R[0][1]/detR},
                {-R[1][0]/detR,  R[0][0]/detR}
        };

        double[][] Pinv = inv3(PPrior);
        double[] x = xPrior.clone();

        for (int it=0; it<iters; it++) {
            // Accumulate normal equation pieces:
            // A = Pinv + sum H^T Rinv H,  b = sum H^T Rinv res + Pinv(xPrior - x)
            double[][] A = new double[3][3];
            for (int r=0;r<3;r++) System.arraycopy(Pinv[r], 0, A[r], 0, 3);

            double[] b = matVec3(Pinv, new double[]{xPrior[0]-x[0], xPrior[1]-x[1], wrapAngle(xPrior[2]-x[2])});

            for (int j=0; j<m; j++) {
                double[] z = zList[j];
                double[] lm = lmList[j];

                double[] h = measModel(x, lm);
                double[] res = new double[]{z[0]-h[0], wrapAngle(z[1]-h[1])};

                double[][] H = measJacobian(x, lm); // 2x3

                // Compute H^T Rinv H (3x3) and H^T Rinv res (3)
                double[][] RtH = new double[2][3];
                for (int r=0;r<2;r++) for (int c=0;c<3;c++) {
                    RtH[r][c] = Rinv[r][0]*H[0][c] + Rinv[r][1]*H[1][c];
                }
                // add to A: H^T * (Rinv*H)
                for (int r=0;r<3;r++) for (int c=0;c<3;c++) {
                    A[r][c] += H[0][r]*RtH[0][c] + H[1][r]*RtH[1][c];
                }

                // add to b: H^T * (Rinv*res)
                double r0 = Rinv[0][0]*res[0] + Rinv[0][1]*res[1];
                double r1 = Rinv[1][0]*res[0] + Rinv[1][1]*res[1];
                b[0] += H[0][0]*r0 + H[1][0]*r1;
                b[1] += H[0][1]*r0 + H[1][1]*r1;
                b[2] += H[0][2]*r0 + H[1][2]*r1;
            }

            double[] delta = solve3(A, b);
            x[0] += delta[0];
            x[1] += delta[1];
            x[2] = wrapAngle(x[2] + delta[2]);
        }

        // Approximate posterior covariance by recomputing A at final x
        double[][] A = new double[3][3];
        for (int r=0;r<3;r++) System.arraycopy(Pinv[r], 0, A[r], 0, 3);
        for (int j=0; j<m; j++) {
            double[][] H = measJacobian(x, lmList[j]);
            // A += H^T Rinv H
            double[][] RtH = new double[2][3];
            for (int r=0;r<2;r++) for (int c=0;c<3;c++) {
                RtH[r][c] = Rinv[r][0]*H[0][c] + Rinv[r][1]*H[1][c];
            }
            for (int r=0;r<3;r++) for (int c=0;c<3;c++) {
                A[r][c] += H[0][r]*RtH[0][c] + H[1][r]*RtH[1][c];
            }
        }
        double[][] P = inv3(A);

        System.arraycopy(x, 0, xPost, 0, 3);
        for (int r=0;r<3;r++) System.arraycopy(P[r], 0, PPost[r], 0, 3);
    }

    public static void main(String[] args) throws IOException {
        Random rng = new Random(7);

        double[][] landmarks = new double[][] {
                {5.0, 0.0},
                {6.0, 6.0},
                {0.0, 6.0}
        };

        double dt = 0.1;
        int N = 350;

        double[] xTrue = new double[]{0.0, 0.0, 0.0};
        double[] xHat  = new double[]{0.0, 0.0, 0.0};

        double[][] P = new double[][] {
                {0.15*0.15, 0.0, 0.0},
                {0.0, 0.15*0.15, 0.0},
                {0.0, 0.0, Math.pow(Math.toRadians(8.0), 2)}
        };

        double sigmaV = 0.08;
        double sigmaW = Math.toRadians(3.0);
        double sigmaR = 0.12;
        double sigmaB = Math.toRadians(2.0);

        double[] goal = new double[]{7.0, 7.0};
        double kHeading = 1.6;
        double vMax = 0.9;

        double[][] R = new double[][] {
                {sigmaR*sigmaR, 0.0},
                {0.0, sigmaB*sigmaB}
        };

        FileWriter csv = new FileWriter("traj_ch1_l4_java.csv");
        csv.write("k,x_true,y_true,th_true,x_hat,y_hat,th_hat\n");

        for (int k=0; k<N; k++) {
            // Navigation uses estimate
            double dx = goal[0] - xHat[0];
            double dy = goal[1] - xHat[1];
            double dist = Math.hypot(dx, dy);
            double desired = Math.atan2(dy, dx);
            double headingErr = wrapAngle(desired - xHat[2]);

            double vCmd = vMax * Math.tanh(dist);
            double wCmd = kHeading * headingErr;

            // True motion includes mild slip
            double slip = Math.toRadians(0.35) * rng.nextGaussian();
            xTrue = unicycleStep(xTrue, vCmd, wCmd + slip/dt, dt);

            // Odometry
            double vMeas = vCmd + sigmaV * rng.nextGaussian();
            double wMeas = wCmd + sigmaW * rng.nextGaussian();

            double[] xPrior = unicycleStep(xHat, vMeas, wMeas, dt);

            double th = xHat[2];
            double[][] F = new double[][] {
                    {1.0, 0.0, -vMeas*dt*Math.sin(th)},
                    {0.0, 1.0,  vMeas*dt*Math.cos(th)},
                    {0.0, 0.0,  1.0}
            };

            double[][] Q = new double[][] {
                    {Math.pow(sigmaV*dt,2), 0.0, 0.0},
                    {0.0, Math.pow(sigmaV*dt,2), 0.0},
                    {0.0, 0.0, Math.pow(sigmaW*dt,2)}
            };

            double[][] FP = matMul3(F, P);
            double[][] FPFT = matMul3(FP, transpose3(F));
            double[][] PPrior = matAdd3(FPFT, Q);

            // Measurements
            double maxRange = 8.0;
            int count = 0;
            double[][] zListTemp = new double[landmarks.length][2];
            double[][] lmListTemp = new double[landmarks.length][2];
            for (int j=0; j<landmarks.length; j++) {
                double[] zTrue = measModel(xTrue, landmarks[j]);
                if (zTrue[0] <= maxRange) {
                    double[] zNoisy = new double[] {
                            zTrue[0] + sigmaR * rng.nextGaussian(),
                            wrapAngle(zTrue[1] + sigmaB * rng.nextGaussian())
                    };
                    zListTemp[count] = zNoisy;
                    lmListTemp[count] = landmarks[j];
                    count++;
                }
            }
            double[][] zList = new double[count][2];
            double[][] lmList = new double[count][2];
            for (int j=0; j<count; j++) {
                zList[j] = zListTemp[j];
                lmList[j] = lmListTemp[j];
            }

            double[] xPost = new double[3];
            double[][] PPost = new double[3][3];
            wlsUpdateIterated(xPrior, PPrior, zList, lmList, R, 2, xPost, PPost);

            xHat = xPost;
            P = PPost;

            csv.write(String.format("%d,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n",
                    k, xTrue[0], xTrue[1], xTrue[2], xHat[0], xHat[1], xHat[2]));
        }

        csv.close();
        System.out.println("Wrote traj_ch1_l4_java.csv");
    }
}
      

Robotics libraries to know (Java): EJML for matrices, ROSJava for ROS ecosystems, and simulation frameworks that integrate Java components. In later chapters, using EJML would simplify the estimator code.

MATLAB is widely used for control design and rapid prototyping. The first script mirrors the Python simulation. The second script programmatically builds a Simulink model of the pipeline (useful for block-diagram workflows).

Chapter1_Lesson4.m

% Chapter1_Lesson4.m
% Sensing–Estimation–Navigation pipeline demo (2D unicycle) using WLS (prior + range-bearing).
% Requires: base MATLAB. Optional (not required): Robotics System Toolbox / Simulink for later chapters.
% Run:
%   Chapter1_Lesson4

clear; clc; close all;
rng(7);

wrapAngle = @(a) mod(a + pi, 2*pi) - pi;

unicycleStep = @(x,v,w,dt) [ ...
    x(1) + v*dt*cos(x(3)); ...
    x(2) + v*dt*sin(x(3)); ...
    wrapAngle(x(3) + w*dt) ];

measModel = @(x,lm) [ ...
    hypot(lm(1)-x(1), lm(2)-x(2)); ...
    wrapAngle(atan2(lm(2)-x(2), lm(1)-x(1)) - x(3)) ];

measJacobian = @(x,lm) localMeasJacobian(x,lm);

% Map (known landmarks)
landmarks = [5 0; 6 6; 0 6];

dt = 0.1; N = 350;

xTrue = [0;0;0];
xHat  = [0;0;0];

P = diag([0.15^2, 0.15^2, deg2rad(8)^2]);

sigmaV = 0.08;
sigmaW = deg2rad(3);
sigmaR = 0.12;
sigmaB = deg2rad(2);

goal = [7;7];
kHeading = 1.6;
vMax = 0.9;

R = diag([sigmaR^2, sigmaB^2]);

histTrue = zeros(N,3);
histHat  = zeros(N,3);

for k=1:N
    % --- Navigation (policy uses estimate) ---
    dx = goal(1) - xHat(1);
    dy = goal(2) - xHat(2);
    dist = hypot(dx,dy);
    desired = atan2(dy,dx);
    headingErr = wrapAngle(desired - xHat(3));
    vCmd = vMax*tanh(dist);
    wCmd = kHeading*headingErr;

    % --- True motion with mild slip disturbance ---
    slip = deg2rad(0.35)*randn;
    xTrue = unicycleStep(xTrue, vCmd, wCmd + slip/dt, dt);

    % --- Odometry ---
    vMeas = vCmd + sigmaV*randn;
    wMeas = wCmd + sigmaW*randn;

    % --- Prediction ---
    xPrior = unicycleStep(xHat, vMeas, wMeas, dt);

    th = xHat(3);
    F = [1 0 -vMeas*dt*sin(th);
         0 1  vMeas*dt*cos(th);
         0 0  1];

    Q = diag([(sigmaV*dt)^2, (sigmaV*dt)^2, (sigmaW*dt)^2]);
    PPrior = F*P*F' + Q;

    % --- Landmark measurements ---
    zList = [];
    lmList = [];
    maxRange = 8.0;
    for j=1:size(landmarks,1)
        zTrue = measModel(xTrue, landmarks(j,:)');
        if zTrue(1) <= maxRange
            zNoisy = zTrue + [sigmaR*randn; sigmaB*randn];
            zNoisy(2) = wrapAngle(zNoisy(2));
            zList = [zList; zNoisy];
            lmList = [lmList; landmarks(j,:)];
        end
    end

    % --- Iterated WLS correction (2 iterations) ---
    [xHat, P] = wlsUpdateIterated(xPrior, PPrior, zList, lmList, R, 2, wrapAngle, measModel, measJacobian);

    histTrue(k,:) = xTrue';
    histHat(k,:)  = xHat';
end

figure;
plot(histTrue(:,1), histTrue(:,2), 'LineWidth', 1.5); hold on;
plot(histHat(:,1), histHat(:,2),  'LineWidth', 1.5);
scatter(landmarks(:,1), landmarks(:,2), 70, 'x', 'LineWidth', 2);
scatter(goal(1), goal(2), 90, '*', 'LineWidth', 2);
axis equal; grid on;
legend('true','estimated','landmarks','goal');
title('Sensing–Estimation–Navigation loop (demo)');

% ===== Local functions =====

function H = localMeasJacobian(x,lm)
    dx = lm(1) - x(1);
    dy = lm(2) - x(2);
    q = dx^2 + dy^2;
    q = max(q, 1e-12);
    r = sqrt(q);
    r = max(r, 1e-12);

    H = zeros(2,3);
    H(1,1) = -dx/r;
    H(1,2) = -dy/r;
    H(1,3) = 0;

    H(2,1) =  dy/q;
    H(2,2) = -dx/q;
    H(2,3) = -1;
end

function [xPost, PPost] = wlsUpdateIterated(xPrior, PPrior, zList, lmList, R, iters, wrapAngle, measModel, measJacobian)
    if isempty(zList)
        xPost = xPrior;
        PPost = PPrior;
        return;
    end

    m = size(zList,1);
    z = reshape(zList', [], 1); % (2m x 1)

    Rbig = kron(eye(m), R);
    Rinv = inv(Rbig);
    Pinv = inv(PPrior);

    x = xPrior;
    for it = 1:iters
        h = zeros(2*m,1);
        H = zeros(2*m,3);
        for j=1:m
            lm = lmList(j,:)';
            hj = measModel(x,lm);
            h(2*j-1:2*j) = hj;
            H(2*j-1:2*j,:) = measJacobian(x,lm);
        end

        res = z - h;
        for j=1:m
            res(2*j) = wrapAngle(res(2*j));
        end

        A = Pinv + H'*Rinv*H;
        b = H'*Rinv*res + Pinv*(xPrior - x);

        delta = A \ b;
        x = x + delta;
        x(3) = wrapAngle(x(3));
    end

    % Posterior covariance at final linearization point
    H = zeros(2*m,3);
    for j=1:m
        lm = lmList(j,:)';
        H(2*j-1:2*j,:) = measJacobian(x,lm);
    end
    PPost = inv(Pinv + H'*Rinv*H);
    xPost = x;
end
      

Chapter1_Lesson4_Simulink.m

% Chapter1_Lesson4_Simulink.m
% Programmatically builds a Simulink model that mirrors the sensing–estimation–navigation pipeline.
%
% Requires: Simulink. Optional: Robotics System Toolbox (not required).
%
% What you get:
%   Model name: ch1_l4_pipeline.slx
%   Blocks:
%     - Controller (MATLAB Function): go-to-goal based on x_hat
%     - Plant (MATLAB Function): unicycle step producing x_true
%     - Odometry noise and integration (MATLAB Function): produces x_prior
%     - Landmark sensor (MATLAB Function): range-bearing z
%     - Estimator (MATLAB Function): one Gauss-Newton/WLS step -> x_hat
%
% Run:
%   Chapter1_Lesson4_Simulink
%   open_system('ch1_l4_pipeline')

clear; clc;

model = 'ch1_l4_pipeline';
if bdIsLoaded(model)
    close_system(model, 0);
end
new_system(model);
open_system(model);

set_param(model, 'StopTime', '35');      % 35 s
set_param(model, 'FixedStep', '0.1');    % dt = 0.1
set_param(model, 'Solver', 'FixedStepDiscrete');

% Layout helpers
x0 = 50; y0 = 60; dx = 220; dy = 110;
blkW = 150; blkH = 60;

% --- Source: goal (constant) ---
add_block('simulink/Sources/Constant', [model '/Goal'], ...
    'Position', [x0 y0 x0+blkW y0+blkH], ...
    'Value', '[7;7]');

% --- Initial x_hat (Unit Delay) ---
add_block('simulink/Discrete/Unit Delay', [model '/x_hat(z^-1)'], ...
    'Position', [x0 y0+dy x0+blkW y0+dy+blkH], ...
    'InitialCondition', '[0;0;0]');

% --- Controller MATLAB Function ---
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Controller'], ...
    'Position', [x0+dx y0+dy x0+dx+blkW y0+dy+blkH]);

set_param([model '/Controller'], 'Script', sprintf([ ...
'function u = f(goal, xhat)\n' ...
'%% go-to-goal controller using estimate\n' ...
'%% xhat = [x;y;th], u = [v;w]\n' ...
'kHeading = 1.6; vMax = 0.9;\n' ...
'dx = goal(1) - xhat(1);\n' ...
'dy = goal(2) - xhat(2);\n' ...
'dist = hypot(dx,dy);\n' ...
'desired = atan2(dy,dx);\n' ...
'headingErr = wrapToPi(desired - xhat(3));\n' ...
'v = vMax * tanh(dist);\n' ...
'w = kHeading * headingErr;\n' ...
'u = [v;w];\n' ...
'end\n' ...
])));

% --- Plant MATLAB Function (true dynamics) ---
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Plant'], ...
    'Position', [x0+2*dx y0+dy x0+2*dx+blkW y0+dy+blkH]);

set_param([model '/Plant'], 'Script', sprintf([ ...
'function xtrue = f(xtrue_prev, u)\n' ...
'%% unicycle discrete dynamics, dt = 0.1 assumed\n' ...
'dt = 0.1;\n' ...
'v = u(1); w = u(2);\n' ...
'th = xtrue_prev(3);\n' ...
'x = xtrue_prev(1) + v*dt*cos(th);\n' ...
'y = xtrue_prev(2) + v*dt*sin(th);\n' ...
'th = wrapToPi(xtrue_prev(3) + w*dt);\n' ...
'xtrue = [x;y;th];\n' ...
'end\n' ...
])));

% --- True state memory (Unit Delay) ---
add_block('simulink/Discrete/Unit Delay', [model '/x_true(z^-1)'], ...
    'Position', [x0+2*dx y0 x0+2*dx+blkW y0+blkH], ...
    'InitialCondition', '[0;0;0]');

% --- Odometry + Prior (MATLAB Function) ---
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/OdometryPrior'], ...
    'Position', [x0+2*dx y0+2*dy x0+2*dx+blkW y0+2*dy+blkH]);

set_param([model '/OdometryPrior'], 'Script', sprintf([ ...
'function xprior = f(xhat_prev, u)\n' ...
'%% noisy odometry integration, dt=0.1 assumed\n' ...
'dt = 0.1;\n' ...
'sigmaV = 0.08; sigmaW = deg2rad(3);\n' ...
'v = u(1) + sigmaV*randn;\n' ...
'w = u(2) + sigmaW*randn;\n' ...
'th = xhat_prev(3);\n' ...
'x = xhat_prev(1) + v*dt*cos(th);\n' ...
'y = xhat_prev(2) + v*dt*sin(th);\n' ...
'th = wrapToPi(xhat_prev(3) + w*dt);\n' ...
'xprior = [x;y;th];\n' ...
'end\n' ...
])));

% --- Sensor (range-bearing to one landmark for simplicity) ---
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/LandmarkSensor'], ...
    'Position', [x0+3*dx y0 x0+3*dx+blkW y0+blkH]);

set_param([model '/LandmarkSensor'], 'Script', sprintf([ ...
'function z = f(xtrue)\n' ...
'%% range-bearing to a single known landmark, with noise\n' ...
'lm = [6;6];\n' ...
'sigmaR = 0.12; sigmaB = deg2rad(2);\n' ...
'dx = lm(1) - xtrue(1);\n' ...
'dy = lm(2) - xtrue(2);\n' ...
'r = hypot(dx,dy);\n' ...
'b = wrapToPi(atan2(dy,dx) - xtrue(3));\n' ...
'z = [r + sigmaR*randn; wrapToPi(b + sigmaB*randn)];\n' ...
'end\n' ...
])));

% --- Estimator (one WLS/GN step) ---
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Estimator'], ...
    'Position', [x0+3*dx y0+dy x0+3*dx+blkW y0+dy+blkH]);

set_param([model '/Estimator'], 'Script', sprintf([ ...
'function xhat = f(xprior, z)\n' ...
'%% one Gauss-Newton step for pose from a single landmark range-bearing\n' ...
'lm = [6;6];\n' ...
'sigmaR = 0.12; sigmaB = deg2rad(2);\n' ...
'R = diag([sigmaR^2, sigmaB^2]);\n' ...
'\n' ...
'x = xprior; %% linearize at prior\n' ...
'dx = lm(1) - x(1);\n' ...
'dy = lm(2) - x(2);\n' ...
'q = max(dx^2 + dy^2, 1e-12);\n' ...
'rhat = sqrt(q);\n' ...
'bhat = wrapToPi(atan2(dy,dx) - x(3));\n' ...
'res = [z(1)-rhat; wrapToPi(z(2)-bhat)];\n' ...
'\n' ...
'H = [-dx/rhat, -dy/rhat, 0;\n' ...
'      dy/q,    -dx/q,  -1];\n' ...
'\n' ...
'%% simple diagonal prior weight (tunable)\n' ...
'P = diag([0.15^2, 0.15^2, deg2rad(8)^2]);\n' ...
'A = inv(P) + H''/R*H;\n' ...
'b = H''/R*res;\n' ...
'delta = A\\b;\n' ...
'xhat = x + delta;\n' ...
'xhat(3) = wrapToPi(xhat(3));\n' ...
'end\n' ...
])));

% --- Scope blocks ---
add_block('simulink/Sinks/Scope', [model '/Scope'], ...
    'Position', [x0+4*dx y0+dy x0+4*dx+blkW y0+dy+blkH]);
set_param([model '/Scope'], 'NumInputPorts', '2');

% --- Wiring ---
add_line(model, 'Goal/1', 'Controller/1');
add_line(model, 'x_hat(z^-1)/1', 'Controller/2');

add_line(model, 'Controller/1', 'Plant/2');
add_line(model, 'x_true(z^-1)/1', 'Plant/1');

add_line(model, 'Plant/1', 'x_true(z^-1)/1'); % feedback state

add_line(model, 'Controller/1', 'OdometryPrior/2');
add_line(model, 'x_hat(z^-1)/1', 'OdometryPrior/1');

add_line(model, 'x_true(z^-1)/1', 'LandmarkSensor/1');

add_line(model, 'OdometryPrior/1', 'Estimator/1');
add_line(model, 'LandmarkSensor/1', 'Estimator/2');

add_line(model, 'Estimator/1', 'x_hat(z^-1)/1'); % update estimate

add_line(model, 'x_true(z^-1)/1', 'Scope/1');
add_line(model, 'x_hat(z^-1)/1', 'Scope/2');

save_system(model);
disp('Built and saved ch1_l4_pipeline.slx');
      

Robotics libraries to know (MATLAB/Simulink): Robotics System Toolbox (transforms, localization utilities), Navigation Toolbox (maps/costmaps), and Simulink for real-time simulation and code generation.

Mathematica is useful for symbolic derivations (Jacobians, linearization checks) and quick numerical experiments. The following notebook (stored as a plain-text .nb) reproduces the WLS correction approach.

Chapter1_Lesson4.nb

(* Chapter1_Lesson4.nb
   Wolfram Mathematica notebook (plain-text representation).
   It simulates a 2D unicycle with noisy odometry and range-bearing landmark measurements,
   then performs a Gauss-Newton/WLS correction step each time.
*)

Notebook[{
  Cell["Chapter 1 — Lesson 4: Sensing–Estimation–Navigation Pipeline (Demo)", "Title"],
  Cell["This notebook is a minimal, self-contained simulation of a sensing–estimation–navigation loop.", "Text"],

  Cell[BoxData[
    ToBoxes[
      ClearAll["Global`*"];
      wrapAngle[a_] := Module[{x = Mod[a + Pi, 2 Pi]}, If[x < 0, x + 2 Pi, x] - Pi];

      unicycleStep[x_, v_, w_, dt_] := Module[{th = x[[3]]},
        {x[[1]] + v dt Cos[th], x[[2]] + v dt Sin[th], wrapAngle[x[[3]] + w dt]}
      ];

      measModel[x_, lm_] := Module[{dx = lm[[1]] - x[[1]], dy = lm[[2]] - x[[2]], r, b},
        r = Sqrt[dx^2 + dy^2];
        b = wrapAngle[ArcTan[dx, dy] - x[[3]]];
        {r, b}
      ];

      measJacobian[x_, lm_] := Module[{dx = lm[[1]] - x[[1]], dy = lm[[2]] - x[[2]], q, r},
        q = Max[dx^2 + dy^2, 10^-12];
        r = Sqrt[q];
        {
          {-dx/r, -dy/r, 0},
          {dy/q, -dx/q, -1}
        }
      ];
    ]
  ], "Input"],

  Cell["Simulation parameters", "Section"],
  Cell[BoxData[
    ToBoxes[
      SeedRandom[7];
      landmarks = { {5., 0.}, {6., 6.}, {0., 6.} };
      dt = 0.1; N = 350;

      xTrue = {0., 0., 0.};
      xHat  = {0., 0., 0.};

      P = DiagonalMatrix[{0.15^2, 0.15^2, (8 Degree)^2}];

      sigmaV = 0.08;
      sigmaW = 3 Degree;
      sigmaR = 0.12;
      sigmaB = 2 Degree;

      goal = {7., 7.};
      kHeading = 1.6;
      vMax = 0.9;

      R = DiagonalMatrix[{sigmaR^2, sigmaB^2}];
    ]
  ], "Input"],

  Cell["Iterated WLS update (prior + stacked measurements)", "Section"],
  Cell[BoxData[
    ToBoxes[
      wlsUpdateIterated[xPrior_, PPrior_, zList_, lmList_, Rmeas_, iters_: 2] :=
        Module[{m, z, Rbig, Rinv, Pinv, x, h, H, res, A, b, delta},
          m = Length[zList];
          If[m == 0, Return[{xPrior, PPrior}]];

          z = Flatten[zList];
          Rbig = KroneckerProduct[IdentityMatrix[m], Rmeas];
          Rinv = Inverse[Rbig];
          Pinv = Inverse[PPrior];

          x = xPrior;
          Do[
            h = Flatten[measModel[x, #] & /@ lmList];
            H = ArrayFlatten[Partition[measJacobian[x, lmList[[#]]], 1] & /@ Range[m]];

            res = z - h;
            (* wrap each bearing residual *)
            Do[res[[2 i]] = wrapAngle[res[[2 i]]], {i, 1, m}];

            A = Pinv + Transpose[H].Rinv.H;
            b = Transpose[H].Rinv.res + Pinv.(xPrior - x);

            delta = LinearSolve[A, b];
            x = x + delta;
            x[[3]] = wrapAngle[x[[3]]];
          , {iters}];

          (* posterior covariance at final linearization point *)
          H = ArrayFlatten[Partition[measJacobian[x, lmList[[#]]], 1] & /@ Range[m]];
          {x, Inverse[Pinv + Transpose[H].Rinv.H]}
        ];
    ]
  ], "Input"],

  Cell["Main loop", "Section"],
  Cell[BoxData[
    ToBoxes[
      histTrue = ConstantArray[0., {N, 3}];
      histHat  = ConstantArray[0., {N, 3}];

      Do[
        (* navigation uses estimate *)
        dx = goal[[1]] - xHat[[1]];
        dy = goal[[2]] - xHat[[2]];
        dist = Sqrt[dx^2 + dy^2];
        desired = ArcTan[dx, dy];
        headingErr = wrapAngle[desired - xHat[[3]]];

        vCmd = vMax Tanh[dist];
        wCmd = kHeading headingErr;

        (* true motion with mild slip *)
        slip = (0.35 Degree) RandomVariate[NormalDistribution[0, 1]];
        xTrue = unicycleStep[xTrue, vCmd, wCmd + slip/dt, dt];

        (* odometry *)
        vMeas = vCmd + sigmaV RandomVariate[NormalDistribution[0, 1]];
        wMeas = wCmd + sigmaW RandomVariate[NormalDistribution[0, 1]];

        xPrior = unicycleStep[xHat, vMeas, wMeas, dt];

        th = xHat[[3]];
        F = { {1, 0, -vMeas dt Sin[th]},
             {0, 1,  vMeas dt Cos[th]},
             {0, 0,  1} };

        Q = DiagonalMatrix[{(sigmaV dt)^2, (sigmaV dt)^2, (sigmaW dt)^2}];
        PPrior = F.P.Transpose[F] + Q;

        (* measurements: use landmarks within range *)
        maxRange = 8.;
        zList = {};
        lmList = {};
        Do[
          zTrue = measModel[xTrue, lm];
          If[zTrue[[1]] <= maxRange,
            zNoisy = {zTrue[[1]] + sigmaR RandomVariate[NormalDistribution[0, 1]],
                      wrapAngle[zTrue[[2]] + sigmaB RandomVariate[NormalDistribution[0, 1]]]};
            zList = Append[zList, zNoisy];
            lmList = Append[lmList, lm];
          ];
        , {lm, landmarks}];

        {xHat, P} = wlsUpdateIterated[xPrior, PPrior, zList, lmList, R, 2];

        histTrue[[k]] = xTrue;
        histHat[[k]] = xHat;
      , {k, 1, N}];

      ListLinePlot[{histTrue[[All, {1, 2}]], histHat[[All, {1, 2}]]},
        PlotLegends -> {"true", "estimated"},
        AspectRatio -> 1,
        GridLines -> Automatic,
        PlotRange -> All,
        AxesLabel -> {"x (m)", "y (m)"},
        PlotLabel -> "Sensing–Estimation–Navigation loop (demo)"
      ]
    ]
  ], "Input"]
}]
      

Problem 1 (ML \( \Rightarrow \) WLS): Let \( \mathbf{y} = \mathbf{h}(\mathbf{x}) + \mathbf{v} \) with \( \mathbf{v} \sim \mathcal{N}(\mathbf{0},\mathbf{R}) \). Derive the maximum-likelihood estimator for \( \mathbf{x} \).

Solution: The likelihood is Gaussian with mean \( \mathbf{h}(\mathbf{x}) \). Maximizing \( p(\mathbf{y}\mid\mathbf{x}) \) is equivalent to minimizing the negative log-likelihood, which equals (up to constants):

\[ J(\mathbf{x}) = \left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right)^\top \mathbf{R}^{-1} \left(\mathbf{y}-\mathbf{h}(\mathbf{x})\right). \]

Therefore \( \hat{\mathbf{x}}_{\mathrm{ML}} = \arg\min_{\mathbf{x}} J(\mathbf{x}) \).

Problem 2 (Range–bearing Jacobian): A landmark at \( \boldsymbol{\ell}=(\ell_x,\ell_y)^\top \) is observed from pose \( \mathbf{x}=(x,y,\theta)^\top \) by: \( r = \sqrt{(\ell_x-x)^2+(\ell_y-y)^2} \) and \( b = \mathrm{atan2}(\ell_y-y,\ell_x-x)-\theta \). Compute \( \mathbf{H}=\tfrac{\partial (r,b)}{\partial (x,y,\theta)} \).

Solution: Let \( d_x=\ell_x-x \), \( d_y=\ell_y-y \), \( q=d_x^2+d_y^2 \), \( r=\sqrt{q} \). Then:

\[ \mathbf{H} = \begin{bmatrix} -\tfrac{d_x}{r} & -\tfrac{d_y}{r} & 0 \\ \tfrac{d_y}{q} & -\tfrac{d_x}{q} & -1 \end{bmatrix}. \]

Problem 3 (Unobservability with range-only): Suppose measurements are range-only to any set of landmarks, i.e., \( y_i = \sqrt{(\ell_{ix}-x)^2+(\ell_{iy}-y)^2} + v_i \), with no bearing term. Prove that \( \theta \) is unobservable from these measurements.

Solution: The measurement function depends only on \( (x,y) \). Formally, for any \( \theta \) and any increment \( \Delta\theta \), we have \( \mathbf{h}(x,y,\theta) = \mathbf{h}(x,y,\theta+\Delta\theta) \). Therefore distinct states that differ only in \( \theta \) produce identical measurement predictions, so no estimator can recover \( \theta \) uniquely from range-only data. \( \square \)

Problem 4 (One-step Gauss–Newton normal equations): Consider the prior+measurement objective in Section 3 and linearize \( \mathbf{h}(\mathbf{x}) \) at \( \mathbf{x}_0 \). Derive the normal equations for the minimizing increment \( \delta \).

Solution: Using \( \mathbf{h}(\mathbf{x}_0+\delta) \approx \mathbf{h}(\mathbf{x}_0)+\mathbf{H}\delta \), the objective becomes a quadratic in \( \delta \). Setting the gradient to zero yields:

\[ \left(\mathbf{P}^{-1} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{H}\right)\,\delta = \mathbf{H}^\top\mathbf{R}^{-1}\left(\mathbf{y}-\mathbf{h}(\mathbf{x}_0)\right) + \mathbf{P}^{-1}\left(\mathbf{x}^{-}-\mathbf{x}_0\right). \]

which matches the update used in the lab codes.

Problem 5 (Practical stability with bounded estimation error): In Section 4, assume \( \|\tilde{\mathbf{p}}_k\| \le \varepsilon \) and \( 0 \lt k\Delta t \lt 1 \). Prove that \( \|\mathbf{e}_k\| \) converges to an ultimate bound of size at most \( \varepsilon \).

Solution: The recursion is \( \mathbf{e}_{k+1} = a\mathbf{e}_k + (1-a)\tilde{\mathbf{p}}_k \) with \( a=1-k\Delta t \) and \( |a| \lt 1 \). Unrolling and bounding the geometric series gives: \( \|\mathbf{e}_k\| \le |a|^k\|\mathbf{e}_0\| + (1-a)\varepsilon\sum_{j=0}^{k-1}|a|^j \). Taking \( k\to\infty \) yields \( \limsup\|\mathbf{e}_k\| \le \varepsilon \). \( \square \)

We built a compact mathematical model of the sensing–estimation–navigation pipeline and showed how Gaussian measurement assumptions lead naturally to weighted least squares (with a prior term for dead-reckoning). We also proved a practical stability bound demonstrating that navigation accuracy cannot exceed estimation accuracy. The labs implemented the entire loop in Python, C++, Java, MATLAB/Simulink, and Mathematica, establishing a baseline simulation artifact that we will progressively refine in later chapters.