From Lesson 4, we can view autonomy as a pipeline of transformations: sensing → estimation → planning → control → physical motion. Failure modes are best understood as structured violations of modeling assumptions at specific interfaces.

We use the planar pose state from Lesson 2, \( \mathbf{x} = [x,\,y,\,\theta]^\top \in \mathbb{R}^2 \times \mathbb{S}^1 \), and a body-frame velocity command \( \mathbf{u} = [v,\,\omega]^\top \). The nominal discrete-time kinematic propagation (unicycle form) is

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k,\Delta t) = \begin{bmatrix} x_k + \Delta t\, v_k \cos\theta_k \\ y_k + \Delta t\, v_k \sin\theta_k \\ \theta_k + \Delta t\,\omega_k \end{bmatrix} \]

The three failure modes can be expressed as perturbations of either the input (drift, slip) or the measurement stream (occlusion). A convenient abstraction is:

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k + \boldsymbol{\delta u}_k,\Delta t),\qquad \mathbf{z}_k = \begin{cases} h(\mathbf{x}_k) + \mathbf{v}_k & \text{if measurement visible/valid} \\ \varnothing & \text{if occluded} \\ \tilde h(\mathbf{x}_k) & \text{if outlier (mismatch/ghost)} \end{cases} \]

flowchart TD
  S["Sensors (wheel, IMU, \nlidar/camera)"] --> P["Perception \n(features/obstacles)"]
  P --> E["Estimation \n(pose x_hat)"]
  E --> PL["Planning \n(path/trajectory)"]
  PL --> C["Control \n(v_cmd, w_cmd)"]
  C --> R["Robot + ground contact"]

  D1["Drift: bias/scale/noise \nin wheel/IMU"] --> E
  SL["Slip: rolling \nassumption violated"] --> R
  OC["Occlusion: missing/corrupted \nexteroceptive data"] --> P
        

In Chapter 1 terms, drift, slip, and occlusion are not “edge cases”; they are the dominant reasons the simplifying assumptions (perfect rolling contact, perfect sensing availability, perfectly repeatable actuators) break down in practice. We now formalize each mode with error dynamics.

Drift is persistent deviation between the estimated pose \( \hat{\mathbf{x}}_k \) and the true pose \( \mathbf{x}_k \) caused by integrating imperfect motion information. In AMRs, the motion information most commonly comes from wheel encoders (odometry) and gyroscopes (yaw rate).

2.1 Input error model

Let the controller request \( \mathbf{u}_k \), but the estimator receives an odometric input \( \tilde{\mathbf{u}}_k \):

\[ \tilde{\mathbf{u}}_k = \mathbf{S}\mathbf{u}_k + \mathbf{b} + \boldsymbol{\eta}_k, \qquad \mathbf{S} = \begin{bmatrix} s_v & 0 \\ 0 & s_\omega \end{bmatrix}, \qquad \mathbf{b} = \begin{bmatrix} b_v \\ b_\omega \end{bmatrix} \]

where \( \mathbf{S} \) captures scale errors (e.g., wrong wheel radius), \( \mathbf{b} \) captures biases (e.g., gyro bias), and \( \boldsymbol{\eta}_k \) captures zero-mean noise. Define the pose error \( \mathbf{e}_k = \hat{\mathbf{x}}_k - \mathbf{x}_k \).

2.2 Drift from constant bias (linear-in-time growth)

To isolate the mechanism, consider 1D motion along a line with true dynamics \( x_{k+1} = x_k + \Delta t\, v_k \) and estimated dynamics using biased odometry \( \hat{x}_{k+1} = \hat{x}_k + \Delta t\, (v_k + b_v) \).

Proposition 1 (bias-induced drift): If \( b_v \) is constant, then

\[ e_{k} = \hat{x}_k - x_k = e_0 + k\Delta t\, b_v \]

Proof: Subtract the two recursions:

\[ e_{k+1} = (\hat{x}_{k+1} - x_{k+1}) = (\hat{x}_k - x_k) + \Delta t\, b_v = e_k + \Delta t\, b_v. \]

Unrolling the recursion yields \( e_k = e_0 + k\Delta t\,b_v \). This is the defining signature of deterministic drift: error grows linearly with time.

2.3 Drift from zero-mean noise (square-root-in-time RMS growth)

Now assume \( b_v=0 \) and \( \eta_k \) are i.i.d. with \( \mathbb{E}[\eta_k]=0 \) and variance \( \operatorname{Var}(\eta_k)=\sigma_v^2 \). Then

\[ e_{k+1} = e_k + \Delta t\,\eta_k \]

Proposition 2 (random-walk RMS): For \( e_0=0 \),

\[ \mathbb{E}[e_k]=0, \qquad \operatorname{Var}(e_k)=k\Delta t^2\sigma_v^2, \qquad \sqrt{\mathbb{E}[e_k^2]} = \Delta t\,\sigma_v\sqrt{k}. \]

Proof: Since \( e_k = \Delta t \sum_{i=0}^{k-1}\eta_i \), linearity gives \( \mathbb{E}[e_k]=\Delta t\sum \mathbb{E}[\eta_i]=0 \). Independence gives \( \operatorname{Var}(e_k)=\Delta t^2\sum \operatorname{Var}(\eta_i)=k\Delta t^2\sigma_v^2 \).

2.4 Why yaw drift is especially damaging

In planar motion, orientation error couples into lateral position error. Consider forward speed \( v \) and small heading error \( e_\theta \). The true vs estimated lateral velocity differs by approximately:

\[ \hat{\dot y} - \dot y = v\sin(\theta + e_\theta) - v\sin\theta \approx v\cos\theta\, e_\theta \quad (|e_\theta| \text{ small}). \]

If \( e_\theta(t) \) drifts linearly due to a constant gyro bias \( b_\omega \), i.e., \( e_\theta(t)=b_\omega t \), then integrating once more yields lateral error that grows approximately quadratically:

\[ |e_y(t)| \approx \left|\int_0^t v\cos\theta(\tau)\, b_\omega \tau\, d\tau\right| \le |v b_\omega|\int_0^t \tau\, d\tau = \tfrac{1}{2}|v b_\omega| t^2. \]

This is a practical reason AMR stacks prioritize heading quality: small yaw biases can dominate position error.

Slip means the wheel-ground contact does not enforce the ideal rolling constraint. In Chapter 2 we will derive rolling constraints in detail; here we only need the key implication: wheel encoder counts are no longer a reliable proxy for ground-relative displacement.

3.1 A compact slip parameterization

For a driven wheel with radius \( r \), wheel angular speed \( \omega_w \), and ground-relative longitudinal speed \( v \), a common longitudinal slip ratio is \( s \), defined (one standard convention) by:

\[ s \;\triangleq\; \frac{r\omega_w - v}{\max(|r\omega_w|,\,|v|,\,\epsilon)}, \qquad 0 \le s < 1, \qquad \epsilon > 0. \]

When \( s=0 \), the wheel is rolling without longitudinal slip (ideal for odometry). When \( s > 0 \), the wheel is “spinning” relative to the ground, and the true speed is attenuated relative to what the encoder implies: \( v = (1-s)\, r\omega_w \).

3.2 Differential-drive example: how slip distorts the kinematic map

For a differential drive with wheelbase \( L \), ideal (no-slip) mapping from wheel speeds \( \omega_R,\omega_L \) to body velocities is:

\[ v = \frac{r}{2}\left(\omega_R + \omega_L\right), \qquad \omega = \frac{r}{L}\left(\omega_R - \omega_L\right). \]

Introduce (possibly time-varying) slip ratios \( s_R, s_L \). One minimal model is to treat slip as an effective radius contraction: \( r_R^{\mathrm{eff}}=(1-s_R)r \), \( r_L^{\mathrm{eff}}=(1-s_L)r \). Then the true body velocities become:

\[ v^{\mathrm{true}} = \frac{1}{2}\Big((1-s_R)r\,\omega_R + (1-s_L)r\,\omega_L\Big), \qquad \omega^{\mathrm{true}} = \frac{1}{L}\Big((1-s_R)r\,\omega_R - (1-s_L)r\,\omega_L\Big). \]

Meanwhile, the odometry computation (which assumes \( s_R=s_L=0 \)) produces \( (v^{\mathrm{odo}},\omega^{\mathrm{odo}}) \) using the ideal map, yielding systematic errors that can be sudden (slip bursts) or slowly varying (terrain-dependent).

3.3 A fundamental identifiability limitation (why encoders alone cannot “see” slip)

The core difficulty is that both “wrong wheel radius” and “wheel slip” enter the kinematics as a multiplicative factor in front of encoder-derived wheel motion.

Proposition 3 (encoder-only ambiguity): Consider any encoder sequence \( \{\omega_{R,k},\omega_{L,k}\} \) and any slip sequence \( \{s_{R,k}, s_{L,k}\} \). Define effective radii \( r_{R,k}^{\mathrm{eff}}=(1-s_{R,k})r \), \( r_{L,k}^{\mathrm{eff}}=(1-s_{L,k})r \). Then the same body velocities can be produced by a no-slip model with time-varying radii \( r_R(t), r_L(t) \).

Proof (constructive): Choose \( r_R(t_k) = r_{R,k}^{\mathrm{eff}} \) and Choose \( r_L(t_k) = r_{L,k}^{\mathrm{eff}} \). Substituting into the no-slip map recovers exactly the slip-distorted \( v^{\mathrm{true}},\omega^{\mathrm{true}} \). Therefore, from encoders alone, slip cannot be uniquely separated from multiplicative wheel parameters.

Practical implication: detecting slip requires redundancy (Lesson 4’s pipeline concept): compare wheel-derived motion to an independent modality (IMU yaw rate, vision/LiDAR motion cues, contact sensing), or use physics-informed dynamics (Chapter 4).

Occlusion refers to the loss or corruption of exteroceptive observations because the environment blocks line-of-sight (physical occlusion), because the scene lacks usable structure (textureless walls, reflective glass), or because dynamic objects temporarily dominate the field of view. At the estimation interface, occlusion appears as missing measurements or outliers.

4.1 A minimal measurement-availability model

Let \( o_k \in \{0,1\} \) be an availability indicator: \( o_k=1 \) means the measurement is usable; \( o_k=0 \) means occluded. For a generic measurement model (e.g., range-bearing to a known landmark),

\[ \mathbf{z}_k = \begin{cases} h(\mathbf{x}_k) + \mathbf{v}_k & \text{if } o_k = 1 \\ \varnothing & \text{if } o_k = 0 \end{cases} \]

A common practical complication is outliers due to mis-association or multipath/ghosting. One simple abstraction is a mixture:

\[ p(\mathbf{z}_k\mid \mathbf{x}_k) = (1-\pi)\,\mathcal{N}\big(h(\mathbf{x}_k),\,\mathbf{R}\big) + \pi\,p_\text{out}(\mathbf{z}_k), \qquad 0 \le \pi < 1, \]

where \( p_\text{out} \) is a broad “outlier” distribution. The key modeling point for this lesson is: the estimator must survive long intervals with no usable measurements.

4.2 Residual gating (a deterministic, pre-probabilistic view)

Even before formal Bayes/Kalman filtering (Chapters 6–7), one can treat measurement acceptance as a residual test. Linearize the measurement model about the current estimate: \( h(\mathbf{x}) \approx h(\hat{\mathbf{x}}) + \mathbf{H}(\mathbf{x}-\hat{\mathbf{x}}) \), where \( \mathbf{H} \) is the Jacobian.

Define the innovation (residual) \( \mathbf{r}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_k) \). A widely used acceptance statistic is the (squared) Mahalanobis distance:

\[ d_k^2 = \mathbf{r}_k^\top \mathbf{S}_k^{-1}\mathbf{r}_k, \qquad \mathbf{S}_k = \mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^\top + \mathbf{R}. \]

Here \( \mathbf{P}_k \) is a tuning proxy for current state uncertainty (or simply a scaling matrix), and \( \mathbf{R} \) represents nominal measurement noise. Then a deterministic gate is: \( d_k^2 < \gamma \Rightarrow \text{accept} \), \( d_k^2 \ge \gamma \Rightarrow \text{reject (possible occlusion/outlier)} \).

4.3 Bounded inter-update time and bounded drift (a useful guarantee)

Suppose the robot receives a usable exteroceptive correction at least once every \( T_\max \) seconds (i.e., the time between non-occluded measurements is bounded), and suppose the odometry velocity bias is bounded: \( \|\mathbf{b}\| \le b_\max \). Then the drift between corrections can be bounded.

Proposition 4 (inter-update drift bound): In a straight-line 1D model, the worst-case position drift between two corrections separated by time \( \Delta t_c \le T_\max \) satisfies:

\[ |e| \le |e^+| + b_\max\,\Delta t_c \le |e^+| + b_\max\,T_\max, \]

where \( e^+ \) is the error immediately after the last correction. Proof: Between corrections, Proposition 1 gives linear growth \( e(t)=e^+ + b_v t \); apply the bound \( |b_v|\le b_\max \).

This proposition is a formal way to express an operational insight: occlusion is dangerous when it creates long gaps in external correction opportunities.

The practical goal is to map symptoms in logs to one of the three modes (or a combination). Below is a structured diagnosis flow based on quantities already introduced: pose/velocity estimates (Lesson 2) and the pipeline structure (Lesson 4).

flowchart TD
  A["Unexpected navigation \nerror observed"] --> B["Is error growing steadily \neven on uniform floor?"]
  B -->|yes| D["Suspect drift: bias/scale \nin wheel or yaw-rate"]
  B -->|no| C["Is error correlated with turns, \naccel, or terrain patches?"]
  C -->|yes| S["Suspect slip: rolling \nassumption violated"]
  C -->|no| OQ["Do exteroceptive signals \nshow dropouts? \n(feature count, lidar returns)"]
  OQ -->|yes| O["Suspect occlusion/outliers: \nmissing or corrupted measurements"]
  OQ -->|no| M["Check model mismatch: \nwrong parameters, timing, \nframe transforms"]

  D --> D2["Mitigate: calibration,\n bias estimation, periodic \nexternal correction"]
  S --> S2["Mitigate: reduce accel, \nadd slip detectors, \nfuse redundant sensors"]
  O --> O2["Mitigate: sensor placement, \nredundancy, gating, \nrecovery behaviors"]
        

5.1 Practical signatures

• Drift: error grows approximately monotone with time/distance; repeatable across runs; strong dependence on heading bias; insensitive to short-lived dynamic obstacles.
• Slip: bursts during high acceleration/deceleration, wet/dusty patches, ramps; wheel-derived yaw rate conflicts with IMU yaw rate; odometry shows motion while camera/LiDAR indicates near-zero.
• Occlusion: perception confidence drops (few features, low return counts); estimator “coasts” on dead-reckoning; sudden jumps can occur when outliers are accepted.

5.2 Mitigation within Chapter 1 scope

At this stage (before formal probabilistic filtering), mitigation focuses on:

• Parameter hygiene: wheel radius and wheelbase calibration; consistent time stamps; consistent frames.
• Redundancy: at least two independent motion cues (e.g., wheel + yaw gyro) and at least one exteroceptive cue.
• Validation logic: residual tests and simple consistency checks before accepting updates.
• Operational envelopes: limit acceleration/turn rate when traction is uncertain; choose sensor placements that reduce self-occlusion.

This lab implements the models from Sections 2–4 and visualizes error growth. It uses numpy and matplotlib. In a ROS 2 stack, the analogous data sources are typically: nav_msgs/Odometry (wheel odometry), sensor_msgs/Imu (yaw rate), and an exteroceptive stream (camera/LiDAR) producing feature/landmark measurements.

File: Chapter1_Lesson5.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Chapter1_Lesson5.py

Autonomous Mobile Robots (Control Engineering) — Chapter 1, Lesson 5
Typical AMR Failure Modes: drift, slip, occlusion

This script simulates:
  1) Drift from velocity and yaw-rate bias + noise
  2) Slip bursts that scale down true translational/rotational velocity
  3) Occlusion that randomly drops measurement updates

Outputs:
  - A plot of true vs estimated trajectory
  - Error vs time
  - A simple residual-gating demonstration

Dependencies: numpy, matplotlib
"""

from __future__ import annotations

import math
import numpy as np
import matplotlib.pyplot as plt


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


def unicycle_step(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """
    Discrete-time unicycle integration.
    x = [x, y, theta], u = [v, omega]
    """
    v, w = float(u[0]), float(u[1])
    th = float(x[2])

    x_next = x.copy()
    x_next[0] = x[0] + dt * v * math.cos(th)
    x_next[1] = x[1] + dt * v * math.sin(th)
    x_next[2] = wrap_to_pi(x[2] + dt * w)
    return x_next


def simulate_failure_modes(
    T: float = 30.0,
    dt: float = 0.05,
    v_cmd: float = 0.6,
    w_cmd: float = 0.15,
    # Drift parameters
    b_v: float = 0.02,
    b_w: float = 0.01,
    sigma_v: float = 0.03,
    sigma_w: float = 0.02,
    # Slip parameters
    slip_prob: float = 0.06,
    slip_scale_v: float = 0.65,
    slip_scale_w: float = 0.70,
    # Occlusion parameters
    meas_period: float = 0.5,
    occlusion_prob: float = 0.35,
    meas_sigma_xy: float = 0.10,
    meas_sigma_th: float = 0.06,
    # Residual gating
    gate_gamma: float = 9.21,  # ~chi-square 0.99 quantile for 2 dof (illustrative)
    rng_seed: int = 2,
) -> dict:
    """
    Returns a dict containing trajectories, errors, and diagnostic signals.
    We use a very simple estimator:
      - propagate with biased/noisy odometry
      - occasionally apply a noisy "measurement" of pose, unless occluded
      - reject measurement if residual is too large (gating)
    """

    rng = np.random.default_rng(rng_seed)
    N = int(T / dt)
    t = np.arange(N + 1) * dt

    # True and estimated states: [x, y, theta]
    x_true = np.zeros((N + 1, 3), dtype=float)
    x_hat = np.zeros((N + 1, 3), dtype=float)

    # For demonstration, start at same pose
    x_true[0, :] = np.array([0.0, 0.0, 0.0], dtype=float)
    x_hat[0, :] = np.array([0.0, 0.0, 0.0], dtype=float)

    # Diagnostics
    slip_flag = np.zeros(N, dtype=int)
    occluded_flag = np.zeros(N + 1, dtype=int)
    gate_reject_flag = np.zeros(N + 1, dtype=int)

    # Simple (tunable) covariance proxy for gating in x-y only
    # (This is *not* a Kalman filter; formal filtering is in Ch.6–7.)
    P_xy = (0.20**2) * np.eye(2)
    R_xy = (meas_sigma_xy**2) * np.eye(2)

    # Measurement schedule
    meas_steps = max(1, int(meas_period / dt))

    for k in range(N):
        # Command
        u_cmd = np.array([v_cmd, w_cmd], dtype=float)

        # --- True motion with slip bursts ---
        if rng.random() < slip_prob:
            slip_flag[k] = 1
            u_true = np.array([slip_scale_v * v_cmd, slip_scale_w * w_cmd], dtype=float)
        else:
            u_true = u_cmd

        x_true[k + 1, :] = unicycle_step(x_true[k, :], u_true, dt)

        # --- Odometry (drift): biased + noisy ---
        v_odo = v_cmd + b_v + rng.normal(0.0, sigma_v)
        w_odo = w_cmd + b_w + rng.normal(0.0, sigma_w)
        u_odo = np.array([v_odo, w_odo], dtype=float)

        x_hat[k + 1, :] = unicycle_step(x_hat[k, :], u_odo, dt)

        # --- Occasional measurement update (unless occluded) ---
        do_meas = ((k + 1) % meas_steps) == 0
        if do_meas:
            if rng.random() < occlusion_prob:
                occluded_flag[k + 1] = 1
                # No correction (estimator coasts)
                continue

            # Synthetic measurement of full pose (x, y, theta)
            z = x_true[k + 1, :].copy()
            z[0] += rng.normal(0.0, meas_sigma_xy)
            z[1] += rng.normal(0.0, meas_sigma_xy)
            z[2] = wrap_to_pi(z[2] + rng.normal(0.0, meas_sigma_th))

            # Residual gating on x-y
            r_xy = z[0:2] - x_hat[k + 1, 0:2]
            S = P_xy + R_xy
            d2 = float(r_xy.T @ np.linalg.inv(S) @ r_xy)

            if d2 >= gate_gamma:
                gate_reject_flag[k + 1] = 1
                continue  # reject as outlier
            else:
                # Simple correction (blend). This is a heuristic "gain".
                K = P_xy @ np.linalg.inv(S)
                x_hat[k + 1, 0:2] = x_hat[k + 1, 0:2] + K @ r_xy
                # Blend heading too (no gating here; illustrative)
                alpha_th = 0.35
                th_err = wrap_to_pi(z[2] - x_hat[k + 1, 2])
                x_hat[k + 1, 2] = wrap_to_pi(x_hat[k + 1, 2] + alpha_th * th_err)

                # Very simple covariance proxy update
                P_xy = (np.eye(2) - K) @ P_xy

    # Errors
    e = x_hat - x_true
    e[:, 2] = np.array([wrap_to_pi(a) for a in e[:, 2]])

    out = {
        "t": t,
        "x_true": x_true,
        "x_hat": x_hat,
        "error": e,
        "slip_flag": slip_flag,
        "occluded_flag": occluded_flag,
        "gate_reject_flag": gate_reject_flag,
    }
    return out


def plot_results(sim: dict) -> None:
    t = sim["t"]
    x_true = sim["x_true"]
    x_hat = sim["x_hat"]
    e = sim["error"]
    slip_flag = sim["slip_flag"]
    occluded = sim["occluded_flag"]
    rejected = sim["gate_reject_flag"]

    # Trajectory
    plt.figure()
    plt.plot(x_true[:, 0], x_true[:, 1], label="true")
    plt.plot(x_hat[:, 0], x_hat[:, 1], label="estimated")
    plt.axis("equal")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.title("Trajectory: drift + slip + occlusion")
    plt.legend()
    plt.grid(True)

    # Error vs time
    plt.figure()
    plt.plot(t, e[:, 0], label="e_x")
    plt.plot(t, e[:, 1], label="e_y")
    plt.plot(t, e[:, 2], label="e_theta")
    plt.xlabel("time [s]")
    plt.ylabel("error")
    plt.title("Estimation error vs time")
    plt.legend()
    plt.grid(True)

    # Diagnostic markers (simple visualization)
    plt.figure()
    plt.step(t[:-1], slip_flag, where="post", label="slip_burst")
    plt.step(t, occluded, where="post", label="occluded_meas")
    plt.step(t, rejected, where="post", label="gated_reject")
    plt.xlabel("time [s]")
    plt.ylabel("flag")
    plt.title("Failure/diagnostic flags")
    plt.legend()
    plt.grid(True)

    plt.show()


if __name__ == "__main__":
    sim = simulate_failure_modes()
    plot_results(sim)
      

This program mirrors the Python logic and writes trajectory.csv. For robotics development, common C++ libraries include Eigen (linear algebra) and ROS 2 (rclcpp, tf2). The code below is self-contained and does not require ROS.

File: Chapter1_Lesson5.cpp

/*
Chapter1_Lesson5.cpp

Autonomous Mobile Robots (Control Engineering) — Chapter 1, Lesson 5
Typical AMR Failure Modes: drift, slip, occlusion

This C++ program simulates a planar unicycle robot:
  - True motion experiences occasional slip bursts (scaling v and w)
  - Odometry used for estimation has bias + noise (drift)
  - Occasional external measurements are dropped by occlusion, and gated by residual

Outputs a CSV file "trajectory.csv" with columns:
  t, x_true, y_true, th_true, x_hat, y_hat, th_hat, slip, occluded, rejected

No external dependencies beyond the standard library.

Compile:
  g++ -O2 -std=c++17 -o Chapter1_Lesson5 Chapter1_Lesson5.cpp
Run:
  ./Chapter1_Lesson5
*/

#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

struct State {
    double x{0.0};
    double y{0.0};
    double th{0.0};
};

static double wrap_to_pi(double a) {
    const double pi = 3.14159265358979323846;
    a = std::fmod(a + pi, 2.0 * pi);
    if (a < 0.0) a += 2.0 * pi;
    return a - pi;
}

static State unicycle_step(const State& s, double v, double w, double dt) {
    State sn = s;
    sn.x  = s.x + dt * v * std::cos(s.th);
    sn.y  = s.y + dt * v * std::sin(s.th);
    sn.th = wrap_to_pi(s.th + dt * w);
    return sn;
}

int main() {
    // Simulation parameters
    const double T = 30.0;
    const double dt = 0.05;
    const int N = static_cast<int>(T / dt);

    const double v_cmd = 0.6;
    const double w_cmd = 0.15;

    // Drift (odometry bias + noise)
    const double b_v = 0.02;
    const double b_w = 0.01;
    const double sigma_v = 0.03;
    const double sigma_w = 0.02;

    // Slip
    const double slip_prob = 0.06;
    const double slip_scale_v = 0.65;
    const double slip_scale_w = 0.70;

    // Occlusion and measurement
    const double meas_period = 0.5;
    const int meas_steps = std::max(1, static_cast<int>(meas_period / dt));
    const double occlusion_prob = 0.35;
    const double meas_sigma_xy = 0.10;
    const double meas_sigma_th = 0.06;

    // Residual gate threshold (illustrative)
    const double gate_gamma = 9.21;

    // RNG
    std::mt19937_64 rng(2);
    std::uniform_real_distribution<double> uni01(0.0, 1.0);
    std::normal_distribution<double> n_v(0.0, sigma_v);
    std::normal_distribution<double> n_w(0.0, sigma_w);
    std::normal_distribution<double> n_xy(0.0, meas_sigma_xy);
    std::normal_distribution<double> n_th(0.0, meas_sigma_th);

    // States
    State x_true{}, x_hat{};

    // Simple covariance proxy for gating in x-y
    double Pxx = 0.20 * 0.20;
    double Pyy = 0.20 * 0.20;
    const double Rxx = meas_sigma_xy * meas_sigma_xy;
    const double Ryy = meas_sigma_xy * meas_sigma_xy;

    // Output CSV
    std::ofstream ofs("trajectory.csv");
    ofs << "t,x_true,y_true,th_true,x_hat,y_hat,th_hat,slip,occluded,rejected\n";

    // Log initial
    ofs << 0.0 << ","
        << x_true.x << "," << x_true.y << "," << x_true.th << ","
        << x_hat.x  << "," << x_hat.y  << "," << x_hat.th  << ","
        << 0 << "," << 0 << "," << 0 << "\n";

    for (int k = 0; k < N; ++k) {
        const double t = (k + 1) * dt;

        // True motion with slip bursts
        int slip = 0;
        double v_true = v_cmd;
        double w_true = w_cmd;
        if (uni01(rng) < slip_prob) {
            slip = 1;
            v_true = slip_scale_v * v_cmd;
            w_true = slip_scale_w * w_cmd;
        }
        x_true = unicycle_step(x_true, v_true, w_true, dt);

        // Odometry propagation (biased + noisy)
        const double v_odo = v_cmd + b_v + n_v(rng);
        const double w_odo = w_cmd + b_w + n_w(rng);
        x_hat = unicycle_step(x_hat, v_odo, w_odo, dt);

        int occluded = 0;
        int rejected = 0;

        // Measurement update
        const bool do_meas = ((k + 1) % meas_steps) == 0;
        if (do_meas) {
            if (uni01(rng) < occlusion_prob) {
                occluded = 1;
            } else {
                // Synthetic measurement z ~ x_true + noise
                const double zx = x_true.x + n_xy(rng);
                const double zy = x_true.y + n_xy(rng);
                const double zt = wrap_to_pi(x_true.th + n_th(rng));

                // Residual gating on x-y
                const double rx = zx - x_hat.x;
                const double ry = zy - x_hat.y;

                // S = P + R (diagonal proxy)
                const double Sxx = Pxx + Rxx;
                const double Syy = Pyy + Ryy;
                const double d2 = (rx * rx) / Sxx + (ry * ry) / Syy;

                if (d2 >= gate_gamma) {
                    rejected = 1;
                } else {
                    // K = P (P+R)^{-1} (diagonal proxy)
                    const double Kx = Pxx / Sxx;
                    const double Ky = Pyy / Syy;

                    x_hat.x = x_hat.x + Kx * rx;
                    x_hat.y = x_hat.y + Ky * ry;

                    // Blend heading (heuristic)
                    const double alpha_th = 0.35;
                    const double th_err = wrap_to_pi(zt - x_hat.th);
                    x_hat.th = wrap_to_pi(x_hat.th + alpha_th * th_err);

                    // Covariance proxy update: P <- (I-K)P
                    Pxx = (1.0 - Kx) * Pxx;
                    Pyy = (1.0 - Ky) * Pyy;
                }
            }
        }

        // Write log
        ofs << t << ","
            << x_true.x << "," << x_true.y << "," << x_true.th << ","
            << x_hat.x  << "," << x_hat.y  << "," << x_hat.th  << ","
            << slip << "," << occluded << "," << rejected << "\n";
    }

    ofs.close();
    std::cout << "Wrote trajectory.csv (" << (N + 1) << " rows)." << std::endl;
    return 0;
}
      

Java is less common in modern AMR stacks, but still appears in some enterprise robotics systems. Typical supporting libraries include EJML (linear algebra) and, historically, rosjava bindings. The program below remains dependency-free and writes trajectory_java.csv.

File: Chapter1_Lesson5.java

/*
Chapter1_Lesson5.java

Autonomous Mobile Robots (Control Engineering) — Chapter 1, Lesson 5
Typical AMR Failure Modes: drift, slip, occlusion

This Java program simulates a planar unicycle robot:
  - True motion experiences occasional slip bursts (scaling v and w)
  - Odometry used for estimation has bias + noise (drift)
  - Occasional external measurements are dropped by occlusion, and gated by residual

Outputs a CSV file "trajectory_java.csv" with columns:
  t, x_true, y_true, th_true, x_hat, y_hat, th_hat, slip, occluded, rejected

Compile:
  javac Chapter1_Lesson5.java
Run:
  java Chapter1_Lesson5
*/

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

public class Chapter1_Lesson5 {

    static class State {
        double x, y, th;
        State(double x, double y, double th) { this.x = x; this.y = y; this.th = th; }
    }

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

    static State unicycleStep(State s, double v, double w, double dt) {
        double x = s.x + dt * v * Math.cos(s.th);
        double y = s.y + dt * v * Math.sin(s.th);
        double th = wrapToPi(s.th + dt * w);
        return new State(x, y, th);
    }

    // Gaussian via Random.nextGaussian()
    static double gauss(Random rng, double sigma) {
        return sigma * rng.nextGaussian();
    }

    public static void main(String[] args) throws IOException {
        // Simulation parameters
        double T = 30.0;
        double dt = 0.05;
        int N = (int)Math.floor(T / dt);

        double vCmd = 0.6;
        double wCmd = 0.15;

        // Drift
        double bV = 0.02;
        double bW = 0.01;
        double sigmaV = 0.03;
        double sigmaW = 0.02;

        // Slip
        double slipProb = 0.06;
        double slipScaleV = 0.65;
        double slipScaleW = 0.70;

        // Occlusion and measurement
        double measPeriod = 0.5;
        int measSteps = Math.max(1, (int)Math.floor(measPeriod / dt));
        double occlusionProb = 0.35;
        double measSigmaXY = 0.10;
        double measSigmaTh = 0.06;

        // Gate threshold (illustrative)
        double gateGamma = 9.21;

        Random rng = new Random(2);

        State xTrue = new State(0.0, 0.0, 0.0);
        State xHat  = new State(0.0, 0.0, 0.0);

        // Covariance proxy (diagonal) for gating in x-y
        double Pxx = 0.20 * 0.20;
        double Pyy = 0.20 * 0.20;
        double Rxx = measSigmaXY * measSigmaXY;
        double Ryy = measSigmaXY * measSigmaXY;

        try (PrintWriter out = new PrintWriter(new FileWriter("trajectory_java.csv"))) {
            out.println("t,x_true,y_true,th_true,x_hat,y_hat,th_hat,slip,occluded,rejected");
            out.printf("%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%d,%d,%d%n",
                    0.0, xTrue.x, xTrue.y, xTrue.th, xHat.x, xHat.y, xHat.th, 0, 0, 0);

            for (int k = 0; k < N; k++) {
                double t = (k + 1) * dt;

                // True motion with slip bursts
                int slip = 0;
                double vTrue = vCmd;
                double wTrue = wCmd;
                if (rng.nextDouble() < slipProb) {
                    slip = 1;
                    vTrue = slipScaleV * vCmd;
                    wTrue = slipScaleW * wCmd;
                }
                xTrue = unicycleStep(xTrue, vTrue, wTrue, dt);

                // Odometry propagation (biased + noisy)
                double vOdo = vCmd + bV + gauss(rng, sigmaV);
                double wOdo = wCmd + bW + gauss(rng, sigmaW);
                xHat = unicycleStep(xHat, vOdo, wOdo, dt);

                int occluded = 0;
                int rejected = 0;

                boolean doMeas = ((k + 1) % measSteps) == 0;
                if (doMeas) {
                    if (rng.nextDouble() < occlusionProb) {
                        occluded = 1;
                    } else {
                        // Synthetic measurement
                        double zx = xTrue.x + gauss(rng, measSigmaXY);
                        double zy = xTrue.y + gauss(rng, measSigmaXY);
                        double zt = wrapToPi(xTrue.th + gauss(rng, measSigmaTh));

                        // Gate on x-y
                        double rx = zx - xHat.x;
                        double ry = zy - xHat.y;

                        double Sxx = Pxx + Rxx;
                        double Syy = Pyy + Ryy;
                        double d2 = (rx * rx) / Sxx + (ry * ry) / Syy;

                        if (d2 >= gateGamma) {
                            rejected = 1;
                        } else {
                            double Kx = Pxx / Sxx;
                            double Ky = Pyy / Syy;

                            xHat.x = xHat.x + Kx * rx;
                            xHat.y = xHat.y + Ky * ry;

                            // Heading blend
                            double alphaTh = 0.35;
                            double thErr = wrapToPi(zt - xHat.th);
                            xHat.th = wrapToPi(xHat.th + alphaTh * thErr);

                            // Covariance update proxy
                            Pxx = (1.0 - Kx) * Pxx;
                            Pyy = (1.0 - Ky) * Pyy;
                        }
                    }
                }

                out.printf("%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%d,%d,%d%n",
                        t, xTrue.x, xTrue.y, xTrue.th, xHat.x, xHat.y, xHat.th, slip, occluded, rejected);
            }
        }

        System.out.println("Wrote trajectory_java.csv (" + (N + 1) + " rows).");
    }
}
      

MATLAB is frequently used for rapid prototyping and system identification. In Robotics System Toolbox, odometry and pose integration commonly connect to ROS interfaces; Simulink is often used for real-time control prototyping. The script below runs the same simulation and includes an optional programmatic Simulink “stub” builder to illustrate how a block diagram can be generated.

File: Chapter1_Lesson5.m

% Chapter1_Lesson5.m
%
% Autonomous Mobile Robots (Control Engineering) — Chapter 1, Lesson 5
% Typical AMR Failure Modes: drift, slip, occlusion
%
% This MATLAB script simulates:
%   - True unicycle motion with occasional slip bursts (velocity scaling)
%   - Estimated motion driven by biased/noisy odometry (drift)
%   - External measurements that are randomly occluded and residual-gated
%
% Outputs:
%   - Plots of true vs estimated trajectory and errors
%   - A CSV file "trajectory_matlab.csv" for inspection
%
% Notes:
%   - This is not a Kalman filter; formal filtering is developed in later chapters.
%   - Optional: a small Simulink "stub" model can be created programmatically.
%
% Requires: base MATLAB. (Simulink optional for the stub.)

clear; clc;

%% Parameters
T = 30.0;
dt = 0.05;
N = floor(T/dt);

v_cmd = 0.6;
w_cmd = 0.15;

% Drift (bias + noise)
b_v = 0.02;
b_w = 0.01;
sigma_v = 0.03;
sigma_w = 0.02;

% Slip
slip_prob = 0.06;
slip_scale_v = 0.65;
slip_scale_w = 0.70;

% Occlusion and measurement
meas_period = 0.5;
meas_steps = max(1, floor(meas_period/dt));
occlusion_prob = 0.35;
meas_sigma_xy = 0.10;
meas_sigma_th = 0.06;

% Residual gate (illustrative)
gate_gamma = 9.21;

rng(2);

%% State arrays: [x y theta]
x_true = zeros(N+1,3);
x_hat  = zeros(N+1,3);

slip_flag = zeros(N,1);
occluded_flag = zeros(N+1,1);
rejected_flag = zeros(N+1,1);

% Covariance proxy for gating on x-y (diagonal)
P_xy = (0.20^2)*eye(2);
R_xy = (meas_sigma_xy^2)*eye(2);

%% Helpers
wrapToPi = @(a) mod(a + pi, 2*pi) - pi;

unicycleStep = @(x,u) [ ...
    x(1) + dt*u(1)*cos(x(3)), ...
    x(2) + dt*u(1)*sin(x(3)), ...
    wrapToPi(x(3) + dt*u(2)) ];

%% Main loop
t = (0:N)'*dt;

for k = 1:N
    % True motion with slip bursts
    if rand() < slip_prob
        slip_flag(k) = 1;
        u_true = [slip_scale_v*v_cmd, slip_scale_w*w_cmd];
    else
        u_true = [v_cmd, w_cmd];
    end
    x_true(k+1,:) = unicycleStep(x_true(k,:), u_true);

    % Odometry propagation (biased + noisy)
    v_odo = v_cmd + b_v + sigma_v*randn();
    w_odo = w_cmd + b_w + sigma_w*randn();
    u_odo = [v_odo, w_odo];
    x_hat(k+1,:) = unicycleStep(x_hat(k,:), u_odo);

    % Measurement update occasionally
    do_meas = mod(k, meas_steps) == 0;
    if do_meas
        if rand() < occlusion_prob
            occluded_flag(k+1) = 1;
        else
            % Synthetic measurement
            z = x_true(k+1,:);
            z(1) = z(1) + meas_sigma_xy*randn();
            z(2) = z(2) + meas_sigma_xy*randn();
            z(3) = wrapToPi(z(3) + meas_sigma_th*randn());

            % Gate on x-y
            r_xy = (z(1:2) - x_hat(k+1,1:2))';
            S = P_xy + R_xy;
            d2 = r_xy' * (S\r_xy);

            if d2 >= gate_gamma
                rejected_flag(k+1) = 1;
            else
                K = P_xy / S;
                x_hat(k+1,1:2) = x_hat(k+1,1:2) + (K*r_xy)';

                % Heading blend (heuristic)
                alpha_th = 0.35;
                th_err = wrapToPi(z(3) - x_hat(k+1,3));
                x_hat(k+1,3) = wrapToPi(x_hat(k+1,3) + alpha_th*th_err);

                P_xy = (eye(2) - K) * P_xy;
            end
        end
    end
end

%% Errors
e = x_hat - x_true;
e(:,3) = arrayfun(wrapToPi, e(:,3));

%% Plots
figure;
plot(x_true(:,1), x_true(:,2), 'LineWidth', 1.5); hold on;
plot(x_hat(:,1),  x_hat(:,2),  'LineWidth', 1.5);
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
title('Trajectory: drift + slip + occlusion');
legend('true','estimated');

figure;
plot(t, e(:,1), 'LineWidth', 1.2); hold on;
plot(t, e(:,2), 'LineWidth', 1.2);
plot(t, e(:,3), 'LineWidth', 1.2);
grid on;
xlabel('time [s]'); ylabel('error');
title('Estimation error vs time');
legend('e_x','e_y','e_\theta');

figure;
stairs(t(1:end-1), slip_flag, 'LineWidth', 1.2); hold on;
stairs(t, occluded_flag, 'LineWidth', 1.2);
stairs(t, rejected_flag, 'LineWidth', 1.2);
grid on;
xlabel('time [s]'); ylabel('flag');
title('Failure/diagnostic flags');
legend('slip','occluded','gated reject');

%% Save CSV
csvData = [t, x_true, x_hat, [slip_flag; 0], occluded_flag, rejected_flag];
hdr = { ...
    't','x_true','y_true','th_true','x_hat','y_hat','th_hat','slip','occluded','rejected'};
fid = fopen('trajectory_matlab.csv','w');
fprintf(fid, '%s,', hdr{1,1:end-1});
fprintf(fid, '%s\n', hdr{1,end});
fclose(fid);
dlmwrite('trajectory_matlab.csv', csvData, '-append');

disp('Wrote trajectory_matlab.csv');

%% Optional: Simulink stub (programmatic)
% This creates a minimal empty model with notes. Uncomment if you have Simulink.
%{
modelName = 'Chapter1_Lesson5_SimulinkStub';
new_system(modelName);
open_system(modelName);
add_block('simulink/Commonly Used Blocks/Clock', [modelName '/Clock']);
add_block('simulink/Commonly Used Blocks/Terminator', [modelName '/Terminator']);
add_line(modelName, 'Clock/1', 'Terminator/1');
set_param(modelName, 'StopTime', num2str(T));
save_system(modelName);
disp(['Created Simulink stub: ', modelName]);
%}
      

Mathematica is useful for symbolic derivations (e.g., Jacobians) and rapid experimentation. The following is a plain-text .nb notebook expression that can be opened directly in Mathematica.

File: Chapter1_Lesson5.nb

Notebook[
 {
  Cell[
   TextData[{
     "Chapter1_Lesson5.nb\n\n",
     "Autonomous Mobile Robots (Control Engineering) — Chapter 1, Lesson 5\n",
     "Typical AMR Failure Modes: drift, slip, occlusion\n\n",
     "This notebook simulates a planar unicycle robot with:\n",
     "  - true motion with occasional slip bursts (velocity scaling)\n",
     "  - an estimator driven by biased/noisy odometry (drift)\n",
     "  - external measurements that are randomly occluded and gated\n\n",
     "Outputs: trajectory plots and basic error statistics.\n"
     }],
   "Text"
   ],

  Cell[
   BoxData[
    ToBoxes[
     ClearAll[wrapToPi, unicycleStep, simulate];
     wrapToPi[a_] := Module[{x = Mod[a + Pi, 2 Pi] - Pi}, x];

     unicycleStep[x_, u_, dt_] := Module[{v = u[[1]], w = u[[2]], th = x[[3]]},
       {
        x[[1]] + dt v Cos[th],
        x[[2]] + dt v Sin[th],
        wrapToPi[x[[3]] + dt w]
        }
       ];

     simulate[T_: 30., dt_: 0.05,
       vCmd_: 0.6, wCmd_: 0.15,
       bV_: 0.02, bW_: 0.01,
       sigV_: 0.03, sigW_: 0.02,
       slipProb_: 0.06, slipScaleV_: 0.65, slipScaleW_: 0.70,
       measPeriod_: 0.5, occProb_: 0.35, measSigXY_: 0.10, measSigTh_: 0.06,
       gateGamma_: 9.21, seed_: 2] :=
      Module[
       {n = Floor[T/dt], t, xTrue, xHat, slip, occluded, rejected,
        measSteps, Pxx, Pyy, Rxx, Ryy, k, uTrue, uOdo, z, rx, ry, Sxx, Syy, d2, Kx, Ky,
        alphaTh = 0.35, thErr},
       SeedRandom[seed];
       t = Range[0, n] dt;
       xTrue = ConstantArray[{0., 0., 0.}, n + 1];
       xHat  = ConstantArray[{0., 0., 0.}, n + 1];
       slip = ConstantArray[0, n];
       occluded = ConstantArray[0, n + 1];
       rejected = ConstantArray[0, n + 1];

       measSteps = Max[1, Floor[measPeriod/dt]];

       (* diagonal covariance proxy for x-y gating *)
       Pxx = 0.20^2; Pyy = 0.20^2;
       Rxx = measSigXY^2; Ryy = measSigXY^2;

       For[k = 1, k <= n, k++,
        (* true motion with slip bursts *)
        If[RandomReal[] < slipProb,
         slip[[k]] = 1;
         uTrue = {slipScaleV vCmd, slipScaleW wCmd},
         uTrue = {vCmd, wCmd}
         ];
        xTrue[[k + 1]] = unicycleStep[xTrue[[k]], uTrue, dt];

        (* odometry propagation: bias + noise *)
        uOdo = {vCmd + bV + sigV RandomVariate[NormalDistribution[0, 1]],
                wCmd + bW + sigW RandomVariate[NormalDistribution[0, 1]]};
        xHat[[k + 1]] = unicycleStep[xHat[[k]], uOdo, dt];

        (* occasional measurement update *)
        If[Mod[k, measSteps] == 0,
         If[RandomReal[] < occProb,
          occluded[[k + 1]] = 1,
          z = xTrue[[k + 1]];
          z[[1]] += measSigXY RandomVariate[NormalDistribution[0, 1]];
          z[[2]] += measSigXY RandomVariate[NormalDistribution[0, 1]];
          z[[3]] = wrapToPi[z[[3]] + measSigTh RandomVariate[NormalDistribution[0, 1]]];

          rx = z[[1]] - xHat[[k + 1, 1]];
          ry = z[[2]] - xHat[[k + 1, 2]];
          Sxx = Pxx + Rxx;
          Syy = Pyy + Ryy;
          d2 = rx^2/Sxx + ry^2/Syy;

          If[d2 >= gateGamma,
           rejected[[k + 1]] = 1,
           Kx = Pxx/Sxx; Ky = Pyy/Syy;
           xHat[[k + 1, 1]] += Kx rx;
           xHat[[k + 1, 2]] += Ky ry;

           thErr = wrapToPi[z[[3]] - xHat[[k + 1, 3]]];
           xHat[[k + 1, 3]] = wrapToPi[xHat[[k + 1, 3]] + alphaTh thErr];

           Pxx = (1 - Kx) Pxx; Pyy = (1 - Ky) Pyy;
           ]
          ]
         ]
        ];
       <|"t" -> t, "xTrue" -> xTrue, "xHat" -> xHat,
         "slip" -> slip, "occluded" -> occluded, "rejected" -> rejected|>
       ];

     sim = simulate[];

     (* Trajectory plot *)
     trajPlot = ListLinePlot[
       {sim["xTrue"][[All, {1, 2}]], sim["xHat"][[All, {1, 2}]]},
       PlotLegends -> {"true", "estimated"},
       AspectRatio -> 1,
       GridLines -> Automatic,
       PlotLabel -> "Trajectory: drift + slip + occlusion",
       AxesLabel -> {"x [m]", "y [m]"}
       ];

     (* Error vs time *)
     e = sim["xHat"] - sim["xTrue"];
     e[[All, 3]] = wrapToPi /@ e[[All, 3]];
     errPlot = ListLinePlot[
       {Transpose[{sim["t"], e[[All, 1]]}],
        Transpose[{sim["t"], e[[All, 2]]}],
        Transpose[{sim["t"], e[[All, 3]]}]},
       PlotLegends -> {"e_x", "e_y", "e_theta"},
       GridLines -> Automatic,
       PlotLabel -> "Estimation error vs time",
       AxesLabel -> {"time [s]", "error"}
       ];

     Column[{trajPlot, errPlot}]
     ]
    ],
   "Input"
   ]
  },
 WindowSize -> {960, 720},
 WindowTitle -> "Chapter1_Lesson5"
 ]
      

Problem 1 (Bias drift, discrete-time): A 1D robot moves with true velocity \( v_k = 0.8 \) m/s and sampling time \( \Delta t = 0.1 \) s for \( N=200 \) steps. The estimator integrates odometry with a constant bias \( b_v = 0.02 \) m/s and \( e_0=0 \). Compute \( e_N \).

Solution: By Proposition 1,

\[ e_N = N\Delta t\,b_v = 200 \cdot 0.1 \cdot 0.02 = 0.4\ \text{m}. \]

Problem 2 (Noise drift RMS): In the same setup, let \( b_v=0 \) and \( \eta_k \sim \mathcal{N}(0,\sigma_v^2) \) with \( \sigma_v=0.03 \) m/s. Compute the RMS position error after \( N=200 \) steps.

Solution: Proposition 2 gives

\[ \sqrt{\mathbb{E}[e_N^2]} = \Delta t\,\sigma_v\sqrt{N} = 0.1 \cdot 0.03 \cdot \sqrt{200} \approx 0.0424\ \text{m}. \]

Problem 3 (Slip distance discrepancy): A wheel encoder implies traveled distance \( d_\text{odo} = 12 \) m over a segment. If the longitudinal slip ratio is constant \( s=0.25 \), compute the true traveled distance \( d_\text{true} \) under the simple attenuation model.

Solution: If \( v = (1-s) r\omega_w \), then distance scales the same way:

\[ d_\text{true} = (1-s)\,d_\text{odo} = 0.75 \cdot 12 = 9\ \text{m}. \]

Problem 4 (Occlusion run length): Suppose an exteroceptive update is attempted every \( T=0.5 \) s and each attempt is occluded with probability \( p=0.3 \), independently. Let \( L \) be the number of consecutive occluded attempts before the first non-occluded one. Compute \( \mathbb{E}[L] \) and the expected time without a usable measurement.

Solution: \( L \) is geometric with success probability \( 1-p \):

\[ \mathbb{P}(L=\ell) = p^\ell(1-p), \qquad \mathbb{E}[L] = \frac{p}{1-p} = \frac{0.3}{0.7} \approx 0.4286. \]

The expected occlusion time is \( \mathbb{E}[L]T \approx 0.2143 \) s. (Longer runs occur with probability \( \mathbb{P}(L \ge m)=p^m \).)

Problem 5 (Residual gating decision): A range-bearing residual is \( \mathbf{r}=[0.30,\,0.05]^\top \) (meters, radians). Let \( \mathbf{S}=\operatorname{diag}([0.10^2,\,0.04^2]) \). Compute \( d^2=\mathbf{r}^\top\mathbf{S}^{-1}\mathbf{r} \) and decide whether to accept the measurement with gate \( \gamma=9.21 \).

Solution:

\[ d^2 = \frac{0.30^2}{0.10^2} + \frac{0.05^2}{0.04^2} = 9 + 1.5625 = 10.5625. \]

Since \( d^2 > \gamma \), the measurement is rejected as inconsistent (possible outlier/occlusion artifact).

Drift, slip, and occlusion are the dominant ways real AMRs violate Chapter 1 modeling assumptions. We derived drift growth laws (linear for bias, square-root for noise), showed how slip breaks the encoder-to-motion map and why slip is not identifiable from encoders alone, and modeled occlusion as measurement dropout/outliers with residual gating. These results motivate Chapters 2–5 (more faithful kinematics/dynamics and odometry) and Chapters 6–7 (probabilistic estimation and Kalman-filter localization).