By the end of this lesson, you will be able to:

• Design and implement practical discrete-time filters for odometry streams (velocities, increments, heading).
• Prove stability conditions for common IIR filters and compute frequency responses for FIR/IIR filters used in practice.
• Build a complementary heading fusion filter combining wheel-derived heading (low-frequency reference) and IMU yaw-rate integration (high-frequency dynamics).
• Apply robust statistics (median/MAD) for outlier suppression and implement slip-aware gating rules consistent with mobile robot kinematic limits.

In Chapter 5 (Lessons 1–3), you built raw odometry from wheel encoders and integrated IMU angular rates, and you analyzed drift sources (bias, slip, quantization, wheel radius uncertainty). This lesson focuses on practical filtering that:

• reduces high-frequency noise and quantization jitter,
• rejects occasional spikes/outliers,
• limits integration damage under short slip episodes,
• does not “solve” long-term bias drift (handled rigorously in probabilistic localization later).

flowchart TD
  A["Raw sensors: encoders + IMU yaw-rate"] --> B["Compute v_w, w_w and theta_w (wheel heading)"]
  B --> C["Sanity checks: limits, time-stamps, dt"]
  C --> D["Outlier suppression: median/MAD window"]
  D --> E["Low-pass filtering: v_f, w_f"]
  E --> F["Slip-aware gating: compare gyro vs wheel yaw-rate"]
  F --> G["Heading fusion: complementary filter -> theta_hat"]
  G --> H["Integrate pose: x,y,theta using v_f and theta_hat"]
  H --> I["Publish filtered odometry"]
        

For ground robots in this chapter, we treat planar motion with pose \( \mathbf{p}_k = (x_k, y_k, \theta_k) \). From Lesson 1, a typical pipeline produces wheel-based linear and angular velocities \( v_w[k] \) and \( \omega_w[k] \), and from Lesson 2 we obtain IMU yaw rate \( \omega_g[k] \). We will filter these streams prior to integration:

\[ \begin{aligned} v_w[k] &= v[k] + n_v[k] + o_v[k],\\ \omega_w[k] &= \omega[k] + n_{\omega,w}[k] + o_{\omega,w}[k],\\ \omega_g[k] &= \omega[k] + b_g[k] + n_{\omega,g}[k]. \end{aligned} \]

Here \( n[\cdot] \) denotes “ordinary” noise (quantization + sensor noise), \( o[\cdot] \) denotes sporadic outliers (spikes from slip, missed ticks, timestamp glitches), and \( b_g[k] \) is the gyro bias (slow drift).

A key limitation can be stated precisely: if \( b_g[k] \) has a nonzero mean over time, then integrating \( \omega_g \) yields an orientation drift whose expected magnitude grows with time. Filtering can attenuate high-frequency components but cannot eliminate an unknown constant or slowly varying bias without a reference.

In practice we also create a wheel-based heading reference by integrating the wheel yaw-rate: \( \theta_w[k] \). With sampling time \( T_s \):

\[ \theta_w[k] = \mathrm{wrap}\!\left(\theta_w[k-1] + T_s \, \omega_w[k]\right), \quad \theta_g[k] = \mathrm{wrap}\!\left(\theta_g[k-1] + T_s \, \omega_g[k]\right). \]

Wheel heading is typically less affected by gyro bias but is vulnerable to wheel slip and uneven terrain; gyro integration is smooth and responsive but drifts in the long term. This is the exact scenario complementary filtering was designed for.

A workhorse in odometry filtering is the discrete first-order IIR low-pass filter. We derive it from a continuous-time model to obtain a physically meaningful parameter (cutoff / time constant), then prove stability and compute the frequency response.

Consider the continuous-time low-pass dynamics (analog “RC” form): \( x(t) \) is the noisy measurement (e.g., raw velocity), and \( y(t) \) is the filtered output.

\[ \tau \, \dot{y}(t) + y(t) = x(t), \quad \tau > 0. \]

Using a zero-order hold (ZOH) assumption on \( x(t) \) over each interval \( [kT_s,(k+1)T_s) \), the exact discrete-time update is:

\[ y[k] = \alpha\, y[k-1] + (1-\alpha)\,x[k], \quad \alpha = \exp\!\left(-\frac{T_s}{\tau}\right). \]

It is often convenient to select the cutoff frequency \( f_c \) (Hz) and set \( \tau = \frac{1}{2\pi f_c} \), giving:

\[ \alpha = \exp\!\left(-2\pi f_c T_s\right). \]

3.1 BIBO Stability Proof

The Z-transform transfer function from \( x \) to \( y \) is:

\[ H(z) = \frac{Y(z)}{X(z)} = \frac{1-\alpha}{1-\alpha z^{-1}}. \]

The impulse response is obtained by expanding as a geometric series for \( |z| > |\alpha| \):

\[ h[k] = (1-\alpha)\alpha^k u[k], \]

where \( u[k] \) is the unit step. The filter is BIBO-stable if \( \sum_{k=0}^{\infty} |h[k]| < \infty \). For \( 0 < \alpha < 1 \):

\[ \sum_{k=0}^{\infty} |h[k]| = \sum_{k=0}^{\infty} (1-\alpha)\alpha^k = (1-\alpha)\frac{1}{1-\alpha} = 1. \]

Hence the filter is BIBO-stable for \( |\alpha| < 1 \); in practical low-pass usage we pick \( 0 < \alpha < 1 \).

3.2 Frequency Response and Practical Interpretation

On the unit circle \( z = e^{j\Omega} \) (discrete rad/sample), the frequency response is:

\[ H(e^{j\Omega}) = \frac{1-\alpha}{1-\alpha e^{-j\Omega}}, \quad \left|H(e^{j\Omega})\right| = \frac{1-\alpha}{\sqrt{1+\alpha^2-2\alpha\cos\Omega}}. \]

Large \( \alpha \) (close to 1) yields stronger smoothing but more phase lag; smaller \( \alpha \) tracks changes faster but filters less. In odometry, that trade-off is critical: lag in \( \omega \) causes heading lag, which can distort integrated trajectories during turning.

A moving-average filter is common when encoder quantization causes jitter. For window length \( N \):

\[ y[k] = \frac{1}{N}\sum_{i=0}^{N-1} x[k-i]. \]

Its frequency response is (a standard FIR result):

\[ H(e^{j\Omega}) = \frac{1}{N}e^{-j\Omega\frac{N-1}{2}} \frac{\sin\left(\frac{N\Omega}{2}\right)}{\sin\left(\frac{\Omega}{2}\right)}. \]

The magnitude term yields “notches” at \( \Omega = \frac{2\pi m}{N} \) for integers \( m \neq 0 \), while the linear phase factor \( e^{-j\Omega\frac{N-1}{2}} \) implies constant group delay:

\[ \tau_g = \frac{N-1}{2}\;T_s. \]

This delay is often acceptable for smoothing \( v \) when odometry is used for mapping/localization offline, but it can destabilize fast feedback loops if you close control on filtered velocity. In mobile robots, a common compromise is:

• use mild filtering on \( v \) (small delay),
• use stronger filtering on \( \omega \) only if an IMU-based heading fusion is present (Section 5),
• always gate slip/outliers before applying filters (Section 6).

Complementary filtering fuses two estimates of the same quantity whose error spectra are complementary: wheel-based heading \( \theta_w \) provides a low-frequency reference (bounded drift when wheels do not slip), while gyro integration provides high-frequency responsiveness but suffers low-frequency drift from bias.

5.1 Continuous-Time Construction and “Sum to One” Property

Define a first-order low-pass and high-pass pair with time constant \( \tau \):

\[ H_{LP}(s) = \frac{1}{1+s\tau}, \quad H_{HP}(s) = \frac{s\tau}{1+s\tau}. \]

They satisfy the complementary identity:

\[ H_{LP}(s) + H_{HP}(s) = 1. \]

For heading fusion, interpret gyro output as yaw-rate; integrate then high-pass:

\[ \hat{\theta}(s) = H_{HP}(s)\frac{1}{s}\Omega_g(s) + H_{LP}(s)\Theta_w(s). \]

If \( \Omega_g(s) = s\Theta(s) + B(s) \) with bias term \( B(s) \), then \( \frac{1}{s}\Omega_g(s) = \Theta(s) + \frac{1}{s}B(s) \). The high-pass attenuates the bias-induced low-frequency drift, while the low-pass injects the low-frequency reference from wheels.

5.2 Discrete Implementation Used in Practice

Using the same discretization idea as Section 3, pick \( \alpha = \exp\!\left(-\frac{T_s}{\tau}\right) \). A standard discrete complementary filter for heading is:

\[ \hat{\theta}[k] = \alpha \,\mathrm{wrap}\!\left(\hat{\theta}[k-1] + T_s \,\omega_g[k]\right) + (1-\alpha)\,\theta_w[k]. \]

This formula is simple, stable for \( 0 < \alpha < 1 \), and computationally trivial. The tuning parameter is effectively the crossover frequency (how quickly you trust the wheels to correct drift vs. how strongly you trust IMU integration for fast motion).

flowchart TD
  G["Gyro yaw-rate omega_g"] --> I["Integrate: theta_pred"]
  I --> HP["High-frequency path (dominant when fast)"]
  W["Wheel heading theta_w"] --> LP["Low-frequency path (dominant when slow)"]
  HP --> S["Sum -> theta_hat"]
  LP --> S
  S --> OUT["Heading used for pose integration"]
        

Practical note: when slip is detected (Section 6), you can temporarily increase \( \alpha \) to rely more on the gyro path. This is a deterministic heuristic that significantly improves behavior during brief wheel slip events.

Odometry failures are often dominated by rare but large spikes (missed ticks, sudden slip, timestamp glitches). Classical low-pass filters are not robust: a single spike can pollute the filter state for multiple time steps. Therefore, in practice:

• apply robust outlier suppression before low-pass filtering,
• apply physical feasibility gates (max speed, max yaw rate),
• apply cross-sensor consistency checks (wheel vs gyro yaw-rate) to detect slip.

6.1 Hampel Filter (Median + MAD)

For a window \( \mathcal{W}_k = \{x[k-m],\dots,x[k+m]\} \), define the median \( \mathrm{med}_k \) and median absolute deviation (MAD):

\[ \mathrm{med}_k = \mathrm{median}(\mathcal{W}_k), \quad \mathrm{MAD}_k = \mathrm{median}\left(\left|x[i]-\mathrm{med}_k\right|\right)_{i\in\mathcal{W}_k}. \]

A robust scale estimate is \( \hat{\sigma}_k = 1.4826\,\mathrm{MAD}_k \) (Gaussian-consistent). The Hampel rule replaces an outlier by the median:

\[ x[k] \; ← \begin{cases} \mathrm{med}_k, & \text{if } \left|x[k]-\mathrm{med}_k\right| > n_{\sigma}\hat{\sigma}_k,\\ x[k], & \text{otherwise.} \end{cases} \]

Robustness intuition (formalized in Problems): the median has a 50% breakdown point, so a minority of arbitrarily large spikes cannot drag it arbitrarily far.

6.2 Slip-Aware Gating Using Wheel vs Gyro Yaw-Rate

Define a consistency residual: \( r_{\omega}[k] = \omega_g[k]-\omega_w[k] \). When \( |r_{\omega}[k]| \) exceeds a threshold, treat wheel yaw-rate as unreliable (slip or encoder glitch) and temporarily rely more on gyro integration.

\[ \alpha_{\mathrm{eff}}[k] = \begin{cases} \min(\alpha + \Delta\alpha, \, 0.995), & \text{if } |r_{\omega}[k]| > \gamma,\\ \alpha, & \text{otherwise.} \end{cases} \]

Choosing \( \gamma \): a practical rule is \( \gamma \approx 3\sigma_r \), where \( \sigma_r^2 \approx \sigma_{\omega,g}^2 + \sigma_{\omega,w}^2 \) if noises are roughly independent. This is a deterministic “3-sigma” gate; probabilistic gating will be formalized in Chapter 6.

After filtering, integrate the planar pose using the fused heading \( \hat{\theta}[k] \) and filtered linear velocity \( \hat{v}[k] \):

\[ \begin{aligned} x[k] &= x[k-1] + T_s \,\hat{v}[k]\cos(\hat{\theta}[k]),\\ y[k] &= y[k-1] + T_s \,\hat{v}[k]\sin(\hat{\theta}[k]),\\ \hat{\theta}[k] &= \mathrm{wrap}(\hat{\theta}[k]) \quad \text{(numerical hygiene)}. \end{aligned} \]

Practical note: filtering \( \omega \) and then integrating can introduce lag during fast turns; complementary fusion mitigates this because the gyro path preserves higher-frequency yaw dynamics. For wheel-only odometry, you must choose conservative smoothing or accept heading lag.

The following implementations demonstrate the same algorithmic structure: Hampel outlier suppression, low-pass filtering, slip-aware gating, complementary heading fusion, and pose integration.

8.1 Python (NumPy/SciPy/Matplotlib; ROS2 integration notes)

File: Chapter5_Lesson4.py

"""
Chapter5_Lesson4.py
Autonomous Mobile Robots — Chapter 5 Lesson 4: Practical Odometry Filtering

This script demonstrates practical, deterministic filtering for odometry streams:
- Robust outlier suppression (Hampel filter)
- First-order low-pass filtering (IIR)
- Complementary heading fusion: wheel-based heading (low-frequency) + gyro integration (high-frequency)
- Simple kinematic integration of planar pose (x, y, theta)

Dependencies: numpy, scipy (optional, only for comparison), matplotlib
"""

from __future__ import annotations

import math
from dataclasses import dataclass
from typing import List, Tuple, Optional

import numpy as np
import matplotlib.pyplot as plt


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


@dataclass
class FirstOrderLowPass:
    """y[k] = alpha*y[k-1] + (1-alpha)*x[k] with 0<alpha<1."""
    alpha: float
    y: float = 0.0
    initialized: bool = False

    @staticmethod
    def from_cutoff(fc_hz: float, dt: float) -> "FirstOrderLowPass":
        # Exact discretization of tau*dy/dt + y = x with tau = 1/(2*pi*fc)
        tau = 1.0 / (2.0 * math.pi * fc_hz)
        alpha = math.exp(-dt / tau)
        return FirstOrderLowPass(alpha=alpha)

    def step(self, x: float) -> float:
        if not self.initialized:
            self.y = x
            self.initialized = True
            return self.y
        self.y = self.alpha * self.y + (1.0 - self.alpha) * x
        return self.y


@dataclass
class HampelFilter:
    """
    Hampel filter for outlier suppression:
      x[k] replaced by median if |x[k]-median| > n_sigmas * 1.4826*MAD
    """
    window: int = 11
    n_sigmas: float = 3.0

    def apply(self, x: np.ndarray) -> np.ndarray:
        assert self.window % 2 == 1, "window must be odd"
        k = self.window // 2
        y = x.copy()
        for i in range(k, len(x) - k):
            w = x[i - k : i + k + 1]
            med = np.median(w)
            mad = np.median(np.abs(w - med)) + 1e-12
            sigma = 1.4826 * mad
            if abs(x[i] - med) > self.n_sigmas * sigma:
                y[i] = med
        return y


@dataclass
class ComplementaryHeadingFilter:
    """
    Discrete complementary fusion for heading:
        theta_hat[k] = alpha*(theta_hat[k-1] + dt*omega_g[k]) + (1-alpha)*theta_w[k]
    where theta_w is wheel-based heading (low-freq stable), omega_g is gyro yaw rate (high-freq responsive).
    """
    alpha: float
    theta_hat: float = 0.0
    initialized: bool = False

    @staticmethod
    def from_cutoff(fc_hz: float, dt: float) -> "ComplementaryHeadingFilter":
        tau = 1.0 / (2.0 * math.pi * fc_hz)
        alpha = math.exp(-dt / tau)
        return ComplementaryHeadingFilter(alpha=alpha)

    def step(self, omega_g: float, theta_w: float, dt: float) -> float:
        if not self.initialized:
            self.theta_hat = theta_w
            self.initialized = True
            return self.theta_hat
        pred = wrap_to_pi(self.theta_hat + dt * omega_g)
        self.theta_hat = wrap_to_pi(self.alpha * pred + (1.0 - self.alpha) * theta_w)
        return self.theta_hat


@dataclass
class OdometryFilterConfig:
    dt: float = 0.01
    v_fc_hz: float = 5.0
    w_fc_hz: float = 8.0
    theta_fc_hz: float = 0.7
    v_max: float = 2.0
    w_max: float = 3.0
    slip_gate_rad_s: float = 1.2  # gate residual between gyro and wheel yaw rate


class OdometryFilter:
    def __init__(self, cfg: OdometryFilterConfig):
        self.cfg = cfg
        self.v_lp = FirstOrderLowPass.from_cutoff(cfg.v_fc_hz, cfg.dt)
        self.w_lp = FirstOrderLowPass.from_cutoff(cfg.w_fc_hz, cfg.dt)
        self.theta_cf = ComplementaryHeadingFilter.from_cutoff(cfg.theta_fc_hz, cfg.dt)

        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

    def step(self, v_w: float, w_w: float, w_g: float, theta_w: float) -> Tuple[float, float, float]:
        # Physical sanity clamp
        v_w = float(np.clip(v_w, -self.cfg.v_max, self.cfg.v_max))
        w_w = float(np.clip(w_w, -self.cfg.w_max, self.cfg.w_max))
        w_g = float(np.clip(w_g, -self.cfg.w_max, self.cfg.w_max))

        # Slip gating: if wheel yaw rate disagrees strongly with gyro, downweight wheel yaw (use gyro path more)
        resid = abs(w_g - w_w)
        if resid > self.cfg.slip_gate_rad_s:
            # increase alpha temporarily (more reliance on gyro integration)
            alpha_eff = min(0.995, self.theta_cf.alpha + 0.15)
        else:
            alpha_eff = self.theta_cf.alpha

        # Low-pass filter velocities (reduces quantization and high-frequency jitter)
        v_f = self.v_lp.step(v_w)
        w_f = self.w_lp.step(w_w)

        # Complementary heading fusion (use gyro yaw rate w_g, wheel heading theta_w)
        # Use alpha_eff for this step
        pred = wrap_to_pi(self.theta + self.cfg.dt * w_g)
        theta_hat = wrap_to_pi(alpha_eff * pred + (1.0 - alpha_eff) * theta_w)

        # Integrate pose using filtered linear velocity and fused heading
        self.theta = theta_hat
        self.x += self.cfg.dt * v_f * math.cos(self.theta)
        self.y += self.cfg.dt * v_f * math.sin(self.theta)

        return self.x, self.y, self.theta


def simulate_truth(T: float, dt: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Generate ground-truth v(t), w(t), and theta(t) for a planar robot."""
    N = int(T / dt)
    t = np.arange(N) * dt

    # piecewise commands
    v = np.zeros(N)
    w = np.zeros(N)
    for k in range(N):
        if 1.0 <= t[k] < 6.0:
            v[k] = 1.1
            w[k] = 0.0
        elif 6.0 <= t[k] < 12.0:
            v[k] = 0.8
            w[k] = 0.35
        elif 12.0 <= t[k] < 16.0:
            v[k] = 0.0
            w[k] = -0.6
        elif 16.0 <= t[k] < 20.0:
            v[k] = 1.0
            w[k] = 0.15

    theta = np.zeros(N)
    for k in range(1, N):
        theta[k] = wrap_to_pi(theta[k - 1] + dt * w[k])
    return t, v, w, theta


def make_measurements(v_true: np.ndarray, w_true: np.ndarray, theta_true: np.ndarray, dt: float, seed: int = 2):
    rng = np.random.default_rng(seed)

    # Wheel measurements: noise + quantization + sporadic slip spikes
    v_w = v_true + 0.05 * rng.standard_normal(len(v_true))
    w_w = w_true + 0.08 * rng.standard_normal(len(w_true))

    # quantization effect (like finite encoder resolution, mapped to velocity)
    qv = 0.02
    qw = 0.02
    v_w = np.round(v_w / qv) * qv
    w_w = np.round(w_w / qw) * qw

    # Inject slip spikes
    spike_idx = rng.choice(len(v_true), size=12, replace=False)
    w_w[spike_idx] += rng.normal(loc=0.0, scale=2.0, size=len(spike_idx))
    v_w[spike_idx] += rng.normal(loc=0.0, scale=0.8, size=len(spike_idx))

    # Wheel heading from integrating wheel yaw rate (raw)
    theta_w = np.zeros_like(theta_true)
    for k in range(1, len(theta_w)):
        theta_w[k] = wrap_to_pi(theta_w[k - 1] + dt * w_w[k])

    # Gyro yaw rate: bias + noise + slow bias drift
    bias0 = 0.03
    bias_rw = 0.0005
    bias = np.zeros_like(w_true)
    bias[0] = bias0
    for k in range(1, len(bias)):
        bias[k] = bias[k - 1] + bias_rw * rng.standard_normal()
    w_g = w_true + bias + 0.05 * rng.standard_normal(len(w_true))

    return v_w, w_w, theta_w, w_g


def main():
    dt = 0.01
    T = 20.0

    t, v_true, w_true, theta_true = simulate_truth(T, dt)
    v_w, w_w, theta_w, w_g = make_measurements(v_true, w_true, theta_true, dt)

    # Robust pre-filter: Hampel suppresses spikes before low-pass
    hampel = HampelFilter(window=11, n_sigmas=3.0)
    v_w_h = hampel.apply(v_w)
    w_w_h = hampel.apply(w_w)

    cfg = OdometryFilterConfig(dt=dt)
    filt = OdometryFilter(cfg)

    x_f = np.zeros_like(t)
    y_f = np.zeros_like(t)
    th_f = np.zeros_like(t)

    for k in range(len(t)):
        x_f[k], y_f[k], th_f[k] = filt.step(v_w_h[k], w_w_h[k], w_g[k], theta_w[k])

    # Ground-truth position
    x_true = np.zeros_like(t)
    y_true = np.zeros_like(t)
    for k in range(1, len(t)):
        x_true[k] = x_true[k - 1] + dt * v_true[k] * math.cos(theta_true[k])
        y_true[k] = y_true[k - 1] + dt * v_true[k] * math.sin(theta_true[k])

    # Plots
    fig1 = plt.figure()
    plt.plot(t, v_true, label="v true")
    plt.plot(t, v_w, label="v wheel raw", alpha=0.5)
    plt.plot(t, v_w_h, label="v wheel Hampel", alpha=0.8)
    plt.xlabel("t [s]")
    plt.ylabel("v [m/s]")
    plt.legend()
    plt.title("Linear velocity filtering")

    fig2 = plt.figure()
    plt.plot(t, w_true, label="w true")
    plt.plot(t, w_w, label="w wheel raw", alpha=0.5)
    plt.plot(t, w_w_h, label="w wheel Hampel", alpha=0.8)
    plt.plot(t, w_g, label="w gyro", alpha=0.6)
    plt.xlabel("t [s]")
    plt.ylabel("w [rad/s]")
    plt.legend()
    plt.title("Yaw-rate streams (wheel vs gyro)")

    fig3 = plt.figure()
    plt.plot(x_true, y_true, label="truth")
    plt.plot(x_f, y_f, label="filtered odometry")
    plt.axis("equal")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.legend()
    plt.title("Trajectory comparison")

    fig4 = plt.figure()
    e_th = np.array([wrap_to_pi(th_f[k] - theta_true[k]) for k in range(len(t))])
    plt.plot(t, e_th)
    plt.xlabel("t [s]")
    plt.ylabel("heading error [rad]")
    plt.title("Heading error after complementary fusion")

    plt.show()


if __name__ == "__main__":
    main()
      

8.2 C++ (Standard Library; ROS2 integration notes)

File: Chapter5_Lesson4.cpp

// Chapter5_Lesson4.cpp
// Autonomous Mobile Robots — Chapter 5 Lesson 4: Practical Odometry Filtering
//
// Demonstrates:
//  - Hampel outlier suppression (median + MAD)
//  - First-order low-pass filter for velocities
//  - Complementary fusion for heading: wheel heading (low-freq) + gyro integration (high-freq)
//  - Planar pose integration
//
// Notes on robotics integration:
//  - In ROS2, subscribe to sensor_msgs::msg::Imu (yaw rate) and encoder-derived twist,
//    publish nav_msgs::msg::Odometry with filtered pose/twist.
//  - For message synchronization and time stamps, consider message_filters.
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter5_Lesson4.cpp -o odo_filter
//
// Output:
//   Writes CSV "chapter5_lesson4_out.csv" with columns: t,x,y,theta,v_f,w_f

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

static double wrapToPi(double a) {
  a = std::fmod(a + M_PI, 2.0 * M_PI);
  if (a < 0) a += 2.0 * M_PI;
  return a - M_PI;
}

struct FirstOrderLowPass {
  double alpha;
  double y;
  bool initialized;
  explicit FirstOrderLowPass(double alpha_) : alpha(alpha_), y(0.0), initialized(false) {}

  static FirstOrderLowPass fromCutoff(double fc_hz, double dt) {
    const double tau = 1.0 / (2.0 * M_PI * fc_hz);
    const double alpha = std::exp(-dt / tau);
    return FirstOrderLowPass(alpha);
  }

  double step(double x) {
    if (!initialized) {
      y = x;
      initialized = true;
      return y;
    }
    y = alpha * y + (1.0 - alpha) * x;
    return y;
  }
};

static double median(std::vector<double> w) {
  std::nth_element(w.begin(), w.begin() + w.size() / 2, w.end());
  return w[w.size() / 2];
}

static double mad(const std::vector<double>& w, double med) {
  std::vector<double> d(w.size());
  for (size_t i = 0; i < w.size(); ++i) d[i] = std::abs(w[i] - med);
  return median(d);
}

static std::vector<double> hampelFilter(const std::vector<double>& x, int window, double nSigmas) {
  if (window % 2 == 0) throw std::runtime_error("window must be odd");
  const int k = window / 2;
  std::vector<double> y = x;

  for (int i = k; i < static_cast<int>(x.size()) - k; ++i) {
    std::vector<double> w;
    w.reserve(window);
    for (int j = i - k; j <= i + k; ++j) w.push_back(x[j]);

    const double med = median(w);
    const double MAD = mad(w, med) + 1e-12;
    const double sigma = 1.4826 * MAD;
    if (std::abs(x[i] - med) > nSigmas * sigma) y[i] = med;
  }
  return y;
}

struct ComplementaryHeadingFilter {
  double alpha;
  double thetaHat;
  bool initialized;

  explicit ComplementaryHeadingFilter(double alpha_) : alpha(alpha_), thetaHat(0.0), initialized(false) {}

  static ComplementaryHeadingFilter fromCutoff(double fc_hz, double dt) {
    const double tau = 1.0 / (2.0 * M_PI * fc_hz);
    const double alpha = std::exp(-dt / tau);
    return ComplementaryHeadingFilter(alpha);
  }

  double step(double omegaGyro, double thetaWheel, double dt, double alphaEff) {
    if (!initialized) {
      thetaHat = thetaWheel;
      initialized = true;
      return thetaHat;
    }
    const double pred = wrapToPi(thetaHat + dt * omegaGyro);
    thetaHat = wrapToPi(alphaEff * pred + (1.0 - alphaEff) * thetaWheel);
    return thetaHat;
  }
};

struct Config {
  double dt = 0.01;
  double vFcHz = 5.0;
  double wFcHz = 8.0;
  double thetaFcHz = 0.7;
  double vMax = 2.0;
  double wMax = 3.0;
  double slipGate = 1.2;
};

int main() {
  Config cfg;
  const double T = 20.0;
  const int N = static_cast<int>(T / cfg.dt);

  std::vector<double> t(N), vTrue(N), wTrue(N), thetaTrue(N, 0.0);
  for (int k = 0; k < N; ++k) {
    t[k] = k * cfg.dt;
    if (t[k] >= 1.0 && t[k] < 6.0) {
      vTrue[k] = 1.1;
      wTrue[k] = 0.0;
    } else if (t[k] >= 6.0 && t[k] < 12.0) {
      vTrue[k] = 0.8;
      wTrue[k] = 0.35;
    } else if (t[k] >= 12.0 && t[k] < 16.0) {
      vTrue[k] = 0.0;
      wTrue[k] = -0.6;
    } else if (t[k] >= 16.0 && t[k] < 20.0) {
      vTrue[k] = 1.0;
      wTrue[k] = 0.15;
    } else {
      vTrue[k] = 0.0;
      wTrue[k] = 0.0;
    }
  }
  for (int k = 1; k < N; ++k) thetaTrue[k] = wrapToPi(thetaTrue[k - 1] + cfg.dt * wTrue[k]);

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

  // Wheel measurements (noise + quantization + spikes)
  std::vector<double> vW(N), wW(N);
  const double qv = 0.02, qw = 0.02;
  for (int k = 0; k < N; ++k) {
    vW[k] = vTrue[k] + 0.05 * n01(rng);
    wW[k] = wTrue[k] + 0.08 * n01(rng);
    vW[k] = std::round(vW[k] / qv) * qv;
    wW[k] = std::round(wW[k] / qw) * qw;
  }
  // inject spikes
  for (int s = 0; s < 12; ++s) {
    const int idx = rng() % N;
    wW[idx] += 2.0 * n01(rng);
    vW[idx] += 0.8 * n01(rng);
  }

  // Wheel heading from integrating wheel yaw rate
  std::vector<double> thetaW(N, 0.0);
  for (int k = 1; k < N; ++k) thetaW[k] = wrapToPi(thetaW[k - 1] + cfg.dt * wW[k]);

  // Gyro yaw rate: bias + noise + random walk bias drift
  std::vector<double> wG(N), bias(N);
  bias[0] = 0.03;
  for (int k = 1; k < N; ++k) bias[k] = bias[k - 1] + 0.0005 * n01(rng);
  for (int k = 0; k < N; ++k) wG[k] = wTrue[k] + bias[k] + 0.05 * n01(rng);

  // Robust prefilter
  const std::vector<double> vWh = hampelFilter(vW, 11, 3.0);
  const std::vector<double> wWh = hampelFilter(wW, 11, 3.0);

  FirstOrderLowPass vLP = FirstOrderLowPass::fromCutoff(cfg.vFcHz, cfg.dt);
  FirstOrderLowPass wLP = FirstOrderLowPass::fromCutoff(cfg.wFcHz, cfg.dt);
  ComplementaryHeadingFilter thetaCF = ComplementaryHeadingFilter::fromCutoff(cfg.thetaFcHz, cfg.dt);

  double x = 0.0, y = 0.0, theta = 0.0;

  std::ofstream out("chapter5_lesson4_out.csv");
  out << "t,x,y,theta,v_f,w_f\n";

  for (int k = 0; k < N; ++k) {
    double v = std::clamp(vWh[k], -cfg.vMax, cfg.vMax);
    double ww = std::clamp(wWh[k], -cfg.wMax, cfg.wMax);
    double wg = std::clamp(wG[k], -cfg.wMax, cfg.wMax);

    const double resid = std::abs(wg - ww);
    const double alphaEff = (resid > cfg.slipGate) ? std::min(0.995, thetaCF.alpha + 0.15) : thetaCF.alpha;

    const double vF = vLP.step(v);
    const double wF = wLP.step(ww);

    theta = thetaCF.step(wg, thetaW[k], cfg.dt, alphaEff);

    x += cfg.dt * vF * std::cos(theta);
    y += cfg.dt * vF * std::sin(theta);

    out << t[k] << "," << x << "," << y << "," << theta << "," << vF << "," << wF << "\n";
  }

  out.close();
  std::cout << "Wrote chapter5_lesson4_out.csv\n";
  return 0;
}
      

8.3 Java (Library-light; EJML/rosjava notes)

File: Chapter5_Lesson4.java

// Chapter5_Lesson4.java
// Autonomous Mobile Robots — Chapter 5 Lesson 4: Practical Odometry Filtering
//
// Demonstrates practical deterministic odometry filtering:
//  - Hampel outlier suppression (median + MAD)
//  - First-order low-pass filtering (IIR)
//  - Complementary heading fusion: wheel heading (low-freq) + gyro integration (high-freq)
//  - Planar pose integration
//
// Notes on robotics integration:
//  - In ROS ecosystems, Java integrations exist (e.g., rosjava) but are less common in AMR stacks.
//  - This code is written to be library-light. If you want matrix utilities, EJML is a good choice.
//
// Compile:
//   javac Chapter5_Lesson4.java
// Run:
//   java Chapter5_Lesson4
//
// Output:
//   Writes CSV "chapter5_lesson4_out_java.csv".

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

public class Chapter5_Lesson4 {

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

  static class FirstOrderLowPass {
    final double alpha;
    double y = 0.0;
    boolean initialized = false;

    FirstOrderLowPass(double alpha) { this.alpha = alpha; }

    static FirstOrderLowPass fromCutoff(double fcHz, double dt) {
      double tau = 1.0 / (2.0 * Math.PI * fcHz);
      double alpha = Math.exp(-dt / tau);
      return new FirstOrderLowPass(alpha);
    }

    double step(double x) {
      if (!initialized) {
        y = x;
        initialized = true;
        return y;
      }
      y = alpha * y + (1.0 - alpha) * x;
      return y;
    }
  }

  static double median(double[] w) {
    double[] c = w.clone();
    Arrays.sort(c);
    return c[c.length / 2];
  }

  static double mad(double[] w, double med) {
    double[] d = new double[w.length];
    for (int i = 0; i < w.length; i++) d[i] = Math.abs(w[i] - med);
    return median(d);
  }

  static double[] hampelFilter(double[] x, int window, double nSigmas) {
    if (window % 2 == 0) throw new IllegalArgumentException("window must be odd");
    int k = window / 2;
    double[] y = x.clone();

    for (int i = k; i < x.length - k; i++) {
      double[] w = new double[window];
      for (int j = 0; j < window; j++) w[j] = x[i - k + j];

      double med = median(w);
      double MAD = mad(w, med) + 1e-12;
      double sigma = 1.4826 * MAD;
      if (Math.abs(x[i] - med) > nSigmas * sigma) y[i] = med;
    }
    return y;
  }

  static class ComplementaryHeadingFilter {
    final double alpha;
    double thetaHat = 0.0;
    boolean initialized = false;

    ComplementaryHeadingFilter(double alpha) { this.alpha = alpha; }

    static ComplementaryHeadingFilter fromCutoff(double fcHz, double dt) {
      double tau = 1.0 / (2.0 * Math.PI * fcHz);
      double alpha = Math.exp(-dt / tau);
      return new ComplementaryHeadingFilter(alpha);
    }

    double step(double omegaGyro, double thetaWheel, double dt, double alphaEff) {
      if (!initialized) {
        thetaHat = thetaWheel;
        initialized = true;
        return thetaHat;
      }
      double pred = wrapToPi(thetaHat + dt * omegaGyro);
      thetaHat = wrapToPi(alphaEff * pred + (1.0 - alphaEff) * thetaWheel);
      return thetaHat;
    }
  }

  static double clamp(double x, double lo, double hi) {
    return Math.max(lo, Math.min(hi, x));
  }

  public static void main(String[] args) throws IOException {
    double dt = 0.01;
    double T = 20.0;
    int N = (int)(T / dt);

    double[] t = new double[N];
    double[] vTrue = new double[N];
    double[] wTrue = new double[N];
    double[] thetaTrue = new double[N];

    for (int k = 0; k < N; k++) {
      t[k] = k * dt;
      if (t[k] >= 1.0 && t[k] < 6.0) { vTrue[k] = 1.1; wTrue[k] = 0.0; }
      else if (t[k] >= 6.0 && t[k] < 12.0) { vTrue[k] = 0.8; wTrue[k] = 0.35; }
      else if (t[k] >= 12.0 && t[k] < 16.0) { vTrue[k] = 0.0; wTrue[k] = -0.6; }
      else if (t[k] >= 16.0 && t[k] < 20.0) { vTrue[k] = 1.0; wTrue[k] = 0.15; }
      else { vTrue[k] = 0.0; wTrue[k] = 0.0; }
    }

    for (int k = 1; k < N; k++) thetaTrue[k] = wrapToPi(thetaTrue[k - 1] + dt * wTrue[k]);

    Random rng = new Random(2);
    // Wheel measurements: noise + quantization + spikes
    double[] vW = new double[N];
    double[] wW = new double[N];
    double qv = 0.02, qw = 0.02;

    for (int k = 0; k < N; k++) {
      vW[k] = vTrue[k] + 0.05 * rng.nextGaussian();
      wW[k] = wTrue[k] + 0.08 * rng.nextGaussian();
      vW[k] = Math.rint(vW[k] / qv) * qv;
      wW[k] = Math.rint(wW[k] / qw) * qw;
    }

    for (int s = 0; s < 12; s++) {
      int idx = rng.nextInt(N);
      wW[idx] += 2.0 * rng.nextGaussian();
      vW[idx] += 0.8 * rng.nextGaussian();
    }

    // Wheel heading from integrating wheel yaw rate
    double[] thetaW = new double[N];
    for (int k = 1; k < N; k++) thetaW[k] = wrapToPi(thetaW[k - 1] + dt * wW[k]);

    // Gyro yaw rate: bias + noise + random walk bias drift
    double[] wG = new double[N];
    double[] bias = new double[N];
    bias[0] = 0.03;
    for (int k = 1; k < N; k++) bias[k] = bias[k - 1] + 0.0005 * rng.nextGaussian();
    for (int k = 0; k < N; k++) wG[k] = wTrue[k] + bias[k] + 0.05 * rng.nextGaussian();

    // Robust pre-filter
    double[] vWh = hampelFilter(vW, 11, 3.0);
    double[] wWh = hampelFilter(wW, 11, 3.0);

    // Filters
    double vFcHz = 5.0, wFcHz = 8.0, thetaFcHz = 0.7;
    double vMax = 2.0, wMax = 3.0, slipGate = 1.2;

    FirstOrderLowPass vLP = FirstOrderLowPass.fromCutoff(vFcHz, dt);
    FirstOrderLowPass wLP = FirstOrderLowPass.fromCutoff(wFcHz, dt);
    ComplementaryHeadingFilter thetaCF = ComplementaryHeadingFilter.fromCutoff(thetaFcHz, dt);

    double x = 0.0, y = 0.0, theta = 0.0;

    FileWriter out = new FileWriter("chapter5_lesson4_out_java.csv");
    out.write("t,x,y,theta,v_f,w_f\n");

    for (int k = 0; k < N; k++) {
      double v = clamp(vWh[k], -vMax, vMax);
      double ww = clamp(wWh[k], -wMax, wMax);
      double wg = clamp(wG[k], -wMax, wMax);

      double resid = Math.abs(wg - ww);
      double alphaEff = (resid > slipGate) ? Math.min(0.995, thetaCF.alpha + 0.15) : thetaCF.alpha;

      double vF = vLP.step(v);
      double wF = wLP.step(ww);

      theta = thetaCF.step(wg, thetaW[k], dt, alphaEff);

      x += dt * vF * Math.cos(theta);
      y += dt * vF * Math.sin(theta);

      out.write(String.format(java.util.Locale.US, "%f,%f,%f,%f,%f,%f\n",
          t[k], x, y, theta, vF, wF));
    }

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

8.4 MATLAB / Simulink (Robotics System Toolbox optional)

File: Chapter5_Lesson4.m

% Chapter5_Lesson4.m
% Autonomous Mobile Robots — Chapter 5 Lesson 4: Practical Odometry Filtering
%
% Demonstrates:
%  - Hampel outlier suppression (median + MAD)
%  - First-order low-pass filtering (IIR)
%  - Complementary heading fusion: wheel heading (low-freq) + gyro integration (high-freq)
%  - Planar pose integration
%  - Programmatic construction of a simple Simulink model implementing the same filter logic
%
% Toolboxes (optional but useful):
%  - Robotics System Toolbox (for ROS message types, not required here)
%  - Simulink (required to build the model)
%
% Run:
%   Chapter5_Lesson4
%
% Output:
%   - Plots
%   - A Simulink model file: Chapter5_Lesson4_OdometryFilter.slx (if Simulink is available)

clear; clc; close all;

dt = 0.01;
T = 20.0;
N = floor(T/dt);
t = (0:N-1)' * dt;

% --- Truth signals
vTrue = zeros(N,1);
wTrue = zeros(N,1);
for k = 1:N
    if t(k) >= 1.0 && t(k) < 6.0
        vTrue(k) = 1.1; wTrue(k) = 0.0;
    elseif t(k) >= 6.0 && t(k) < 12.0
        vTrue(k) = 0.8; wTrue(k) = 0.35;
    elseif t(k) >= 12.0 && t(k) < 16.0
        vTrue(k) = 0.0; wTrue(k) = -0.6;
    elseif t(k) >= 16.0 && t(k) < 20.0
        vTrue(k) = 1.0; wTrue(k) = 0.15;
    end
end

thetaTrue = zeros(N,1);
for k = 2:N
    thetaTrue(k) = wrapToPi(thetaTrue(k-1) + dt*wTrue(k));
end

% --- Measurements
rng(2);
vW = vTrue + 0.05*randn(N,1);
wW = wTrue + 0.08*randn(N,1);

qv = 0.02; qw = 0.02;
vW = round(vW/qv)*qv;
wW = round(wW/qw)*qw;

spikeIdx = randperm(N, 12);
vW(spikeIdx) = vW(spikeIdx) + 0.8*randn(12,1);
wW(spikeIdx) = wW(spikeIdx) + 2.0*randn(12,1);

thetaW = zeros(N,1);
for k = 2:N
    thetaW(k) = wrapToPi(thetaW(k-1) + dt*wW(k));
end

bias = zeros(N,1);
bias(1) = 0.03;
for k = 2:N
    bias(k) = bias(k-1) + 0.0005*randn();
end
wG = wTrue + bias + 0.05*randn(N,1);

% --- Hampel filter
vWh = hampel1d(vW, 11, 3.0);
wWh = hampel1d(wW, 11, 3.0);

% --- Low-pass coefficients (exact discretization)
vFc = 5.0;  wFc = 8.0;  thFc = 0.7;
vAlpha = exp(-dt / (1/(2*pi*vFc)));
wAlpha = exp(-dt / (1/(2*pi*wFc)));
thAlpha = exp(-dt / (1/(2*pi*thFc)));

vMax = 2.0; wMax = 3.0; slipGate = 1.2;

% --- Filtering loop
xF = zeros(N,1); yF = zeros(N,1); thF = zeros(N,1);
vF = zeros(N,1); wF = zeros(N,1);

for k = 1:N
    v = min(max(vWh(k), -vMax), vMax);
    ww = min(max(wWh(k), -wMax), wMax);
    wg = min(max(wG(k), -wMax), wMax);

    if k == 1
        vF(k) = v; wF(k) = ww; thF(k) = thetaW(k);
    else
        vF(k) = vAlpha*vF(k-1) + (1-vAlpha)*v;
        wF(k) = wAlpha*wF(k-1) + (1-wAlpha)*ww;

        resid = abs(wg - ww);
        alphaEff = thAlpha;
        if resid > slipGate
            alphaEff = min(0.995, thAlpha + 0.15);
        end

        pred = wrapToPi(thF(k-1) + dt*wg);
        thF(k) = wrapToPi(alphaEff*pred + (1-alphaEff)*thetaW(k));
    end

    if k == 1
        xF(k) = 0; yF(k) = 0;
    else
        xF(k) = xF(k-1) + dt*vF(k)*cos(thF(k));
        yF(k) = yF(k-1) + dt*vF(k)*sin(thF(k));
    end
end

% --- Truth position
xTrue = zeros(N,1); yTrue = zeros(N,1);
for k = 2:N
    xTrue(k) = xTrue(k-1) + dt*vTrue(k)*cos(thetaTrue(k));
    yTrue(k) = yTrue(k-1) + dt*vTrue(k)*sin(thetaTrue(k));
end

figure; plot(t, vTrue, 'LineWidth', 1.2); hold on;
plot(t, vW, 'LineWidth', 0.7);
plot(t, vWh, 'LineWidth', 1.0);
legend('v true','v wheel raw','v wheel Hampel'); xlabel('t [s]'); ylabel('v [m/s]');
title('Linear velocity filtering');

figure; plot(t, wTrue, 'LineWidth', 1.2); hold on;
plot(t, wW, 'LineWidth', 0.7);
plot(t, wWh, 'LineWidth', 1.0);
plot(t, wG, 'LineWidth', 0.9);
legend('w true','w wheel raw','w wheel Hampel','w gyro'); xlabel('t [s]'); ylabel('w [rad/s]');
title('Yaw-rate streams');

figure; plot(xTrue, yTrue, 'LineWidth', 1.2); hold on;
plot(xF, yF, 'LineWidth', 1.2); axis equal; grid on;
legend('truth','filtered odometry'); xlabel('x [m]'); ylabel('y [m]');
title('Trajectory comparison');

figure; eTh = wrapToPi(thF - thetaTrue);
plot(t, eTh, 'LineWidth', 1.2); xlabel('t [s]'); ylabel('heading error [rad]');
title('Heading error after complementary fusion');

% --- Build Simulink model programmatically (optional)
try
    buildSimulinkModel();
    disp('Built Simulink model: Chapter5_Lesson4_OdometryFilter.slx');
catch ME
    disp('Simulink model build skipped (Simulink may be unavailable):');
    disp(ME.message);
end

% ---------- Local functions ----------
function y = wrapToPi(a)
    y = mod(a + pi, 2*pi) - pi;
end

function y = hampel1d(x, window, nSigmas)
    if mod(window,2) == 0
        error('window must be odd');
    end
    k = floor(window/2);
    y = x;
    for i = (k+1):(length(x)-k)
        w = x((i-k):(i+k));
        med = median(w);
        MAD = median(abs(w-med)) + 1e-12;
        sigma = 1.4826*MAD;
        if abs(x(i)-med) > nSigmas*sigma
            y(i) = med;
        end
    end
end

function buildSimulinkModel()
    mdl = 'Chapter5_Lesson4_OdometryFilter';
    if bdIsLoaded(mdl)
        close_system(mdl, 0);
    end
    new_system(mdl);
    open_system(mdl);

    % Blocks: In1 (v), In1 (w_g), In1 (theta_w), Unit Delay, Gain, Sum, etc.
    % This is a compact educational model (not a full ROS pipeline).

    add_block('simulink/Sources/In1', [mdl '/v_w'], 'Position', [30 50 60 70]);
    add_block('simulink/Sources/In1', [mdl '/omega_g'], 'Position', [30 120 60 140]);
    add_block('simulink/Sources/In1', [mdl '/theta_w'], 'Position', [30 190 60 210]);

    % Discrete low-pass for v: y = alpha*z^-1*y + (1-alpha)*x
    add_block('simulink/Discrete/Unit Delay', [mdl '/z1_v'], 'Position', [150 45 190 75]);
    add_block('simulink/Math Operations/Gain', [mdl '/alpha_v'], 'Gain', '0.9', 'Position', [230 45 280 75]);
    add_block('simulink/Math Operations/Gain', [mdl '/one_minus_alpha_v'], 'Gain', '0.1', 'Position', [230 90 280 120]);
    add_block('simulink/Math Operations/Sum', [mdl '/sum_v'], 'Inputs', '++', 'Position', [320 60 350 100]);

    add_line(mdl, 'v_w/1', 'one_minus_alpha_v/1');
    add_line(mdl, 'z1_v/1', 'alpha_v/1');
    add_line(mdl, 'alpha_v/1', 'sum_v/1');
    add_line(mdl, 'one_minus_alpha_v/1', 'sum_v/2');
    add_line(mdl, 'sum_v/1', 'z1_v/1');

    add_block('simulink/Sinks/Out1', [mdl '/v_f'], 'Position', [390 70 420 90]);
    add_line(mdl, 'sum_v/1', 'v_f/1');

    % Complementary heading: theta = alpha*(z^-1*theta + dt*omega_g) + (1-alpha)*theta_w
    add_block('simulink/Discrete/Unit Delay', [mdl '/z1_theta'], 'Position', [150 160 190 190]);
    add_block('simulink/Math Operations/Gain', [mdl '/dt'], 'Gain', '0.01', 'Position', [150 110 190 140]);
    add_block('simulink/Math Operations/Sum', [mdl '/pred'], 'Inputs', '++', 'Position', [230 135 260 175]);

    add_block('simulink/Math Operations/Gain', [mdl '/alpha_theta'], 'Gain', '0.95', 'Position', [300 135 350 165]);
    add_block('simulink/Math Operations/Gain', [mdl '/one_minus_alpha_theta'], 'Gain', '0.05', 'Position', [300 195 350 225]);
    add_block('simulink/Math Operations/Sum', [mdl '/sum_theta'], 'Inputs', '++', 'Position', [390 160 420 200]);
    add_block('simulink/Sinks/Out1', [mdl '/theta_f'], 'Position', [460 170 490 190]);

    add_line(mdl, 'omega_g/1', 'dt/1');
    add_line(mdl, 'dt/1', 'pred/2');
    add_line(mdl, 'z1_theta/1', 'pred/1');

    add_line(mdl, 'pred/1', 'alpha_theta/1');
    add_line(mdl, 'theta_w/1', 'one_minus_alpha_theta/1');
    add_line(mdl, 'alpha_theta/1', 'sum_theta/1');
    add_line(mdl, 'one_minus_alpha_theta/1', 'sum_theta/2');
    add_line(mdl, 'sum_theta/1', 'z1_theta/1');
    add_line(mdl, 'sum_theta/1', 'theta_f/1');

    set_param(mdl, 'StopTime', '20');
    save_system(mdl);
    close_system(mdl, 1);
end
      

8.5 Wolfram Mathematica

File: Chapter5_Lesson4.nb

(* ::Package:: *)

(* Chapter5_Lesson4.nb
   Autonomous Mobile Robots — Chapter 5 Lesson 4: Practical Odometry Filtering

   This notebook-style script (saved as .nb text) derives and demonstrates:
   - Discrete first-order low-pass filter (exact discretization)
   - Moving-average frequency response
   - Complementary heading fusion in discrete time
*)

Notebook[{
  Cell["Chapter 5 Lesson 4: Practical Odometry Filtering", "Title"],
  Cell["1. First-order low-pass filter: exact discretization", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      (* Continuous-time model: tau y'(t) + y(t) = x(t) *)
      "tau y'(t) + y(t) = x(t)"
    ], "Text"
  ],
  Cell[BoxData@ToBoxes@
    HoldForm[
      (* Exact discrete update with zero-order hold on x *)
      "y[k] = alpha y[k-1] + (1-alpha) x[k],  alpha = Exp[-Ts/tau]"
    ], "Text"
  ],
  Cell[BoxData@ToBoxes@
    Module[{tau = 0.3, Ts = 0.01, alpha, n = 2000, x, y},
      alpha = Exp[-Ts/tau];
      x = Join[ConstantArray[0, 200], ConstantArray[1, n - 200]];
      y = NestList[alpha #1 + (1 - alpha) #2 &, x[[1]], Rest[x]];
      ListLinePlot[{x, y}, PlotLegends -> {"x[k]", "y[k]"}, AxesLabel -> {"k", "value"}]
    ], "Input"
  ],
  Cell["2. Moving average frequency response", "Section"],
  Cell[BoxData@ToBoxes@
    Module[{N = 15, w},
      w = Subdivide[0, Pi, 500];
      (* |H(e^{jw})| for moving average *)
      ListLinePlot[
        Transpose@{w, Abs[Sin[N w/2]/(N Sin[w/2])]},
        AxesLabel -> {"w", "|H(e^{jw})|"},
        PlotRange -> All
      ]
    ], "Input"
  ],
  Cell["3. Complementary heading fusion (discrete)", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      "theta_hat[k] = alpha (theta_hat[k-1] + Ts omega_g[k]) + (1-alpha) theta_w[k]"
    ], "Text"
  ],
  Cell[BoxData@ToBoxes@
    Module[{Ts = 0.01, tau = 0.5, alpha, n = 3000, t, wTrue, bias, wG, wW, thetaW, thetaHat},
      alpha = Exp[-Ts/tau];
      t = Range[0, n - 1] Ts;
      wTrue = 0.4 Exp[-(t - 5)^2/5] + 0.2 UnitStep[t - 10];
      bias = 0.03 + Accumulate[0.0002 RandomVariate[NormalDistribution[], n]];
      wG = wTrue + bias + 0.05 RandomVariate[NormalDistribution[], n];
      wW = wTrue + 0.10 RandomVariate[NormalDistribution[], n];
      thetaW = FoldList[Mod[#1 + Ts #2 + Pi, 2 Pi] - Pi &, 0, wW];
      thetaHat = FoldList[
        Mod[alpha (Mod[#1 + Ts #2 + Pi, 2 Pi] - Pi) + (1 - alpha) #3 + Pi, 2 Pi] - Pi &,
        thetaW[[1]], Transpose[{wG, thetaW}]
      ];
      ListLinePlot[{thetaW, thetaHat}, PlotLegends -> {"wheel heading", "complementary theta_hat"},
        AxesLabel -> {"k", "rad"}]
    ], "Input"
  ]
},
WindowSize -> {1100, 700},
WindowTitle -> "Chapter5_Lesson4"
]
      

Problem 1 (Exact discretization of a first-order low-pass): Starting from the continuous-time model \( \tau \dot{y}(t) + y(t) = x(t) \), assume ZOH on \( x(t) \) over each sampling interval of length \( T_s \). Derive the exact discrete recursion and show that \( 0 < \alpha < 1 \) when \( \tau > 0 \).

Solution: Solve the linear ODE on \( [kT_s,(k+1)T_s) \) with constant input \( x(t)=x[k] \):

\[ \dot{y}(t) + \frac{1}{\tau}y(t) = \frac{1}{\tau}x[k]. \]

The solution with initial condition \( y(kT_s)=y[k-1] \) is:

\[ y((k+1)T_s) = e^{-\frac{T_s}{\tau}} y[k-1] + \left(1-e^{-\frac{T_s}{\tau}}\right) x[k]. \]

Define \( \alpha = e^{-\frac{T_s}{\tau}} \) to obtain \( y[k] = \alpha y[k-1] + (1-\alpha)x[k] \). If \( \tau > 0 \) and \( T_s > 0 \) then \( -\frac{T_s}{\tau} < 0 \), hence \( 0 < e^{-\frac{T_s}{\tau}} < 1 \), i.e., \( 0 < \alpha < 1 \).

Problem 2 (Moving average linear phase and group delay): Show that the moving average filter \( y[k]=\frac{1}{N}\sum_{i=0}^{N-1}x[k-i] \) has linear phase and group delay \( \tau_g=\frac{N-1}{2}T_s \).

Solution: The impulse response is \( h[i]=\frac{1}{N} \) for \( i=0,\dots,N-1 \), and \( 0 \) otherwise. This is symmetric about \( \frac{N-1}{2} \), i.e., \( h[i]=h[N-1-i] \). Such symmetric FIR filters have linear phase. The frequency response can be written as:

\[ H(e^{j\Omega}) = e^{-j\Omega\frac{N-1}{2}} A(\Omega), \]

where \( A(\Omega) \) is real and even. Therefore the phase is \( -\Omega\frac{N-1}{2} \) (modulo sign flips at zeros), and group delay is \( \tau_g = -\frac{d}{d\Omega}\left(-\Omega\frac{N-1}{2}\right)T_s=\frac{N-1}{2}T_s \).

Problem 3 (Complementary identity and steady behavior): Prove that \( H_{LP}(s)+H_{HP}(s)=1 \) for \( H_{LP}(s)=\frac{1}{1+s\tau} \), \( H_{HP}(s)=\frac{s\tau}{1+s\tau} \). Then interpret what this means when the gyro has a constant bias \( b_0 \).

Solution: Direct algebra yields:

\[ H_{LP}(s)+H_{HP}(s) = \frac{1}{1+s\tau} + \frac{s\tau}{1+s\tau} = \frac{1+s\tau}{1+s\tau} = 1. \]

Thus, across frequencies, the two paths “sum” to unity gain. If the gyro has constant bias \( b_0 \), then integrating yaw-rate adds a ramp component to heading. The high-pass path attenuates low-frequency (including near-DC) components, reducing the contribution of bias-induced drift, while the low-pass path injects wheel heading at low frequencies to anchor the estimate.

Problem 4 (Robustness of the Hampel median rule): Explain why a median-based center estimate can tolerate up to (nearly) 50% arbitrary outliers in a window without diverging.

Solution: The median is an order statistic: it depends only on the rank ordering. If fewer than half the samples are corrupted, the median must still lie within the range of the uncorrupted samples (because at least half the data remain “good”, so the middle-ranked element cannot be forced beyond the good set). Hence its breakdown point is 50%, meaning an adversary must corrupt at least half the data in the window to drive the median arbitrarily far. The Hampel filter inherits this robustness because it uses the median and MAD (also median-based) to detect and replace spikes.

We treated odometry as a set of discrete-time signals and derived practical filters that are widely used before introducing probabilistic localization. You derived the exact discrete first-order low-pass from a continuous-time model, proved stability via impulse-response summability, and analyzed FIR moving-average phase delay. You then constructed a complementary heading fusion method combining wheel heading and gyro integration, and integrated robust statistics (Hampel) and slip-aware gating to handle outliers. These methods provide a strong baseline and a clean bridge to Chapter 6, where uncertainty and Bayes filtering will be made explicit.