By the end of this lab you will be able to:

• Implement the Bayes-filter recursion for a discrete state space \( x_t \in \mathcal{X} \) and compute the belief \( bel_t(x_t) \).
• Build a motion model as a stochastic transition operator and recognize when it reduces to a cyclic convolution.
• Build a sensor likelihood function and perform a numerically stable correction step with explicit normalization.
• Validate your implementation with sanity checks (mass conservation, non-negativity, predictable failure modes).

Files (included in the ZIP at the end of this lesson): Chapter6_Lesson5.py, Chapter6_Lesson5_Ex1.py, Chapter6_Lesson5.cpp, Chapter6_Lesson5.java, Chapter6_Lesson5.m, Chapter6_Lesson5_SimulinkBuild.m, Chapter6_Lesson5.nb.

flowchart TD
  S["Start"] --> I["Choose grid X, initialize bel0(x)"]
  I --> LOOP["For t = 1..T"]
  LOOP --> UZ["Read control u_t and measurement z_t"]
  UZ --> P["Prediction: bel_bar(x) = sum_xprev p(x|u_t,xprev)*bel_prev(xprev)"]
  P --> L["Likelihood: l(x) = p(z_t|x)"]
  L --> C["Correction: bel(x) = eta * l(x) * bel_bar(x)"]
  C --> N["Normalize: eta = 1 / sum_x l(x)*bel_bar(x)"]
  N --> E["Estimate: x_hat = argmax_x bel(x)"]
  E --> LOG["Log diagnostics (sum=1, min>=0, entropy)"]
  LOG --> LOOP
        

We represent robot pose on a discrete grid. Let the state be \( x_t \in \mathcal{X}=\{x^{(1)},\dots,x^{(N)}\} \). The belief is the posterior distribution: \( bel_t(x_t) \): the probability mass function over states conditioned on all data up to time \( t \).

Formally, \( bel_t(x_t) \):

\[ bel_t(x_t) \;=\; p\!\left(x_t \mid z_{1:t},\, u_{1:t}\right), \quad z_{1:t} := \{z_1,\dots,z_t\},\;\; u_{1:t} := \{u_1,\dots,u_t\}. \]

In this lab, \( x_t \) is a 1D cyclic position (a ring track) to make the full loop implementable in a single session without introducing higher-dimensional approximations. The same recursion applies to 2D/3D pose grids; only computational cost changes.

The Bayes filter is a two-step recursion: prediction (motion update) and correction (measurement update). The derivation uses the assumptions emphasized in Lesson 4:

• First-order Markov motion: \( p(x_t \mid x_{0:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t) \).
• Conditional independence of measurement: \( p(z_t \mid x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t) \).

Proposition 1 (Prediction step): The predicted belief \( \overline{bel}_t(x_t) \) satisfies

\[ \overline{bel}_t(x_t) \;=\; \sum_{x_{t-1}\in\mathcal{X}} p\!\left(x_t \mid u_t, x_{t-1}\right)\, bel_{t-1}(x_{t-1}). \]

Proof:

\[ \begin{aligned} \overline{bel}_t(x_t) &:= p\!\left(x_t \mid z_{1:t-1}, u_{1:t}\right) \\ &= \sum_{x_{t-1}\in\mathcal{X}} p\!\left(x_t, x_{t-1} \mid z_{1:t-1}, u_{1:t}\right) \\ &= \sum_{x_{t-1}\in\mathcal{X}} p\!\left(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t}\right)\, p\!\left(x_{t-1} \mid z_{1:t-1}, u_{1:t}\right) \\ &\overset{\text{Markov}}{=} \sum_{x_{t-1}\in\mathcal{X}} p\!\left(x_t \mid x_{t-1}, u_t\right)\, p\!\left(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}\right) \\ &= \sum_{x_{t-1}\in\mathcal{X}} p\!\left(x_t \mid u_t, x_{t-1}\right)\, bel_{t-1}(x_{t-1}). \end{aligned} \]

The key step is that \( x_t \) depends on the past only through \( (x_{t-1},u_t) \).

Proposition 2 (Correction step): The posterior belief satisfies

\[ bel_t(x_t) \;=\; \eta_t\; p(z_t \mid x_t)\;\overline{bel}_t(x_t), \quad \eta_t := \frac{1}{p(z_t \mid z_{1:t-1}, u_{1:t})}. \]

Proof: Apply Bayes’ rule and conditional independence.

\[ \begin{aligned} bel_t(x_t) &= p\!\left(x_t \mid z_{1:t}, u_{1:t}\right) \\ &= \frac{p\!\left(z_t \mid x_t, z_{1:t-1}, u_{1:t}\right)\;p\!\left(x_t \mid z_{1:t-1}, u_{1:t}\right)} {p\!\left(z_t \mid z_{1:t-1}, u_{1:t}\right)} \\ &\overset{\text{cond. indep.}}{=} \frac{p(z_t \mid x_t)\;\overline{bel}_t(x_t)}{p\!\left(z_t \mid z_{1:t-1}, u_{1:t}\right)} \;=\; \eta_t\;p(z_t \mid x_t)\;\overline{bel}_t(x_t). \end{aligned} \]

Proposition 3 (Normalization constant): The constant \( \eta_t \) can be computed by summation:

\[ \eta_t \;=\; \left( \sum_{x_t\in\mathcal{X}} p(z_t \mid x_t)\;\overline{bel}_t(x_t) \right)^{-1}. \]

Proof: Sum both sides of Proposition 2 over \( x_t \) and impose \( \sum_{x_t} bel_t(x_t)=1 \).

For implementation, it is convenient to stack probabilities into a vector: \( \mathbf{b}_t \in \mathbb{R}^{N} \) where \( [\mathbf{b}_t]_i = bel_t(x^{(i)}) \). Define the transition matrix \( \mathbf{T}(u_t)\in\mathbb{R}^{N\times N} \): \( [\mathbf{T}(u_t)]_{i j} = p(x^{(i)} \mid u_t, x^{(j)}) \). Let \( \mathbf{L}(z_t)=\mathrm{diag}(\boldsymbol{\ell}_t) \) with \( [\boldsymbol{\ell}_t]_i = p(z_t \mid x^{(i)}) \).

\[ \overline{\mathbf{b}}_t = \mathbf{T}(u_t)\,\mathbf{b}_{t-1}, \quad \mathbf{b}_t = \eta_t\;\mathbf{L}(z_t)\,\overline{\mathbf{b}}_t, \quad \eta_t = \left(\mathbf{1}^\top \mathbf{L}(z_t)\,\overline{\mathbf{b}}_t\right)^{-1}. \]

Mass conservation (important implementation invariant):

If \( \mathbf{T}(u_t) \) is column-stochastic, i.e. \( \sum_{i=1}^N [\mathbf{T}(u_t)]_{i j} = 1 \) for every column \( j \), and \( \mathbf{b}_{t-1} \) is a valid distribution, then \( \overline{\mathbf{b}}_t \) is also a valid distribution:

\[ \sum_{i=1}^N [\overline{\mathbf{b}}_t]_i \;=\; \sum_{i=1}^N \sum_{j=1}^N [\mathbf{T}]_{i j}[\mathbf{b}_{t-1}]_j \;=\; \sum_{j=1}^N \left(\sum_{i=1}^N [\mathbf{T}]_{i j}\right)[\mathbf{b}_{t-1}]_j \;=\; \sum_{j=1}^N 1\cdot [\mathbf{b}_{t-1}]_j \;=\; 1. \]

flowchart TD
  B0["b[t-1] (belief vector)"] --> T["T(u[t]) (transition operator)"]
  T --> BB["b_bar[t] (predicted)"]
  BB --> MUL["Elementwise multiply with l[t]=p(z[t]|x)"]
  L["l[t]"] --> MUL
  MUL --> NORM["Normalize by sum"]
  NORM --> B1["b[t] (posterior)"]
  B1 --> MAP["MAP: argmax(b[t])"]
        

We model a robot driving on a ring track of length \( L \) discretized into \( N \) cells. The state is cell index \( i \in \{0,\dots,N-1\} \) (or physical position \( x=i\,\Delta x \)).

Motion model (Gaussian translation on a ring):

Let the commanded translation be \( \Delta \) meters per step with motion noise \( \sigma_u \). On the discrete ring, the transition probability depends only on offset, making it a cyclic convolution:

\[ p(x_t=x^{(i)} \mid u_t, x_{t-1}=x^{(j)}) \;=\; \kappa\;\exp\!\left( -\frac{1}{2}\frac{(d(i-j)-\mu)^2}{\sigma^2} \right), \]

where \( d(\cdot) \) is the signed cyclic offset (wrapped to the nearest representative), \( \mu=\Delta/\Delta x \) is the mean shift in cells, \( \sigma=\sigma_u/\Delta x \) is the std in cells, and \( \kappa \) normalizes the discrete kernel so the conditional distribution sums to 1.

Sensor model (signed displacement to a landmark):

Place one landmark at \( x_\ell \). The measurement is the signed wrapped displacement \( z_t \approx \mathrm{wrap}(x_t-x_\ell) \) with noise \( \sigma_z \).

\[ p(z_t \mid x_t) \;=\; \frac{1}{\sqrt{2\pi}\sigma_z} \exp\!\left( -\frac{1}{2}\frac{(z_t - \mathrm{wrap}(x_t-x_\ell))^2}{\sigma_z^2} \right). \]

This is intentionally minimal: it isolates Bayes-filter mechanics. Later chapters will replace the discrete pose grid with continuous-state approximations and richer sensors.

Normalization is not optional. In finite precision, even small underflow/overflow errors accumulate. Always compute

\[ \eta_t = \left(\sum_{x_t\in\mathcal{X}} p(z_t \mid x_t)\overline{bel}_t(x_t)\right)^{-1} \quad\text{and set}\quad bel_t(x_t) = \eta_t\,p(z_t \mid x_t)\overline{bel}_t(x_t). \]

Non-negativity: Probability mass must satisfy \( bel_t(x)\ge 0 \). Clip tiny negative values caused by FFT roundoff (if using FFT) and renormalize.

Complexity:

• Naïve prediction (matrix multiply / double loop): \( \mathcal{O}(N^2) \).
• If the transition depends only on offset (shift-invariant on a ring), prediction becomes cyclic convolution: using FFT gives \( \mathcal{O}(N\log N) \).

Diagnostic quantities: Track entropy as a compact summary of concentration:

\[ H(bel_t) \;=\; -\sum_{x\in\mathcal{X}} bel_t(x)\,\log\!\left(bel_t(x) + \epsilon\right), \quad \epsilon > 0. \]

Entropy should decrease when measurements are informative and increase when motion noise dominates.

This implementation uses numpy and an FFT-based cyclic convolution for the prediction step. In robotics practice, Python Bayes-filter concepts appear inside ROS localization stacks (e.g., in AMCL’s discrete logic), but here we implement the loop directly.

Chapter6_Lesson5.py

#!/usr/bin/env python3
# Chapter6_Lesson5.py
# Lab: Build a Basic Bayes Filter Loop (discrete 1D cyclic state)

import math
import numpy as np

def wrap_interval(x, half_period):
    """Wrap x into [-half_period, +half_period)."""
    period = 2.0 * half_period
    return (x + half_period) % period - half_period

def gaussian_pdf(x, mu, sigma):
    return (1.0 / (math.sqrt(2.0 * math.pi) * sigma)) * math.exp(-0.5 * ((x - mu) / sigma) ** 2)

def build_cyclic_motion_kernel(N, dx, delta_m, sigma_m):
    """Return kernel k over cyclic index offsets so that prediction is cyclic convolution."""
    mu_cells = delta_m / dx
    sigma_cells = sigma_m / dx

    # offsets in [-N/2, N/2)
    k = np.zeros(N, dtype=float)
    half = N // 2
    for idx in range(N):
        d = idx
        if idx >= half:
            d = idx - N  # negative offset
        k[idx] = math.exp(-0.5 * ((d - mu_cells) / sigma_cells) ** 2)

    k /= np.sum(k)
    return k

def predict_cyclic_fft(bel_prev, kernel):
    """Cyclic convolution using FFT: bel_bar = bel_prev (*) kernel."""
    B = np.fft.fft(bel_prev)
    K = np.fft.fft(kernel)
    bel_bar = np.real(np.fft.ifft(B * K))
    bel_bar = np.maximum(bel_bar, 0.0)
    bel_bar /= np.sum(bel_bar)
    return bel_bar

def likelihood_vector(N, dx, landmark_x, z_meas, sigma_z, half_period):
    xs = np.arange(N) * dx
    # signed displacement to landmark in [-L/2, L/2)
    d = np.array([wrap_interval(x - landmark_x, half_period) for x in xs])
    l = np.array([gaussian_pdf(z_meas, di, sigma_z) for di in d])
    l = np.maximum(l, 1e-300)
    return l

def bayes_filter_step(bel_prev, u_delta_m, kernel, z_meas, sigma_z, landmark_x, dx, half_period):
    bel_bar = predict_cyclic_fft(bel_prev, kernel)
    l = likelihood_vector(len(bel_prev), dx, landmark_x, z_meas, sigma_z, half_period)
    bel = bel_bar * l
    bel /= np.sum(bel)
    return bel, bel_bar

def run_demo():
    # Discretization
    L = 10.0       # [m] track length (cyclic)
    N = 200        # number of grid cells
    dx = L / N
    half_period = L / 2.0

    # Models
    u_delta_m = 0.35      # commanded translation per step [m]
    sigma_m   = 0.12      # motion noise std [m]
    sigma_z   = 0.20      # sensor noise std [m]
    landmark_x = 2.0      # landmark position [m] on the ring

    # Build motion kernel once (time-invariant for fixed u)
    kernel = build_cyclic_motion_kernel(N, dx, u_delta_m, sigma_m)

    # Initial belief: uniform
    bel = np.ones(N, dtype=float) / N

    # Ground truth (for simulation only)
    rng = np.random.default_rng(7)
    x_true = 7.0

    T = 25
    print("t, x_true[m], x_hat_MAP[m], z_meas[m]")
    for t in range(1, T + 1):
        # Simulate motion
        x_true = (x_true + u_delta_m + rng.normal(0.0, sigma_m)) % L

        # Simulate sensor: signed displacement to landmark
        z_true = wrap_interval(x_true - landmark_x, half_period)
        z_meas = wrap_interval(z_true + rng.normal(0.0, sigma_z), half_period)

        # Bayes filter update
        bel, bel_bar = bayes_filter_step(
            bel_prev=bel,
            u_delta_m=u_delta_m,
            kernel=kernel,
            z_meas=z_meas,
            sigma_z=sigma_z,
            landmark_x=landmark_x,
            dx=dx,
            half_period=half_period
        )

        idx_hat = int(np.argmax(bel))
        x_hat = idx_hat * dx
        print(f"{t:2d}, {x_true:7.3f}, {x_hat:10.3f}, {z_meas:7.3f}")

    # Optional: report posterior entropy as a sanity check
    eps = 1e-12
    H = -np.sum(bel * np.log(bel + eps))
    print(f"Posterior entropy (nats): {H:.4f}")

if __name__ == "__main__":
    run_demo()

Chapter6_Lesson5_Ex1.py

#!/usr/bin/env python3
# Chapter6_Lesson5_Ex1.py
# Exercise: verify normalization and compare FFT convolution to direct O(N^2) prediction

import numpy as np

def predict_direct(bel_prev, kernel):
    N = len(bel_prev)
    bel_bar = np.zeros(N, dtype=float)
    for j in range(N):
        s = 0.0
        for i in range(N):
            # kernel offset (j - i) mod N
            k = (j - i) % N
            s += kernel[k] * bel_prev[i]
        bel_bar[j] = s
    bel_bar = np.maximum(bel_bar, 0.0)
    bel_bar /= np.sum(bel_bar)
    return bel_bar

def predict_fft(bel_prev, kernel):
    B = np.fft.fft(bel_prev)
    K = np.fft.fft(kernel)
    bel_bar = np.real(np.fft.ifft(B * K))
    bel_bar = np.maximum(bel_bar, 0.0)
    bel_bar /= np.sum(bel_bar)
    return bel_bar

def main():
    rng = np.random.default_rng(0)
    N = 128
    bel = rng.random(N)
    bel /= np.sum(bel)

    # random cyclic kernel
    k = rng.random(N)
    k /= np.sum(k)

    d = predict_direct(bel, k)
    f = predict_fft(bel, k)

    print("sum(d) =", float(np.sum(d)))
    print("sum(f) =", float(np.sum(f)))
    print("max|d-f| =", float(np.max(np.abs(d - f))))

if __name__ == "__main__":
    main()

This C++ version uses Eigen for vector storage and implements the prediction step with a direct double loop (useful for clarity and debugging). In production AMR stacks, C++ Bayes filters commonly appear inside ROS 2 nodes (rclcpp) and localization packages; here we keep it stand-alone.

Chapter6_Lesson5.cpp

// Chapter6_Lesson5.cpp
// Lab: Build a Basic Bayes Filter Loop (discrete 1D cyclic state)
//
// Dependencies: Eigen (header-only). Compile example (Linux/macOS):
//   g++ -O2 -std=c++17 Chapter6_Lesson5.cpp -I /usr/include/eigen3 -o bayes_loop
//
// This file implements:
//   bel_bar = T(u) * bel
//   bel     = normalize( bel_bar .* l(z) )
//
// where T(u) is a cyclic convolution operator parameterized by a Gaussian kernel.

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

static double wrap_interval(double x, double half_period) {
    double period = 2.0 * half_period;
    double y = std::fmod(x + half_period, period);
    if (y < 0.0) y += period;
    return y - half_period;
}

static double gaussian_pdf(double x, double mu, double sigma) {
    const double c = 1.0 / (std::sqrt(2.0 * M_PI) * sigma);
    const double z = (x - mu) / sigma;
    return c * std::exp(-0.5 * z * z);
}

static Eigen::VectorXd build_cyclic_kernel(int N, double dx, double delta_m, double sigma_m) {
    double mu_cells = delta_m / dx;
    double sigma_cells = sigma_m / dx;

    Eigen::VectorXd k(N);
    int half = N / 2;
    for (int idx = 0; idx < N; ++idx) {
        int d = idx;
        if (idx >= half) d = idx - N;
        double v = std::exp(-0.5 * std::pow((d - mu_cells) / sigma_cells, 2.0));
        k(idx) = v;
    }
    k /= k.sum();
    return k;
}

static Eigen::VectorXd predict_cyclic_direct(const Eigen::VectorXd& bel_prev, const Eigen::VectorXd& kernel) {
    int N = static_cast<int>(bel_prev.size());
    Eigen::VectorXd bel_bar(N);
    bel_bar.setZero();

    for (int j = 0; j < N; ++j) {
        double s = 0.0;
        for (int i = 0; i < N; ++i) {
            int k = (j - i) % N;
            if (k < 0) k += N;
            s += kernel(k) * bel_prev(i);
        }
        bel_bar(j) = s;
    }

    // numerical hygiene
    for (int j = 0; j < N; ++j) bel_bar(j) = std::max(0.0, bel_bar(j));
    bel_bar /= bel_bar.sum();
    return bel_bar;
}

static Eigen::VectorXd likelihood_vector(int N, double dx, double landmark_x, double z_meas, double sigma_z, double half_period) {
    Eigen::VectorXd l(N);
    for (int i = 0; i < N; ++i) {
        double x = i * dx;
        double d = wrap_interval(x - landmark_x, half_period);
        l(i) = std::max(gaussian_pdf(z_meas, d, sigma_z), 1e-300);
    }
    return l;
}

static Eigen::VectorXd bayes_step(const Eigen::VectorXd& bel_prev,
                                 const Eigen::VectorXd& kernel,
                                 double z_meas, double sigma_z,
                                 double landmark_x, double dx, double half_period) {
    Eigen::VectorXd bel_bar = predict_cyclic_direct(bel_prev, kernel);
    Eigen::VectorXd l = likelihood_vector(static_cast<int>(bel_prev.size()), dx, landmark_x, z_meas, sigma_z, half_period);
    Eigen::VectorXd bel = bel_bar.array() * l.array();
    bel /= bel.sum();
    return bel;
}

int main() {
    // Discretization
    const double L = 10.0;
    const int    N = 200;
    const double dx = L / N;
    const double half_period = L / 2.0;

    // Models
    const double u_delta_m = 0.35;
    const double sigma_m   = 0.12;
    const double sigma_z   = 0.20;
    const double landmark_x = 2.0;

    Eigen::VectorXd kernel = build_cyclic_kernel(N, dx, u_delta_m, sigma_m);

    // Initial belief: uniform
    Eigen::VectorXd bel = Eigen::VectorXd::Ones(N) / static_cast<double>(N);

    // RNG
    std::mt19937 gen(7);
    std::normal_distribution<double> n_m(0.0, sigma_m);
    std::normal_distribution<double> n_z(0.0, sigma_z);

    double x_true = 7.0;

    const int T = 25;
    std::cout << "t, x_true[m], x_hat_MAP[m], z_meas[m]\n";
    for (int t = 1; t <= T; ++t) {
        x_true = std::fmod(x_true + u_delta_m + n_m(gen), L);
        if (x_true < 0.0) x_true += L;

        double z_true = wrap_interval(x_true - landmark_x, half_period);
        double z_meas = wrap_interval(z_true + n_z(gen), half_period);

        bel = bayes_step(bel, kernel, z_meas, sigma_z, landmark_x, dx, half_period);

        Eigen::Index idx_hat = 0;
        bel.maxCoeff(&idx_hat);
        double x_hat = static_cast<double>(idx_hat) * dx;

        std::cout << t << ", " << x_true << ", " << x_hat << ", " << z_meas << "\n";
    }

    return 0;
}

Java is less common in core AMR localization than C++/Python, but it appears in Android robotics, educational stacks, and some ROSJava ecosystems. This version mirrors the same discrete Bayes loop with simple arrays.

Chapter6_Lesson5.java

// Chapter6_Lesson5.java
// Lab: Build a Basic Bayes Filter Loop (discrete 1D cyclic state)
//
// Compile:
//   javac Chapter6_Lesson5.java
// Run:
//   java Chapter6_Lesson5
//
// This implementation avoids external dependencies; for linear algebra in robotics
// you may consider EJML, Apache Commons Math, or ROSJava bindings.

import java.util.Random;

public class Chapter6_Lesson5 {

    static double wrapInterval(double x, double halfPeriod) {
        double period = 2.0 * halfPeriod;
        double y = (x + halfPeriod) % period;
        if (y < 0.0) y += period;
        return y - halfPeriod;
    }

    static double gaussianPdf(double x, double mu, double sigma) {
        double c = 1.0 / (Math.sqrt(2.0 * Math.PI) * sigma);
        double z = (x - mu) / sigma;
        return c * Math.exp(-0.5 * z * z);
    }

    static double[] buildCyclicKernel(int N, double dx, double deltaM, double sigmaM) {
        double muCells = deltaM / dx;
        double sigmaCells = sigmaM / dx;

        double[] k = new double[N];
        int half = N / 2;
        double sum = 0.0;
        for (int idx = 0; idx < N; idx++) {
            int d = idx;
            if (idx >= half) d = idx - N;
            double v = Math.exp(-0.5 * Math.pow((d - muCells) / sigmaCells, 2.0));
            k[idx] = v;
            sum += v;
        }
        for (int i = 0; i < N; i++) k[i] /= sum;
        return k;
    }

    static double[] predictCyclicDirect(double[] belPrev, double[] kernel) {
        int N = belPrev.length;
        double[] belBar = new double[N];

        for (int j = 0; j < N; j++) {
            double s = 0.0;
            for (int i = 0; i < N; i++) {
                int k = (j - i) % N;
                if (k < 0) k += N;
                s += kernel[k] * belPrev[i];
            }
            belBar[j] = Math.max(0.0, s);
        }

        // normalize
        double sum = 0.0;
        for (double v : belBar) sum += v;
        for (int j = 0; j < N; j++) belBar[j] /= sum;

        return belBar;
    }

    static double[] likelihoodVector(int N, double dx, double landmarkX, double zMeas, double sigmaZ, double halfPeriod) {
        double[] l = new double[N];
        for (int i = 0; i < N; i++) {
            double x = i * dx;
            double d = wrapInterval(x - landmarkX, halfPeriod);
            double v = gaussianPdf(zMeas, d, sigmaZ);
            l[i] = Math.max(v, 1e-300);
        }
        return l;
    }

    static double[] bayesStep(double[] belPrev, double[] kernel,
                             double zMeas, double sigmaZ,
                             double landmarkX, double dx, double halfPeriod) {

        double[] belBar = predictCyclicDirect(belPrev, kernel);
        double[] l = likelihoodVector(belPrev.length, dx, landmarkX, zMeas, sigmaZ, halfPeriod);

        double[] bel = new double[belPrev.length];
        double sum = 0.0;
        for (int i = 0; i < bel.length; i++) {
            bel[i] = belBar[i] * l[i];
            sum += bel[i];
        }
        for (int i = 0; i < bel.length; i++) bel[i] /= sum;
        return bel;
    }

    static int argmax(double[] a) {
        int idx = 0;
        double best = a[0];
        for (int i = 1; i < a.length; i++) {
            if (a[i] > best) {
                best = a[i];
                idx = i;
            }
        }
        return idx;
    }

    public static void main(String[] args) {
        // Discretization
        double L = 10.0;
        int N = 200;
        double dx = L / N;
        double halfPeriod = L / 2.0;

        // Models
        double uDeltaM = 0.35;
        double sigmaM  = 0.12;
        double sigmaZ  = 0.20;
        double landmarkX = 2.0;

        double[] kernel = buildCyclicKernel(N, dx, uDeltaM, sigmaM);

        // Initial belief: uniform
        double[] bel = new double[N];
        for (int i = 0; i < N; i++) bel[i] = 1.0 / N;

        Random rng = new Random(7);
        double xTrue = 7.0;

        int T = 25;
        System.out.println("t, x_true[m], x_hat_MAP[m], z_meas[m]");
        for (int t = 1; t <= T; t++) {
            xTrue = (xTrue + uDeltaM + rng.nextGaussian() * sigmaM) % L;
            if (xTrue < 0.0) xTrue += L;

            double zTrue = wrapInterval(xTrue - landmarkX, halfPeriod);
            double zMeas = wrapInterval(zTrue + rng.nextGaussian() * sigmaZ, halfPeriod);

            bel = bayesStep(bel, kernel, zMeas, sigmaZ, landmarkX, dx, halfPeriod);

            int idxHat = argmax(bel);
            double xHat = idxHat * dx;
            System.out.println(t + ", " + xTrue + ", " + xHat + ", " + zMeas);
        }
    }
}

MATLAB is convenient for rapid prototyping and numerical verification. The script below implements the same cyclic Bayes loop. Then we provide an additional script that programmatically builds a minimal Simulink wiring diagram of the loop (Prediction → Likelihood → Multiply → Normalize).

Chapter6_Lesson5.m

% Chapter6_Lesson5.m
% Lab: Build a Basic Bayes Filter Loop (discrete 1D cyclic state)
%
% This script runs a simple Bayes filter on a 1D cyclic track with:
%   - motion model: cyclic Gaussian translation kernel
%   - sensor model: signed displacement to a landmark + Gaussian noise
%
% Requirements:
%   - MATLAB (no special toolboxes required for the core script)
% Optional:
%   - Robotics System Toolbox for later ROS integration (not required here)

clear; clc;

% Discretization
L = 10.0;          % [m]
N = 200;
dx = L / N;
halfPeriod = L / 2;

% Models
uDeltaM = 0.35;    % [m] commanded translation per step
sigmaM  = 0.12;    % [m]
sigmaZ  = 0.20;    % [m]
landmarkX = 2.0;   % [m]

% Kernel (cyclic offsets in [-N/2, N/2))
muCells = uDeltaM / dx;
sigmaCells = sigmaM / dx;

idx = 0:(N-1);
d = idx;
half = floor(N/2);
d(d >= half) = d(d >= half) - N;     % negative offsets
k = exp(-0.5 * ((d - muCells) / sigmaCells).^2);
k = k / sum(k);

% Initial belief: uniform
bel = ones(N,1) / N;

% Ground truth (simulation only)
rng(7);
xTrue = 7.0;

T = 25;
fprintf("t, x_true[m], x_hat_MAP[m], z_meas[m]\n");

for t = 1:T
    % Motion simulation
    xTrue = mod(xTrue + uDeltaM + sigmaM * randn(), L);

    % Sensor simulation: signed displacement in [-L/2, L/2)
    zTrue = wrapInterval(xTrue - landmarkX, halfPeriod);
    zMeas = wrapInterval(zTrue + sigmaZ * randn(), halfPeriod);

    % Prediction: cyclic convolution via FFT
    belBar = real(ifft(fft(bel) .* fft(k(:))));
    belBar = max(belBar, 0);
    belBar = belBar / sum(belBar);

    % Likelihood
    xs = (0:(N-1))' * dx;
    dispToLm = arrayfun(@(x) wrapInterval(x - landmarkX, halfPeriod), xs);
    l = normpdf(zMeas, dispToLm, sigmaZ);
    l = max(l, 1e-300);

    % Update
    bel = belBar .* l;
    bel = bel / sum(bel);

    % MAP estimate
    [~, idxHat] = max(bel);
    xHat = (idxHat - 1) * dx;

    fprintf("%2d, %7.3f, %10.3f, %7.3f\n", t, xTrue, xHat, zMeas);
end

% Posterior entropy (sanity check)
eps = 1e-12;
H = -sum(bel .* log(bel + eps));
fprintf("Posterior entropy (nats): %.4f\n", H);

% --- Local function(s)
function y = wrapInterval(x, halfPeriod)
    % Wrap x into [-halfPeriod, +halfPeriod)
    period = 2.0 * halfPeriod;
    y = mod(x + halfPeriod, period) - halfPeriod;
end

Chapter6_Lesson5_SimulinkBuild.m

% Chapter6_Lesson5_SimulinkBuild.m
% Programmatically build a minimal Simulink diagram for the Bayes filter loop.
% Output model: Chapter6_Lesson5_BayesLoop.slx
%
% Note: This is a pedagogical wiring diagram (not a full estimator blockset).
% It shows the loop structure: Prediction -> Likelihood -> Product -> Normalize.
%
% Run in MATLAB with Simulink installed.

function Chapter6_Lesson5_SimulinkBuild()

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

% Layout helpers
x0 = 30; y0 = 50; dx = 160; dy = 80;

% Sources
add_block('simulink/Sources/Constant', [model '/bel_prev'], 'Value', 'bel0', ...
    'Position', [x0 y0 x0+80 y0+30]);

add_block('simulink/Sources/Constant', [model '/kernel'], 'Value', 'k', ...
    'Position', [x0 y0+dy x0+80 y0+dy+30]);

add_block('simulink/Sources/Constant', [model '/likelihood'], 'Value', 'l', ...
    'Position', [x0 y0+2*dy x0+80 y0+2*dy+30]);

% Prediction (placeholder): bel_bar = Conv(bel_prev, kernel)
add_block('simulink/Math Operations/Product', [model '/ConvPlaceholder'], ...
    'Position', [x0+dx y0+dy/2 x0+dx+80 y0+dy/2+40]);
set_param([model '/ConvPlaceholder'], 'Inputs', '**'); % used as placeholder

% Update: multiply bel_bar .* likelihood
add_block('simulink/Math Operations/Product', [model '/Multiply'], ...
    'Position', [x0+2*dx y0+dy x0+2*dx+80 y0+dy+40]);
set_param([model '/Multiply'], 'Inputs', '.*'); % elementwise

% Normalize: bel = bel_unnorm / sum(bel_unnorm)
add_block('simulink/Math Operations/Sum', [model '/SumBel'], ...
    'Position', [x0+3*dx y0+dy-10 x0+3*dx+60 y0+dy+10]);
set_param([model '/SumBel'], 'Inputs', '+');

add_block('simulink/Math Operations/Divide', [model '/Normalize'], ...
    'Position', [x0+3*dx y0+dy+30 x0+3*dx+80 y0+dy+70]);

% Sink
add_block('simulink/Sinks/Out1', [model '/bel'], ...
    'Position', [x0+4*dx y0+dy+40 x0+4*dx+60 y0+dy+60]);

% Connections
add_line(model, 'bel_prev/1', 'ConvPlaceholder/1');
add_line(model, 'kernel/1',   'ConvPlaceholder/2');

add_line(model, 'ConvPlaceholder/1', 'Multiply/1');
add_line(model, 'likelihood/1',      'Multiply/2');

add_line(model, 'Multiply/1', 'SumBel/1');
add_line(model, 'Multiply/1', 'Normalize/1');
add_line(model, 'SumBel/1',   'Normalize/2');

add_line(model, 'Normalize/1', 'bel/1');

% Clean
set_param(model, 'StopTime', '1');
save_system(model);
disp(['Saved model: ' model '.slx']);

end

Mathematica is useful for symbolic checks and concise numerical prototyping. The notebook file is provided as plain Wolfram Language text.

Chapter6_Lesson5.nb

(* Chapter6_Lesson5.nb
   Lab: Build a Basic Bayes Filter Loop (discrete 1D cyclic state)

   This notebook is stored as plain Wolfram Language so it can be version-controlled.
   You can open it in Mathematica (it will appear as an input cell script).
*)

ClearAll["Global`*"];

wrapInterval[x_, half_] := Module[{period = 2 half},
  Mod[x + half, period] - half
];

gaussianPDF[x_, mu_, sigma_] := (1/(Sqrt[2 Pi] sigma)) Exp[-1/2 ((x - mu)/sigma)^2];

buildCyclicKernel[N_, dx_, deltaM_, sigmaM_] := Module[
  {muCells = deltaM/dx, sigmaCells = sigmaM/dx, half = Floor[N/2], d, k},
  d = Range[0, N - 1];
  d = Map[If[# >= half, # - N, #] &, d];
  k = Exp[-1/2 ((d - muCells)/sigmaCells)^2];
  k/Total[k]
];

predictCyclicFFT[belPrev_, kernel_] := Module[
  {belBar},
  belBar = Re[InverseFourier[Fourier[belPrev] Fourier[kernel]]];
  belBar = Map[Max[#, 0] &, belBar];
  belBar/Total[belBar]
];

likelihoodVector[N_, dx_, landmarkX_, zMeas_, sigmaZ_, halfPeriod_] := Module[
  {xs, dispToLm, l},
  xs = Range[0, N - 1] dx;
  dispToLm = wrapInterval[#, halfPeriod] & /@ (xs - landmarkX);
  l = gaussianPDF[zMeas, #, sigmaZ] & /@ dispToLm;
  Map[Max[#, 10^-300] &, l]
];

bayesStep[belPrev_, kernel_, zMeas_, sigmaZ_, landmarkX_, dx_, halfPeriod_] := Module[
  {belBar, l, bel},
  belBar = predictCyclicFFT[belPrev, kernel];
  l = likelihoodVector[Length[belPrev], dx, landmarkX, zMeas, sigmaZ, halfPeriod];
  bel = belBar * l;
  bel/Total[bel]
];

(* Demo *)
L = 10.0; N = 200; dx = L/N; halfPeriod = L/2;
uDeltaM = 0.35; sigmaM = 0.12; sigmaZ = 0.20; landmarkX = 2.0;

kernel = buildCyclicKernel[N, dx, uDeltaM, sigmaM];
bel = ConstantArray[1.0/N, N];

SeedRandom[7];
xTrue = 7.0;

Print["t, x_true[m], x_hat_MAP[m], z_meas[m]"];
Do[
  xTrue = Mod[xTrue + uDeltaM + RandomVariate[NormalDistribution[0, sigmaM]], L];
  zTrue = wrapInterval[xTrue - landmarkX, halfPeriod];
  zMeas = wrapInterval[zTrue + RandomVariate[NormalDistribution[0, sigmaZ]], halfPeriod];

  bel = bayesStep[bel, kernel, zMeas, sigmaZ, landmarkX, dx, halfPeriod];
  idxHat = First@Ordering[bel, -1];
  xHat = (idxHat - 1) dx;

  Print[t, ", ", NumberForm[xTrue, {Infinity, 3}], ", ", NumberForm[xHat, {Infinity, 3}], ", ", NumberForm[zMeas, {Infinity, 3}]];
, {t, 1, 25}];

H = -Total[bel Log[bel + 10^-12]];
Print["Posterior entropy (nats): ", NumberForm[H, {Infinity, 4}]];

Problem 1 (Derivation drill): Starting from \( bel_t(x_t)=p(x_t \mid z_{1:t},u_{1:t}) \), derive the two-step recursion (prediction and correction) using (i) the law of total probability and (ii) the Markov and conditional-independence assumptions.

Solution: The prediction step follows Proposition 1 by marginalizing \( x_{t-1} \) and applying the Markov property. The correction step follows Proposition 2 by Bayes’ rule and conditional independence:

\[ \overline{bel}_t(x_t)=\sum_{x_{t-1}} p(x_t \mid u_t,x_{t-1})\,bel_{t-1}(x_{t-1}), \quad bel_t(x_t)=\eta_t\,p(z_t \mid x_t)\,\overline{bel}_t(x_t). \]

Problem 2 (Normalization constant): Prove that \( \eta_t^{-1} = p(z_t \mid z_{1:t-1},u_{1:t}) \) equals the evidence obtained by summing over states.

Solution: From Bayes’ rule (Proposition 2), sum over \( x_t \):

\[ 1=\sum_{x_t} bel_t(x_t) =\eta_t \sum_{x_t} p(z_t \mid x_t)\overline{bel}_t(x_t) \;\;\Rightarrow\;\; \eta_t^{-1}=\sum_{x_t} p(z_t \mid x_t)\overline{bel}_t(x_t). \]

But also by definition, \( p(z_t \mid z_{1:t-1},u_{1:t})=\sum_{x_t} p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t}) \), and \( p(x_t \mid z_{1:t-1},u_{1:t})=\overline{bel}_t(x_t) \), proving equality.

Problem 3 (Mass conservation): Let \( \mathbf{T} \) be column-stochastic and \( \mathbf{b} \) a distribution vector. Prove \( \overline{\mathbf{b}}=\mathbf{T}\mathbf{b} \) is also a distribution vector.

Solution: Non-negativity holds because products of non-negative terms are non-negative. For total mass:

\[ \mathbf{1}^\top \overline{\mathbf{b}} = \mathbf{1}^\top \mathbf{T}\mathbf{b} = \left(\mathbf{1}^\top \mathbf{T}\right)\mathbf{b}. \]

Column-stochasticity implies \( \mathbf{1}^\top \mathbf{T}=\mathbf{1}^\top \), hence \( \mathbf{1}^\top \overline{\mathbf{b}}=\mathbf{1}^\top \mathbf{b}=1 \).

Problem 4 (Complexity comparison): For a ring grid of size \( N \), compare the prediction-step complexity of: (i) the direct double loop; (ii) FFT-based cyclic convolution. State when FFT is beneficial.

Solution: Direct computation evaluates \( N^2 \) products, so \( \mathcal{O}(N^2) \). FFT convolution requires two FFTs and one inverse FFT (plus elementwise products), giving \( \mathcal{O}(N\log N) \). FFT becomes beneficial when \( N \) is large enough that \( N\log N \) is far smaller than \( N^2 \) and when the transition is shift-invariant (depends only on offset).

Problem 5 (One-step hand calculation): Consider \( \mathcal{X}=\{0,1,2\} \) and prior \( \mathbf{b}_{t-1}=[0.2,0.5,0.3]^\top \). Let the transition matrix be

\[ \mathbf{T}= \begin{bmatrix} 0.7 & 0.2 & 0.1 \\ 0.2 & 0.6 & 0.2 \\ 0.1 & 0.2 & 0.7 \end{bmatrix} \quad (\text{each column sums to }1), \quad \boldsymbol{\ell}=[0.1, 1.0, 0.4]^\top. \]

Compute \( \overline{\mathbf{b}}_t \) and \( \mathbf{b}_t \).

Solution:

Prediction: \( \overline{\mathbf{b}}_t=\mathbf{T}\mathbf{b}_{t-1} \).

\[ \overline{\mathbf{b}}_t = \begin{bmatrix} 0.7 & 0.2 & 0.1 \\ 0.2 & 0.6 & 0.2 \\ 0.1 & 0.2 & 0.7 \end{bmatrix} \begin{bmatrix} 0.2\\0.5\\0.3 \end{bmatrix} = \begin{bmatrix} 0.7(0.2)+0.2(0.5)+0.1(0.3)\\ 0.2(0.2)+0.6(0.5)+0.2(0.3)\\ 0.1(0.2)+0.2(0.5)+0.7(0.3) \end{bmatrix} = \begin{bmatrix} 0.27\\ 0.40\\ 0.33 \end{bmatrix}. \]

Correction: \( \mathbf{b}_t \propto \boldsymbol{\ell}\odot \overline{\mathbf{b}}_t \).

\[ \tilde{\mathbf{b}}_t = \begin{bmatrix}0.1\\1.0\\0.4\end{bmatrix} \odot \begin{bmatrix}0.27\\0.40\\0.33\end{bmatrix} = \begin{bmatrix}0.027\\0.40\\0.132\end{bmatrix}, \quad \eta_t^{-1} = 0.027+0.40+0.132 = 0.559, \\ \mathbf{b}_t = \frac{1}{0.559} \begin{bmatrix}0.027\\0.40\\0.132\end{bmatrix} \]

Numerically, \( \mathbf{b}_t \approx [0.0483,\;0.7156,\;0.2361]^\top \).

You implemented the full discrete Bayes filter loop: prediction via a stochastic transition operator (specializing to cyclic convolution when shift-invariant), correction via a measurement likelihood, and explicit normalization. You also validated core invariants (non-negativity, unit mass) and extracted a MAP estimate. This implementation becomes the conceptual baseline for later localization methods that change representation (continuous state, Gaussian approximations, particles), while preserving the same Bayesian recursion structure.