From Lesson 1–2, the robot’s belief at time \( t \) is a distribution \( \operatorname{bel}(x_t) \) over its state \( x_t \). The Bayes filter decomposes estimation into two steps:

• Prediction (motion model): propagate \( \operatorname{bel}(x_{t-1}) \) using control \( u_t \) and a probabilistic dynamics model.
• Correction (sensor model): reweight the predicted belief using the measurement \( z_t \).

The motion model is the conditional density \( p(x_t \mid u_t, x_{t-1}) \). The sensor model is the conditional likelihood \( p(z_t \mid x_t) \). Their combination yields the core recursion:

\[ \operatorname{bel}(x_t) = \eta \, p(z_t \mid x_t) \int p(x_t \mid u_t, x_{t-1}) \, \operatorname{bel}(x_{t-1}) \, dx_{t-1}, \quad \eta^{-1} = \int p(z_t \mid x_t)\,\overline{\operatorname{bel}}(x_t)\,dx_t \]

flowchart TD
  B0["belief at t-1: bel(x_{t-1})"] --> P["Prediction using motion model p(x_t | u_t, x_{t-1})"]
  U["control u_t"] --> P
  P --> BB["prior at t: bel_bar(x_t)"]
  Z["measurement z_t"] --> C["Correction using sensor model p(z_t | x_t)"]
  BB --> C
  C --> B1["posterior at t: bel(x_t) (normalized)"]
        

Intuitively: the motion model spreads probability mass (uncertainty grows under actuation), while the sensor model concentrates mass around states that explain the measurement (uncertainty shrinks when observations are informative).

Let the state be \( x_t \in \mathcal{X} \) (for planar mobile robots, often \( x_t = [x, y, \theta]^\top \)). Let the control be \( u_t \) (wheel increments, velocity commands, IMU-derived increments, etc.). Let the measurement be \( z_t \in \mathcal{Z} \).

The Bayes-filter assumptions (from Lesson 2) can be stated as:

\[ \text{(Markov motion)} \quad p(x_t \mid x_{0:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t) \]

\[ \text{(Markov sensing)} \quad p(z_t \mid x_{0:t}, u_{1:t}, z_{1:t-1}) = p(z_t \mid x_t) \]

Proposition (Bayes-filter recursion). Under the two Markov properties above, the posterior belief satisfies:

\[ \operatorname{bel}(x_t) = \eta \, p(z_t \mid x_t) \int p(x_t \mid u_t, x_{t-1}) \operatorname{bel}(x_{t-1}) \, dx_{t-1}. \]

Proof. Start from the definition of belief:

\[ \operatorname{bel}(x_t) \equiv p(x_t \mid z_{1:t}, u_{1:t}). \]

Apply Bayes’ rule (with respect to the newest measurement \( z_t \)):

\[ p(x_t \mid z_{1:t}, u_{1:t}) = \frac{p(z_t \mid x_t, z_{1:t-1}, u_{1:t}) \, p(x_t \mid z_{1:t-1}, u_{1:t})} {p(z_t \mid z_{1:t-1}, u_{1:t})}. \]

By Markov sensing, \( p(z_t \mid x_t, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t) \). The denominator is a normalization constant \( \eta^{-1} \). Hence:

\[ \operatorname{bel}(x_t) = \eta \, p(z_t \mid x_t)\, p(x_t \mid z_{1:t-1}, u_{1:t}). \]

Now expand the predictive term by marginalizing over \( x_{t-1} \) (law of total probability):

\[ p(x_t \mid z_{1:t-1}, u_{1:t}) = \int p(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t}) \, p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) \, dx_{t-1}. \]

By Markov motion, \( p(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t) \). Also, conditioning on controls up to \( t \) does not change the posterior at \( t-1 \) beyond what is already in \( u_{1:t-1} \), so: \( p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) = p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}) = \operatorname{bel}(x_{t-1}) \). Substituting yields the stated recursion. \(\square\)

A common construction starts from deterministic kinematics (Course 2 knowledge) and injects process noise:

\[ x_t = g(x_{t-1}, u_t) + w_t, \quad w_t \sim \mathcal{N}(0, Q_t). \]

This implies the conditional density:

\[ p(x_t \mid x_{t-1}, u_t) = \mathcal{N}\!\big(x_t; g(x_{t-1}, u_t), Q_t\big). \]

Normalization proof (Gaussian motion model). For any fixed \( x_{t-1}, u_t \), the density integrates to one:

\[ \int p(x_t \mid x_{t-1}, u_t)\, dx_t = \int \mathcal{N}\!\big(x_t; g(x_{t-1}, u_t), Q_t\big)\, dx_t = 1. \]

This holds because a multivariate Gaussian is a properly normalized density on \( \mathbb{R}^n \). If you want to verify by change of variables, define \( y = x_t - g(x_{t-1},u_t) \). The Jacobian determinant is \( |\partial y / \partial x_t| = 1 \), hence the integral equals the integral of \( \mathcal{N}(y;0,Q_t) \), which is 1.

Mobile-robot odometry motion model (differential-drive framing). A widely used model constructs the relative motion as three increments: \( \delta_{\mathrm{rot1}}, \delta_{\mathrm{trans}}, \delta_{\mathrm{rot2}} \) (initial rotation, translation, final rotation). Let \( x_{t-1}=(x,y,\theta) \). The noiseless update is:

\[ \begin{aligned} x' &= x + \delta_{\mathrm{trans}} \cos(\theta + \delta_{\mathrm{rot1}}), \\ y' &= y + \delta_{\mathrm{trans}} \sin(\theta + \delta_{\mathrm{rot1}}), \\ \theta' &= \theta + \delta_{\mathrm{rot1}} + \delta_{\mathrm{rot2}}. \end{aligned} \]

A probabilistic version adds zero-mean noise whose variance increases with motion magnitude, e.g., with parameters \( \alpha_1,\dots,\alpha_4 \):

\[ \begin{aligned} \hat{\delta}_{\mathrm{rot1}} &= \delta_{\mathrm{rot1}} + \varepsilon_1,\quad \varepsilon_1 \sim \mathcal{N}\!\big(0,\; \alpha_1 \delta_{\mathrm{rot1}}^2 + \alpha_2 \delta_{\mathrm{trans}}^2\big),\\ \hat{\delta}_{\mathrm{trans}} &= \delta_{\mathrm{trans}} + \varepsilon_2,\quad \varepsilon_2 \sim \mathcal{N}\!\big(0,\; \alpha_3 \delta_{\mathrm{trans}}^2 + \alpha_4(\delta_{\mathrm{rot1}}^2+\delta_{\mathrm{rot2}}^2)\big),\\ \hat{\delta}_{\mathrm{rot2}} &= \delta_{\mathrm{rot2}} + \varepsilon_3,\quad \varepsilon_3 \sim \mathcal{N}\!\big(0,\; \alpha_1 \delta_{\mathrm{rot2}}^2 + \alpha_2 \delta_{\mathrm{trans}}^2\big). \end{aligned} \]

The conditional density \( p(x_t \mid x_{t-1}, u_t) \) is then defined implicitly: you sample \( \hat{\delta}_{\mathrm{rot1}}, \hat{\delta}_{\mathrm{trans}}, \hat{\delta}_{\mathrm{rot2}} \) and apply the kinematic update. This “sampling-defined” model is typical in particle filters (Chapter 8), but the concept is already useful now: motion models are often easier to sample than to evaluate analytically.

Sensor models encode how likely a measurement is, given a hypothetical state. A standard form is:

\[ z_t = h(x_t) + v_t,\quad v_t \sim \mathcal{N}(0, R_t) \quad \Longrightarrow \quad p(z_t \mid x_t) = \mathcal{N}\!\big(z_t; h(x_t), R_t\big). \]

Example: range-to-beacon. Suppose a known beacon at \( m=(m_x,m_y) \) and a range sensor that returns \( z_t \). For pose \( x_t=(x,y,\theta) \), the predicted range is:

\[ \hat{z}(x_t) = \sqrt{(x - m_x)^2 + (y - m_y)^2}. \]

With Gaussian noise \( v_t \sim \mathcal{N}(0,\sigma_z^2) \), the likelihood is:

\[ p(z_t \mid x_t) = \frac{1}{\sqrt{2\pi}\sigma_z} \exp\!\left(-\frac{(z_t - \hat{z}(x_t))^2}{2\sigma_z^2}\right). \]

Robust likelihoods (mixtures). Real sensors exhibit outliers (dropouts, multipath, occlusions). A common modeling strategy is a mixture distribution:

\[ p(z \mid x) = w_{\text{hit}}\,p_{\text{hit}}(z \mid x) + w_{\text{rand}}\,p_{\text{rand}}(z) + w_{\text{max}}\,p_{\text{max}}(z), \quad w_{\text{hit}} + w_{\text{rand}} + w_{\text{max}} = 1,\; w_i \ge 0. \]

Here \( p_{\text{hit}} \) is the physics-based term (often Gaussian), \( p_{\text{rand}} \) models spurious random readings (often uniform on a valid interval), and \( p_{\text{max}} \) models saturation at sensor max range (a point mass or narrow peak). The key takeaway for this lesson: sensor models are typically easier to evaluate than to sample because they are used as likelihood scores in Bayesian correction.

flowchart TD
  A["Choose measurement z_t (range, bearing, scan, feature)"] --> B["Write forward model h(x_t)"]
  B --> C["Select noise family (Gaussian, mixture, heavy-tail)"]
  C --> D["Compute likelihood score p(z_t | x_t)"]
  D --> E["Use in correction: bel(x_t) proportional to p(z_t | x_t)*bel_bar(x_t)"]
        

In a discrete state space (e.g., 1D grid cells), define \( \operatorname{bel}_t[i] \approx \operatorname{bel}(x_t=i) \). The Bayes filter becomes:

\[ \overline{\operatorname{bel}}_t[i] = \sum_{j} p(i \mid u_t, j)\, \operatorname{bel}_{t-1}[j], \quad \operatorname{bel}_t[i] = \eta\, p(z_t \mid i)\, \overline{\operatorname{bel}}_t[i]. \]

The prediction is a linear smoothing operator (a Markov transition acting on a distribution), while the correction is a pointwise multiplicative reweighting followed by normalization.

Proposition (prediction is convolution under translation-invariant motion). Suppose the transition is spatially homogeneous: \( p(i \mid u, j) = k(i - j - u) \) for some kernel \( k \). Then prediction is a discrete convolution:

\[ \overline{\operatorname{bel}}_t[i] = \sum_j k(i - j - u)\, \operatorname{bel}_{t-1}[j] = (k_u * \operatorname{bel}_{t-1})[i], \quad k_u[n] \equiv k(n-u). \]

Proof. Substitute the homogeneous form into the prediction equation and identify the convolution sum definition. \(\square\)

This “convolution view” is not merely cosmetic: it clarifies that motion uncertainty acts like a blur kernel, increasing entropy, while the sensor likelihood acts like a spatial mask, decreasing entropy when informative.

Even before Kalman filtering (Chapter 7), it is essential to understand the algebra that makes Gaussian models tractable. Consider a 1D state \( x \in \mathbb{R} \). Suppose the predicted belief is Gaussian: \( \overline{\operatorname{bel}}(x) = \mathcal{N}(x;\mu_b,\sigma_b^2) \), and the sensor provides a direct noisy observation of position: \( z = x + v,\; v \sim \mathcal{N}(0,\sigma_z^2) \). Then \( p(z \mid x) = \mathcal{N}(z; x, \sigma_z^2) \).

The posterior is proportional to the product: \( \operatorname{bel}(x) \propto p(z \mid x)\overline{\operatorname{bel}}(x) \).

\[ \operatorname{bel}(x) \propto \exp\!\left(-\frac{(x-\mu_b)^2}{2\sigma_b^2}\right)\, \exp\!\left(-\frac{(z-x)^2}{2\sigma_z^2}\right) = \exp\!\left(-\frac{1}{2}\left[\frac{(x-\mu_b)^2}{\sigma_b^2} + \frac{(x-z)^2}{\sigma_z^2}\right]\right). \]

Expand and collect terms in \( x \):

\[ \frac{(x-\mu_b)^2}{\sigma_b^2} + \frac{(x-z)^2}{\sigma_z^2} = x^2\left(\frac{1}{\sigma_b^2}+\frac{1}{\sigma_z^2}\right) - 2x\left(\frac{\mu_b}{\sigma_b^2}+\frac{z}{\sigma_z^2}\right) + \left(\frac{\mu_b^2}{\sigma_b^2}+\frac{z^2}{\sigma_z^2}\right). \]

Complete the square. Let \( \sigma_{\text{post}}^2 = \left(\frac{1}{\sigma_b^2}+\frac{1}{\sigma_z^2}\right)^{-1} \) and \( \mu_{\text{post}} = \sigma_{\text{post}}^2 \left(\frac{\mu_b}{\sigma_b^2}+\frac{z}{\sigma_z^2}\right) \). Then:

\[ x^2\left(\frac{1}{\sigma_b^2}+\frac{1}{\sigma_z^2}\right) - 2x\left(\frac{\mu_b}{\sigma_b^2}+\frac{z}{\sigma_z^2}\right) = \frac{(x-\mu_{\text{post}})^2}{\sigma_{\text{post}}^2} - \frac{\mu_{\text{post}}^2}{\sigma_{\text{post}}^2}. \]

Therefore, the posterior has the Gaussian form:

\[ \operatorname{bel}(x) = \mathcal{N}(x;\mu_{\text{post}},\sigma_{\text{post}}^2), \quad \sigma_{\text{post}}^2 = \left(\frac{1}{\sigma_b^2}+\frac{1}{\sigma_z^2}\right)^{-1}, \quad \mu_{\text{post}} = \sigma_{\text{post}}^2\left(\frac{\mu_b}{\sigma_b^2}+\frac{z}{\sigma_z^2}\right). \]

This identity is the algebraic core of Gaussian filters: motion models typically inflate \( \sigma_b^2 \) (uncertainty growth), while sensor models reduce it through fusion (uncertainty contraction).

In practical AMR stacks, the separation is architectural:

• Motion model inputs: wheel encoders, IMU increments, commanded velocities. Motion-model code often lives near odometry integration and kinematic propagation.
• Sensor model inputs: range/bearing, LiDAR scans, camera features. Sensor-model code often lives near perception pipelines and map matching.

Common libraries by language (for later chapters, but listed now so you can map concepts to tools):

• Python: NumPy/SciPy; ROS 2 Python client libraries; probabilistic tooling around sampling.
• C++: Eigen; ROS 2 C++; common probabilistic back-ends use fast linear algebra.
• Java: Apache Commons Math, EJML; ROSJava in ecosystems that still use Java nodes.
• MATLAB/Simulink: Robotics System Toolbox; Simulink for real-time estimation diagrams.
• Wolfram Mathematica: symbolic manipulation for proofs; rapid prototyping of distributions.

File: Chapter6_Lesson3.py

# Chapter6_Lesson3.py
# Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
# Motion Models vs Sensor Models: A discrete Bayes-filter demonstration (1D grid)

from __future__ import annotations
import math
from dataclasses import dataclass

import numpy as np


def gaussian_pdf(x: np.ndarray, mu: float, sigma: float) -> np.ndarray:
    # Univariate Gaussian density N(x; mu, sigma^2) evaluated elementwise.
    sigma = float(sigma)
    if sigma <= 0.0:
        raise ValueError("sigma must be > 0")
    return (1.0 / (math.sqrt(2.0 * math.pi) * sigma)) * np.exp(-0.5 * ((x - mu) / sigma) ** 2)


def normalize(p: np.ndarray, eps: float = 1e-15) -> np.ndarray:
    s = float(np.sum(p))
    if s < eps:
        raise ValueError("Probability mass nearly zero; check likelihood / parameters.")
    return p / s


@dataclass
class OneDWorld:
    # 1D corridor world discretized into N cells of size dx (meters).
    N: int = 101
    dx: float = 0.1
    beacon_x: float = 4.0  # beacon position in meters

    @property
    def xs(self) -> np.ndarray:
        return np.arange(self.N) * self.dx


def motion_predict(bel: np.ndarray, u: float, sigma_u: float, world: OneDWorld) -> np.ndarray:
    # Motion model: x_t = x_{t-1} + u + w, w ~ N(0, sigma_u^2).
    # Discrete prediction: bel_bar[x] = sum_{x'} p(x | u, x') bel[x'].
    xs = world.xs
    bel_bar = np.zeros_like(bel)

    # For each previous state x', spread its mass according to Gaussian over x.
    for i, x_prev in enumerate(xs):
        mu = x_prev + u
        kernel = gaussian_pdf(xs, mu, sigma_u)
        bel_bar += bel[i] * kernel

    return normalize(bel_bar)


def sensor_update(bel_bar: np.ndarray, z: float, sigma_z: float, world: OneDWorld) -> np.ndarray:
    # Sensor model: z = |x - beacon_x| + v, v ~ N(0, sigma_z^2).
    # Measurement likelihood: p(z | x) = N(z; |x - beacon_x|, sigma_z^2).
    xs = world.xs
    expected = np.abs(xs - world.beacon_x)
    likelihood = gaussian_pdf(np.array([z], dtype=float), expected, sigma_z).reshape(-1)
    post = bel_bar * likelihood
    return normalize(post)


def run_demo() -> None:
    world = OneDWorld(N=121, dx=0.1, beacon_x=6.0)

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

    # Controls and measurements (synthetic)
    u_seq = [0.5, 0.5, 0.5, 0.5]  # move 0.5 m each step
    z_seq = [5.5, 5.0, 4.5, 4.0]  # observed distance-to-beacon decreasing

    sigma_u = 0.25  # motion uncertainty (meters)
    sigma_z = 0.35  # sensor uncertainty (meters)

    print("t |  MAP estimate (m) |  belief entropy (nats)")
    print("--+-------------------+----------------------")

    for t, (u, z) in enumerate(zip(u_seq, z_seq), start=1):
        bel_bar = motion_predict(bel, u=u, sigma_u=sigma_u, world=world)
        bel = sensor_update(bel_bar, z=z, sigma_z=sigma_z, world=world)

        x_map = world.xs[int(np.argmax(bel))]
        entropy = -float(np.sum(np.where(bel > 0, bel * np.log(bel), 0.0)))
        print(f"{t:>1} | {x_map:>17.3f} | {entropy:>20.6f}")

    # Export belief to CSV (useful for plotting elsewhere)
    out = np.column_stack([world.xs, bel])
    np.savetxt("Chapter6_Lesson3_belief_final.csv", out, delimiter=",", header="x,bel", comments="")
    print("\nSaved: Chapter6_Lesson3_belief_final.csv")


if __name__ == "__main__":
    run_demo()
      

The separation is explicit: motion_predict implements \( \overline{\operatorname{bel}}_t \) via a Gaussian kernel (a discrete integral), while sensor_update implements the likelihood product and normalization.

File: Chapter6_Lesson3.cpp

// Chapter6_Lesson3.cpp
// Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
// Motion Models vs Sensor Models: A discrete Bayes-filter demonstration (1D grid)
//
// Build example (Linux/macOS):
//   g++ -O2 -std=c++17 Chapter6_Lesson3.cpp -I /usr/include/eigen3 -o Chapter6_Lesson3
//
// Notes:
// - Uses Eigen for vector math.
// - For robotics ecosystems, Eigen is pervasive (ROS/ROS2, SLAM toolkits, etc.).

#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <vector>

struct OneDWorld {
  int N = 121;
  double dx = 0.1;
  double beacon_x = 6.0;

  Eigen::VectorXd xs() const {
    Eigen::VectorXd x(N);
    for (int i = 0; i < N; ++i) x(i) = i * dx;
    return x;
  }
};

static inline double gaussian_pdf(double x, double mu, double sigma) {
  if (sigma <= 0.0) throw std::runtime_error("sigma must be > 0");
  const double z = (x - mu) / sigma;
  return (1.0 / (std::sqrt(2.0 * M_PI) * sigma)) * std::exp(-0.5 * z * z);
}

static inline Eigen::VectorXd normalize(const Eigen::VectorXd& p) {
  const double s = p.sum();
  if (s <= 1e-15) throw std::runtime_error("Probability mass nearly zero.");
  return p / s;
}

Eigen::VectorXd motion_predict(const Eigen::VectorXd& bel,
                               double u,
                               double sigma_u,
                               const OneDWorld& world) {
  Eigen::VectorXd xs = world.xs();
  Eigen::VectorXd bel_bar = Eigen::VectorXd::Zero(world.N);

  // bel_bar(x) = sum_{x'} N(x; x' + u, sigma_u^2) bel(x')
  for (int i = 0; i < world.N; ++i) {
    const double x_prev = xs(i);
    const double mu = x_prev + u;
    for (int j = 0; j < world.N; ++j) {
      const double x = xs(j);
      bel_bar(j) += bel(i) * gaussian_pdf(x, mu, sigma_u);
    }
  }
  return normalize(bel_bar);
}

Eigen::VectorXd sensor_update(const Eigen::VectorXd& bel_bar,
                              double z,
                              double sigma_z,
                              const OneDWorld& world) {
  Eigen::VectorXd xs = world.xs();
  Eigen::VectorXd post(world.N);

  // p(z|x) = N(z; |x - beacon|, sigma_z^2)
  for (int i = 0; i < world.N; ++i) {
    const double expected = std::abs(xs(i) - world.beacon_x);
    const double likelihood = gaussian_pdf(z, expected, sigma_z);
    post(i) = bel_bar(i) * likelihood;
  }
  return normalize(post);
}

double entropy_nats(const Eigen::VectorXd& p) {
  double H = 0.0;
  for (int i = 0; i < p.size(); ++i) {
    const double pi = p(i);
    if (pi > 0.0) H -= pi * std::log(pi);
  }
  return H;
}

int main() {
  OneDWorld world;

  Eigen::VectorXd bel = Eigen::VectorXd::Ones(world.N);
  bel = normalize(bel);

  std::vector<double> u_seq = {0.5, 0.5, 0.5, 0.5};
  std::vector<double> z_seq = {5.5, 5.0, 4.5, 4.0};

  const double sigma_u = 0.25;
  const double sigma_z = 0.35;

  std::cout << "t |  MAP estimate (m) |  belief entropy (nats)\n";
  std::cout << "--+-------------------+----------------------\n";

  for (size_t t = 0; t < u_seq.size(); ++t) {
    Eigen::VectorXd bel_bar = motion_predict(bel, u_seq[t], sigma_u, world);
    bel = sensor_update(bel_bar, z_seq[t], sigma_z, world);

    Eigen::VectorXd xs = world.xs();
    Eigen::Index idx;
    bel.maxCoeff(&idx);
    const double x_map = xs(idx);

    std::cout << (t + 1) << " | " << std::setw(17) << std::fixed << std::setprecision(3)
              << x_map << " | " << std::setw(20) << std::setprecision(6)
              << entropy_nats(bel) << "\n";
  }

  return 0;
}
      

File: Chapter6_Lesson3.java

// Chapter6_Lesson3.java
// Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
// Motion Models vs Sensor Models: A discrete Bayes-filter demonstration (1D grid)
//
// Compile:
//   javac Chapter6_Lesson3.java
// Run:
//   java Chapter6_Lesson3
//
// Notes on libraries:
// - For probability distributions and linear algebra, consider Apache Commons Math and EJML.
// - For robotics middleware integration, ROSJava can bridge into ROS/ROS2 ecosystems.

import java.util.Arrays;

public class Chapter6_Lesson3 {

    static class OneDWorld {
        final int N;
        final double dx;
        final double beaconX;

        OneDWorld(int N, double dx, double beaconX) {
            this.N = N;
            this.dx = dx;
            this.beaconX = beaconX;
        }

        double[] xs() {
            double[] x = new double[N];
            for (int i = 0; i < N; i++) x[i] = i * dx;
            return x;
        }
    }

    static double gaussianPdf(double x, double mu, double sigma) {
        if (sigma <= 0.0) throw new IllegalArgumentException("sigma must be > 0");
        double z = (x - mu) / sigma;
        return (1.0 / (Math.sqrt(2.0 * Math.PI) * sigma)) * Math.exp(-0.5 * z * z);
    }

    static double[] normalize(double[] p) {
        double s = 0.0;
        for (double v : p) s += v;
        if (s <= 1e-15) throw new IllegalStateException("Probability mass nearly zero.");
        double[] out = new double[p.length];
        for (int i = 0; i < p.length; i++) out[i] = p[i] / s;
        return out;
    }

    static double[] motionPredict(double[] bel, double u, double sigmaU, OneDWorld world) {
        double[] xs = world.xs();
        double[] belBar = new double[world.N];
        Arrays.fill(belBar, 0.0);

        // bel_bar(x) = sum_{x'} N(x; x' + u, sigmaU^2) bel(x')
        for (int i = 0; i < world.N; i++) {
            double xPrev = xs[i];
            double mu = xPrev + u;
            for (int j = 0; j < world.N; j++) {
                double x = xs[j];
                belBar[j] += bel[i] * gaussianPdf(x, mu, sigmaU);
            }
        }
        return normalize(belBar);
    }

    static double[] sensorUpdate(double[] belBar, double z, double sigmaZ, OneDWorld world) {
        double[] xs = world.xs();
        double[] post = new double[world.N];

        for (int i = 0; i < world.N; i++) {
            double expected = Math.abs(xs[i] - world.beaconX);
            double likelihood = gaussianPdf(z, expected, sigmaZ);
            post[i] = belBar[i] * likelihood;
        }
        return normalize(post);
    }

    static double entropyNats(double[] p) {
        double H = 0.0;
        for (double pi : p) if (pi > 0.0) H -= pi * Math.log(pi);
        return H;
    }

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

    public static void main(String[] args) {
        OneDWorld world = new OneDWorld(121, 0.1, 6.0);

        double[] bel = new double[world.N];
        Arrays.fill(bel, 1.0);
        bel = normalize(bel);

        double[] uSeq = new double[] {0.5, 0.5, 0.5, 0.5};
        double[] zSeq = new double[] {5.5, 5.0, 4.5, 4.0};

        double sigmaU = 0.25;
        double sigmaZ = 0.35;

        System.out.println("t |  MAP estimate (m) |  belief entropy (nats)");
        System.out.println("--+-------------------+----------------------");

        for (int t = 0; t < uSeq.length; t++) {
            double[] belBar = motionPredict(bel, uSeq[t], sigmaU, world);
            bel = sensorUpdate(belBar, zSeq[t], sigmaZ, world);

            double[] xs = world.xs();
            int idx = argMax(bel);
            double xMap = xs[idx];

            System.out.printf("%d | %17.3f | %20.6f%n", (t + 1), xMap, entropyNats(bel));
        }
    }
}
      

File: Chapter6_Lesson3.m

% Chapter6_Lesson3.m
% Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
% Motion Models vs Sensor Models: A discrete Bayes-filter demonstration (1D grid)

clear; clc;

% World definition
N = 121;
dx = 0.1;
beacon_x = 6.0;
xs = (0:N-1) * dx;

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

% Sequence of controls and measurements
u_seq = [0.5, 0.5, 0.5, 0.5];          % meters
z_seq = [5.5, 5.0, 4.5, 4.0];          % measured distance to beacon

sigma_u = 0.25;  % motion noise
sigma_z = 0.35;  % sensor noise

fprintf('t |  MAP estimate (m) |  belief entropy (nats)\n');
fprintf('--+-------------------+----------------------\n');

for t = 1:length(u_seq)
    u = u_seq(t);
    z = z_seq(t);

    % Motion prediction: bel_bar(x) = sum_x' N(x; x'+u, sigma_u^2) bel(x')
    bel_bar = zeros(N,1);
    for i = 1:N
        mu = xs(i) + u;
        kernel = (1/(sqrt(2*pi)*sigma_u)) * exp(-0.5*((xs - mu)/sigma_u).^2)';
        bel_bar = bel_bar + bel(i) * kernel;
    end
    bel_bar = bel_bar / sum(bel_bar);

    % Sensor update: p(z|x) = N(z; |x - beacon|, sigma_z^2)
    expected = abs(xs - beacon_x);
    likelihood = (1/(sqrt(2*pi)*sigma_z)) * exp(-0.5*((z - expected)/sigma_z).^2)';
    bel = bel_bar .* likelihood;
    bel = bel / sum(bel);

    [~, idx] = max(bel);
    x_map = xs(idx);
    entropy = -sum(bel(bel>0) .* log(bel(bel>0)));

    fprintf('%d | %17.3f | %20.6f\n', t, x_map, entropy);
end

% Save final belief
T = table(xs', bel, 'VariableNames', {'x','bel'});
writetable(T, 'Chapter6_Lesson3_belief_final.csv');
fprintf('\nSaved: Chapter6_Lesson3_belief_final.csv\n');
      

File: Chapter6_Lesson3_Simulink.m

% Chapter6_Lesson3_Simulink.m
% Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
% Programmatically builds a Simulink model for a Bayes-filter loop (discrete grid).
%
% Usage (MATLAB + Simulink required):
%   run('Chapter6_Lesson3_Simulink.m')
%
% Output:
%   Chapter6_Lesson3_BayesFilter.slx

clear; clc;

modelName = 'Chapter6_Lesson3_BayesFilter';
new_system(modelName);
open_system(modelName);

% Layout coordinates
x0 = 30; y0 = 60; dx = 170;

% Add blocks
add_block('simulink/Discrete/Unit Delay', [modelName '/BeliefDelay'], ...
    'Position', [x0 y0 x0+70 y0+40], 'InitialCondition', 'ones(121,1)/121');

add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/MotionPredict'], ...
    'Position', [x0+dx y0 x0+dx+140 y0+70]);

add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/SensorUpdate'], ...
    'Position', [x0+2*dx y0 x0+2*dx+140 y0+70]);

add_block('simulink/Sinks/Scope', [modelName '/Scope'], ...
    'Position', [x0+3*dx y0 x0+3*dx+60 y0+40]);

% Connect lines
add_line(modelName, 'BeliefDelay/1', 'MotionPredict/1');
add_line(modelName, 'MotionPredict/1', 'SensorUpdate/1');
add_line(modelName, 'SensorUpdate/1', 'BeliefDelay/1', 'autorouting', 'on');
add_line(modelName, 'SensorUpdate/1', 'Scope/1');

% MotionPredict code
mp = find(slroot, '-isa', 'Stateflow.EMChart', 'Path', [modelName '/MotionPredict']);
mp.Script = [
"function bel_bar = MotionPredict(bel)\n" + ...
"% Parameters (edit as needed)\n" + ...
"N = 121; dx = 0.1; u = 0.5; sigma_u = 0.25;\n" + ...
"xs = (0:N-1)' * dx;\n" + ...
"bel_bar = zeros(N,1);\n" + ...
"for i = 1:N\n" + ...
"    mu = xs(i) + u;\n" + ...
"    kernel = exp(-0.5*((xs - mu)/sigma_u).^2) / (sqrt(2*pi)*sigma_u);\n" + ...
"    bel_bar = bel_bar + bel(i) * kernel;\n" + ...
"end\n" + ...
"bel_bar = bel_bar / sum(bel_bar);\n" + ...
"end\n"
];

% SensorUpdate code
su = find(slroot, '-isa', 'Stateflow.EMChart', 'Path', [modelName '/SensorUpdate']);
su.Script = [
"function bel_next = SensorUpdate(bel_bar)\n" + ...
"% Parameters (edit as needed)\n" + ...
"N = 121; dx = 0.1; beacon_x = 6.0; z = 5.5; sigma_z = 0.35;\n" + ...
"xs = (0:N-1)' * dx;\n" + ...
"expected = abs(xs - beacon_x);\n" + ...
"likelihood = exp(-0.5*((z - expected)/sigma_z).^2) / (sqrt(2*pi)*sigma_z);\n" + ...
"bel_next = bel_bar .* likelihood;\n" + ...
"bel_next = bel_next / sum(bel_next);\n" + ...
"end\n"
];

set_param(modelName, 'StopTime', '10', 'Solver', 'FixedStepDiscrete');
save_system(modelName);
fprintf('Saved Simulink model: %s.slx\n', modelName);
      

In Simulink, the motion model and sensor model

File: Chapter6_Lesson3.nb

(* Chapter6_Lesson3.nb
   Autonomous Mobile Robots (Control Engineering) - Chapter 6, Lesson 3
   Motion Models vs Sensor Models: Bayes filter primitives in Wolfram Mathematica *)

Notebook[{
  Cell["Chapter 6 - Lesson 3: Motion Models vs Sensor Models", "Title"],
  Cell["Discrete Bayes filter on a 1D grid with Gaussian motion and range-to-beacon measurement.", "Text"],
  Cell[BoxData[RowBox[{RowBox[{"ClearAll","[",RowBox[{"gauss"," ,","normalize"," ,","motionPredict"," ,","sensorUpdate"}],"]"}],";"}]],"Input"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"gauss","[",RowBox[{"x_"," ,","mu_"," ,","sigma_"}],"]"}],":=",
      RowBox[{"PDF","[",RowBox[{"NormalDistribution","[",RowBox[{"mu"," ,","sigma"}],"]"}]," ,","x"}],"]"}]}]],"Input"],
  Cell[BoxData[RowBox[{RowBox[{"normalize","[","p_List","]"}],":=",RowBox[{"p","/",RowBox[{"Total","[","p","]"}]}]}]],"Input"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"motionPredict","[",RowBox[{"bel_List"," ,","xs_List"," ,","u_"," ,","sigmaU_"}],"]"}],":=",
      RowBox[{"Module","[",RowBox[{"{","N, belBar","}"},
        ",",
        RowBox[{
          RowBox[{"N","=","Length","[","xs","]"}],";",
          RowBox[{"belBar","=","ConstantArray","[",RowBox[{"0"," ,","N"}],"]"}],";",
          RowBox[{"Do","[",
            RowBox[{
              RowBox[{"belBar","=","belBar","+",
                RowBox[{"bel","[",RowBox[{"[","i","]"}],"]"}],"*",
                RowBox[{"Table","[",
                  RowBox[{
                    RowBox[{"gauss","[",
                      RowBox[{"xs","[",RowBox[{"[","j","]"}],"]"}]," ,",
                        RowBox[{"xs","[",RowBox[{"[","i","]"}],"]"}],"+","u"," ,","sigmaU"}],"]"}],
                    ",",RowBox[{"{",RowBox[{"j"," ,","1"," ,","N"}],"}"}]}],"]"}]}],
              ",",RowBox[{"{",RowBox[{"i"," ,","1"," ,","N"}],"}"}]}],"]"}],";",
          RowBox[{"normalize","[","belBar","]"}]}]}],"]"}]}]],"Input"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"sensorUpdate","[",RowBox[{"belBar_List"," ,","xs_List"," ,","z_"," ,","sigmaZ_"," ,","beaconX_"}],"]"}],":=",
      RowBox[{"Module","[",RowBox[{"{","expected, likelihood, post","}"},
        ",",
        RowBox[{
          RowBox[{"expected","=","Abs","[",RowBox[{"xs","-","beaconX"}],"]"}],";",
          RowBox[{"likelihood","=","gauss","[",RowBox[{"z"," ,","expected"," ,","sigmaZ"}],"]"}],";",
          RowBox[{"post","=","belBar","*","likelihood"}],";",
          RowBox[{"normalize","[","post","]"}]}]}],"]"}]}]],"Input"],
  Cell["Demo run", "Section"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"N","=","121"}],";",
      RowBox[{"dx","=","0.1"}],";",
      RowBox[{"xs","=","Table","[",RowBox[{RowBox[{"(",RowBox[{"i","-","1"}],")"}],"*","dx"}],",",RowBox[{"{",RowBox[{"i"," ,","1"," ,","N"}],"}"}]}],"]"}],";",
      RowBox[{"bel","=","normalize","[","ConstantArray","[",RowBox[{"1"," ,","N"}],"]","]"}],";",
      RowBox[{"uSeq","=","{","0.5,0.5,0.5,0.5","}"}],";",
      RowBox[{"zSeq","=","{","5.5,5.0,4.5,4.0","}"}],";",
      RowBox[{"sigmaU","=","0.25"}],";",
      RowBox[{"sigmaZ","=","0.35"}],";",
      RowBox[{"beaconX","=","6.0"}],";",
      RowBox[{"Do","[",
        RowBox[{
          RowBox[{"belBar","=","motionPredict","[",RowBox[{"bel"," ,","xs"," ,",RowBox[{"uSeq","[",RowBox[{"[","t","]"}],"]"}]," ,","sigmaU"}],"]"}],";",
          RowBox[{"bel","=","sensorUpdate","[",RowBox[{"belBar"," ,","xs"," ,",RowBox[{"zSeq","[",RowBox[{"[","t","]"}],"]"}]," ,","sigmaZ"," ,","beaconX"}],"]"}],";"
        },",",RowBox[{"{",RowBox[{"t"," ,","1"," ,","Length","[","uSeq","]"}],"}"}]}],"]"}],";",
      RowBox[{"ListLinePlot","[",RowBox[{RowBox[{"Transpose","[",RowBox[{"{",RowBox[{"xs"," ,","bel"}],"}"}],"]"}],",","PlotRange","->","All",",","AxesLabel","->",RowBox[{"{",RowBox[{"\"x (m)\""," ,","\"belief\""}],"}"}]}],"]"}]}]],"Input"]
}, WindowTitle -> "Chapter6_Lesson3"];
      

Mathematica is particularly effective for verifying identities like Section 6 (completing squares), and for quickly experimenting with non-Gaussian likelihood families.

Problem 1 (Derive Bayes-filter recursion): Starting from \( \operatorname{bel}(x_t) = p(x_t \mid z_{1:t}, u_{1:t}) \), derive the recursion \( \operatorname{bel}(x_t) = \eta \, p(z_t \mid x_t)\int p(x_t \mid u_t, x_{t-1})\operatorname{bel}(x_{t-1})\,dx_{t-1} \) under Markov motion and Markov sensing.

Solution: This is exactly the proof given in Section 2: apply Bayes’ rule w.r.t. \( z_t \), then marginalize over \( x_{t-1} \) and apply the Markov properties.

Problem 2 (Convolution prediction): In a discrete 1D world, assume a translation-invariant motion model \( p(i \mid u, j) = k(i-j-u) \). Show prediction is a convolution operator.

Solution: Substitute into the prediction step:

\[ \overline{\operatorname{bel}}[i] = \sum_j p(i \mid u, j)\operatorname{bel}[j] = \sum_j k(i-j-u)\operatorname{bel}[j] \equiv (k_u * \operatorname{bel})[i], \]

which matches the discrete convolution definition. The kernel shift \( u \) corresponds to commanded displacement.

Problem 3 (Gaussian–Gaussian update): Let \( \overline{\operatorname{bel}}(x)=\mathcal{N}(x;\mu_b,\sigma_b^2) \) and \( p(z \mid x)=\mathcal{N}(z;x,\sigma_z^2) \). Prove the posterior is Gaussian and derive \( \mu_{\text{post}} \), \( \sigma_{\text{post}}^2 \).

Solution: Multiply exponentials, expand, and complete the square (Section 6). The result is:

\[ \sigma_{\text{post}}^2 = \left(\frac{1}{\sigma_b^2}+\frac{1}{\sigma_z^2}\right)^{-1}, \quad \mu_{\text{post}} = \sigma_{\text{post}}^2\left(\frac{\mu_b}{\sigma_b^2}+\frac{z}{\sigma_z^2}\right). \]

Problem 4 (Effect of motion noise): Consider the 1D additive motion model \( x_t = x_{t-1} + u + w \) with \( w \sim \mathcal{N}(0,\sigma_u^2) \). If \( \operatorname{bel}(x_{t-1})=\mathcal{N}(x_{t-1};\mu,\sigma^2) \), compute the predicted belief \( \overline{\operatorname{bel}}(x_t) \).

Solution: Since \( x_t = (x_{t-1}+u) + w \) and the sum of independent Gaussians is Gaussian, the predicted belief is:

\[ \overline{\operatorname{bel}}(x_t) = \mathcal{N}\!\big(x_t;\mu+u,\; \sigma^2+\sigma_u^2\big). \]

Thus the motion model inflates variance additively by \( \sigma_u^2 \), formalizing “uncertainty growth.”

Problem 5 (Robust sensor model weights): In the mixture sensor model \( p(z \mid x) = w_{\text{hit}}p_{\text{hit}}(z \mid x) + w_{\text{rand}}p_{\text{rand}}(z) + w_{\text{max}}p_{\text{max}}(z) \), show that if \( w_{\text{hit}} + w_{\text{rand}} + w_{\text{max}} = 1 \) and each component is a valid density, then \( p(z \mid x) \) is a valid density.

Solution: Nonnegativity holds because each term is nonnegative and weights are nonnegative. For normalization:

\[ \int p(z \mid x)\,dz = w_{\text{hit}}\int p_{\text{hit}}(z \mid x)\,dz + w_{\text{rand}}\int p_{\text{rand}}(z)\,dz + w_{\text{max}}\int p_{\text{max}}(z)\,dz = \\ w_{\text{hit}} + w_{\text{rand}} + w_{\text{max}} = 1. \]

Therefore the mixture is a valid sensor model; it is often more faithful to real AMR sensors than a single Gaussian.

We separated probabilistic estimation into its two fundamental model classes: \( p(x_t \mid u_t, x_{t-1}) \) (motion) and \( p(z_t \mid x_t) \) (sensor). We derived the Bayes-filter recursion rigorously, proved the convolution structure of prediction in homogeneous settings, and proved the Gaussian–Gaussian update identity that underlies tractable inference. Implementations across multiple languages demonstrated the same conceptual separation: prediction is an integral/sum (smoothing), correction is a likelihood product (reweighting) plus normalization.