In mobile robotics, the robot’s planar pose is typically summarized by \( \mathbf{x} \): a 3D state consisting of position and heading. Because odometry and sensing are noisy, the robot does not possess a single “true” pose value internally; instead it maintains a state of knowledge encoded as a probability distribution: \( bel(\mathbf{x}) \): a belief over pose.

This lesson answers: (i) what a belief mathematically is, (ii) how it differs from a point estimate, (iii) how to compute useful summaries (mean/covariance) when heading is periodic, and (iv) how beliefs transform through deterministic kinematics.

flowchart TD
  A["Robot pose (x,y,theta) is unknown"] --> B["Model uncertainty as a distribution"]
  B --> C["Belief bel(x,y,theta)"]
  C --> D["Compute summaries: mean pose, covariance"]
  D --> E["Use belief for decisions (control / safety margins)"]
  E --> F["New information arrives (motion + sensing)"]
  F --> C
        

For planar navigation, we represent pose as \( \mathbf{x} \): a vector in local coordinates:

\[ \mathbf{x} \;=\; \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}, \quad (x,y)\in\mathbb{R}^2,\; \theta \in S^1 \equiv \mathbb{R} \bmod 2\pi. \]

We interpret \( \mathbf{X} \): as a random variable taking values in \( \mathbb{R}^2 \times S^1 \). The periodicity of \( \theta \): matters: headings \( \theta \) and \( \theta + 2\pi k \) represent the same physical orientation for any integer \( k \).

A convenient integration domain for heading is the interval \( [-\pi,\pi) \): (or any length-\(2\pi\) interval). When we integrate or normalize densities, we integrate over \( \mathbb{R}^2 \times [-\pi,\pi) \).

Let \( \mathcal{I} \): denote the robot’s information set (everything the robot knows so far: internal commands, sensor data, timestamps, calibration parameters, etc.). The belief is the conditional distribution of pose given that information:

\[ bel(\mathbf{x}) \;\triangleq\; p(\mathbf{X}=\mathbf{x}\mid \mathcal{I}). \]

In continuous state spaces, we use a density \( b(\mathbf{x}) \): such that probabilities are integrals over regions \( \mathcal{A} \subset \mathbb{R}^2 \times [-\pi,\pi) \):

\[ \mathbb{P}(\mathbf{X}\in\mathcal{A}\mid\mathcal{I}) \;=\; \int\!\!\!\int\!\!\!\int_{\mathcal{A}} b(x,y,\theta)\; d\theta \; dy \; dx. \]

The defining axioms are: \( b(\mathbf{x}) \ge 0 \): and normalization:

\[ \int_{-\pi}^{\pi}\int_{-\infty}^{\infty}\int_{-\infty}^{\infty} b(x,y,\theta)\; dx\; dy\; d\theta \;=\; 1. \]

Proposition (Normalization by a Partition Function): If \( \tilde{b}(\mathbf{x}) \) is any nonnegative integrable function with \( 0 < Z < \infty \):

\[ Z \;\triangleq\; \int_{-\pi}^{\pi}\int_{\mathbb{R}^2} \tilde{b}(x,y,\theta)\; dx\; dy\; d\theta, \qquad b(\mathbf{x}) \;\triangleq\; \frac{\tilde{b}(\mathbf{x})}{Z}, \]

then \( b(\mathbf{x}) \) is a valid belief density.

Proof: Nonnegativity is immediate because \( Z > 0 \) and \( \tilde{b}(\mathbf{x}) \ge 0 \). For normalization,

\[ \int b(\mathbf{x})\, d\mathbf{x} \;=\; \int \frac{\tilde{b}(\mathbf{x})}{Z}\, d\mathbf{x} \;=\; \frac{1}{Z}\int \tilde{b}(\mathbf{x})\, d\mathbf{x} \;=\; \frac{Z}{Z} \;=\; 1. \]

This “normalize by dividing by the integral” pattern appears repeatedly in robotics implementations (grid beliefs, sample weights, likelihood reweighting).

A point estimate (e.g., “the robot is at \( \hat{\mathbf{x}} \)”) discards uncertainty structure. In contrast, a belief quantifies both the most likely pose(s) and how uncertain we are.

Degenerate belief (perfect certainty): If the pose were known exactly as \( \mathbf{x}_0 \), the belief is a Dirac delta:

\[ b(\mathbf{x}) \;=\; \delta(\mathbf{x}-\mathbf{x}_0), \quad \int b(\mathbf{x}) f(\mathbf{x})\, d\mathbf{x} \;=\; f(\mathbf{x}_0) \;\;\text{for suitable } f. \]

Maximum a posteriori (MAP): Given a belief density \( b(\mathbf{x}) \), a common point estimate is the maximizer:

\[ \hat{\mathbf{x}}_{\text{MAP}} \;\in\; \arg\max_{\mathbf{x}\in\mathbb{R}^2\times[-\pi,\pi)} b(\mathbf{x}). \]

MAP is useful but incomplete: two beliefs can share the same MAP yet have very different risk profiles (one sharply peaked, one broad/multimodal).

The mathematical object \( b(\mathbf{x}) \) can be represented in multiple ways. The choice trades expressiveness, computational cost, and numerical stability.

flowchart LR
  A["Belief over (x,y,theta)"] --> B["Parametric (few parameters)"]
  A --> C["Discrete grid (table)"]
  A --> D["Samples / particles"]
  B --> B1["Example: Gaussian mean+cov"]
  C --> C1["Example: 3D array over x,y,theta"]
  D --> D1["Example: set of weighted samples"]
        

Parametric: assume a family such as Gaussian; store mean and covariance. Discrete grid: discretize \( \mathbb{R}^2 \times S^1 \) into cells and store probability mass (or density). Samples: represent belief by weighted samples; normalization becomes “weights sum to 1”.

This lesson uses these representations only to illustrate “belief as a distribution.” Algorithms that update beliefs using motion and sensing are built in the next lessons.

For any measurable function \( f(\mathbf{x}) \): the conditional expectation is

\[ \mathbb{E}[f(\mathbf{X})\mid\mathcal{I}] \;=\; \int f(\mathbf{x})\, b(\mathbf{x})\, d\mathbf{x}. \]

Mean position: for \( X \) and \( Y \) (non-periodic) we use standard expectations:

\[ \mu_x \;=\; \mathbb{E}[X\mid\mathcal{I}], \qquad \mu_y \;=\; \mathbb{E}[Y\mid\mathcal{I}]. \]

Circular mean heading: direct averaging of angles fails near wrap-around. Define

\[ c \;=\; \mathbb{E}[\cos\Theta\mid\mathcal{I}], \qquad s \;=\; \mathbb{E}[\sin\Theta\mid\mathcal{I}], \qquad \mu_\theta \;=\; \operatorname{atan2}(s,c). \]

Why this works (optimization view): Consider the “chordal” loss \( \ell(\phi) \):

\[ \ell(\phi) \;=\; \mathbb{E}\!\left[\left\| \begin{bmatrix}\cos\Theta\\ \sin\Theta\end{bmatrix} - \begin{bmatrix}\cos\phi\\ \sin\phi\end{bmatrix}\right\|^2 \;\middle|\; \mathcal{I}\right]. \]

Expanding and using \( \cos(\Theta-\phi)=\cos\Theta\cos\phi+\sin\Theta\sin\phi \) gives:

\[ \ell(\phi) \;=\; 2 - 2\left(c\cos\phi + s\sin\phi\right). \]

Minimizing \( \ell(\phi) \) is equivalent to maximizing \( c\cos\phi + s\sin\phi \), whose maximizer is \( \phi^\star = \operatorname{atan2}(s,c) \). Hence the circular mean is the best heading summary under this natural embedding of angles into the unit circle.

Covariance: with mean \( \boldsymbol{\mu} \): (use \( \mu_\theta \) for heading) define the centered vector \( \tilde{\mathbf{X}} \):

\[ \tilde{\mathbf{X}} \;=\; \begin{bmatrix} X-\mu_x\\ Y-\mu_y\\ \operatorname{wrap}(\Theta-\mu_\theta) \end{bmatrix}, \qquad \mathbf{\Sigma} \;=\; \mathbb{E}\!\left[\tilde{\mathbf{X}}\tilde{\mathbf{X}}^\top \mid \mathcal{I}\right]. \]

Proposition (Covariance is positive semidefinite): For any vector \( \mathbf{v}\in\mathbb{R}^3 \),

\[ \mathbf{v}^\top \mathbf{\Sigma}\, \mathbf{v} \;=\; \mathbb{E}\!\left[(\mathbf{v}^\top \tilde{\mathbf{X}})^2 \mid \mathcal{I}\right] \;\ge\; 0. \]

Proof: Expand \( \mathbf{v}^\top \mathbf{\Sigma}\mathbf{v} = \mathbf{v}^\top \mathbb{E}[\tilde{\mathbf{X}}\tilde{\mathbf{X}}^\top]\mathbf{v} = \mathbb{E}[\mathbf{v}^\top\tilde{\mathbf{X}}\tilde{\mathbf{X}}^\top\mathbf{v}] = \mathbb{E}[(\mathbf{v}^\top \tilde{\mathbf{X}})^2] \). A square is always nonnegative, so the expectation is nonnegative.

Suppose a deterministic mapping transforms pose coordinates \( \mathbf{y} \): from \( \mathbf{x} \): \( \mathbf{y} = g(\mathbf{x}) \): where \( g \) is bijective and differentiable on the region of interest. Then the density changes by the Jacobian determinant.

Theorem (Change of variables): Let \( \mathbf{Y}=g(\mathbf{X}) \). The density of \( \mathbf{Y} \) satisfies

\[ b_Y(\mathbf{y}) \;=\; b_X\!\left(g^{-1}(\mathbf{y})\right)\; \left|\det\left(\frac{\partial g^{-1}}{\partial \mathbf{y}}\right)\right|. \]

Proof (via probability invariance): For any region \( \mathcal{B} \) in \( \mathbf{y} \)-space, \( \mathbb{P}(\mathbf{Y}\in\mathcal{B}) = \mathbb{P}(\mathbf{X}\in g^{-1}(\mathcal{B})) \). Using densities:

\[ \int_{\mathcal{B}} b_Y(\mathbf{y})\, d\mathbf{y} \;=\; \int_{g^{-1}(\mathcal{B})} b_X(\mathbf{x})\, d\mathbf{x}. \]

Substitute \( \mathbf{x}=g^{-1}(\mathbf{y}) \), so \( d\mathbf{x} = \left|\det\left(\frac{\partial g^{-1}}{\partial \mathbf{y}}\right)\right| d\mathbf{y} \). Then:

\[ \int_{\mathcal{B}} b_Y(\mathbf{y})\, d\mathbf{y} \;=\; \int_{\mathcal{B}} b_X\!\left(g^{-1}(\mathbf{y})\right) \left|\det\left(\frac{\partial g^{-1}}{\partial \mathbf{y}}\right)\right| d\mathbf{y}. \]

Since this holds for all regions \( \mathcal{B} \), the integrands must match almost everywhere, yielding the stated formula.

In mobile robotics, the mapping \( g \) commonly comes from pose composition with a known rigid transform (from Chapter 2 kinematics). When motion is noisy, this deterministic mapping becomes a stochastic kernel; that extension is developed in the next lesson.

This lab builds (i) a Gaussian belief over \( (x,y,\theta) \), (ii) a grid discretization normalized by cell volume, and (iii) a particle set sampled from the grid. Core libraries: numpy.

File: Chapter6_Lesson1.py

"""
Chapter 6 - Lesson 1: Belief as a Probability Distribution Over Pose
Autonomous Mobile Robots (Control Engineering)

This script demonstrates how to represent a belief over planar pose x = (x, y, theta)
as (i) a Gaussian approximation, (ii) a discrete grid approximation of a PDF, and
(iii) a particle set sampled from the grid.

Focus: normalization, expectations, and circular statistics for heading.
"""

from __future__ import annotations
import numpy as np
from dataclasses import dataclass


def wrap_to_pi(angle: float | np.ndarray) -> float | np.ndarray:
    """Wrap angle(s) to (-pi, pi]."""
    return (angle + np.pi) % (2.0 * np.pi) - np.pi


@dataclass
class GaussianBeliefSE2:
    """A simple Gaussian belief over R^2 x S^1 (theta approximated on R with wrapping)."""
    mu: np.ndarray      # shape (3,)
    Sigma: np.ndarray   # shape (3,3)

    def pdf(self, x: np.ndarray) -> float:
        x = np.asarray(x, dtype=float).reshape(3,)
        dx = x - self.mu
        dx[2] = wrap_to_pi(dx[2])
        invS = np.linalg.inv(self.Sigma)
        detS = np.linalg.det(self.Sigma)
        norm = 1.0 / np.sqrt(((2.0 * np.pi) ** 3) * detS)
        return float(norm * np.exp(-0.5 * dx.T @ invS @ dx))


def grid_belief_from_gaussian(mu, Sigma, xs, ys, thetas):
    """Discretize a Gaussian belief onto a grid and normalize w.r.t. cell volume."""
    gb = np.zeros((len(xs), len(ys), len(thetas)), dtype=float)
    g = GaussianBeliefSE2(mu=np.array(mu, dtype=float), Sigma=np.array(Sigma, dtype=float))

    for i, x in enumerate(xs):
        for j, y in enumerate(ys):
            for k, th in enumerate(thetas):
                gb[i, j, k] = g.pdf(np.array([x, y, th], dtype=float))

    dx = float(xs[1] - xs[0])
    dy = float(ys[1] - ys[0])
    dth = float(thetas[1] - thetas[0])
    cell_vol = dx * dy * dth

    Z = gb.sum() * cell_vol
    gb /= Z
    return gb, cell_vol


def grid_expectation(xs, ys, thetas, b, cell_vol):
    """Compute E[x], E[y], and circular mean of theta for a grid belief."""
    X, Y, TH = np.meshgrid(xs, ys, thetas, indexing="ij")

    ex = np.sum(X * b) * cell_vol
    ey = np.sum(Y * b) * cell_vol

    esin = np.sum(np.sin(TH) * b) * cell_vol
    ecos = np.sum(np.cos(TH) * b) * cell_vol
    eth = np.arctan2(esin, ecos)
    return np.array([ex, ey, eth], dtype=float)


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

    # Gaussian belief parameters
    mu = np.array([2.0, -1.0, 0.7], dtype=float)
    Sigma = np.diag([0.2**2, 0.3**2, (10.0 * np.pi / 180.0) ** 2])

    belief = GaussianBeliefSE2(mu=mu, Sigma=Sigma)

    x_test = np.array([2.1, -1.2, 0.75], dtype=float)
    print("pdf(x_test) =", belief.pdf(x_test))

    # Grid discretization (R^2 x S^1)
    xs = np.linspace(1.0, 3.0, 101)
    ys = np.linspace(-2.0, 0.0, 101)
    thetas = np.linspace(-np.pi, np.pi, 121, endpoint=False)

    b, cell_vol = grid_belief_from_gaussian(mu, Sigma, xs, ys, thetas)
    print("grid normalization check:", b.sum() * cell_vol)

    mu_hat = grid_expectation(xs, ys, thetas, b, cell_vol)
    print("grid E[pose] =", mu_hat)

    # Sample particles from the grid belief
    N = 2000
    probs = (b * cell_vol).ravel()
    probs /= probs.sum()

    idx = np.random.choice(probs.size, size=N, replace=True, p=probs)
    ix, iy, ith = np.unravel_index(idx, b.shape)
    particles = np.vstack([xs[ix], ys[iy], thetas[ith]]).T

    mean_xy = particles[:, :2].mean(axis=0)
    mean_th = np.arctan2(np.sin(particles[:, 2]).mean(), np.cos(particles[:, 2]).mean())
    print("particle mean approx =", np.array([mean_xy[0], mean_xy[1], mean_th]))


if __name__ == "__main__":
    main()
      

This C++ implementation mirrors the Python lab and uses Eigen for linear algebra. In robotics C++ stacks, Eigen is a standard dependency (e.g., widely used across ROS/SLAM codebases).

File: Chapter6_Lesson1.cpp

// Chapter 6 - Lesson 1: Belief as a Probability Distribution Over Pose
// Autonomous Mobile Robots (Control Engineering)
//
// This C++ example uses Eigen to represent a Gaussian belief over pose x = (x, y, theta),
// discretizes it onto a grid, normalizes by cell volume, and computes the expected pose.
// Theta is treated with wrap-to-pi for differences (Gaussian-on-R approximation).
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter6_Lesson1.cpp -I /usr/include/eigen3 -o Chapter6_Lesson1
//
// Note: install Eigen (header-only).

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

static double wrapToPi(double a) {
    // Wrap to (-pi, pi]
    const double TWO_PI = 2.0 * M_PI;
    a = std::fmod(a + M_PI, TWO_PI);
    if (a < 0.0) a += TWO_PI;
    return a - M_PI;
}

struct GaussianBeliefSE2 {
    Eigen::Vector3d mu;
    Eigen::Matrix3d Sigma;

    double pdf(const Eigen::Vector3d& x) const {
        Eigen::Vector3d dx = x - mu;
        dx(2) = wrapToPi(dx(2));

        const double detS = Sigma.determinant();
        const Eigen::Matrix3d invS = Sigma.inverse();
        const double norm = 1.0 / std::sqrt(std::pow(2.0 * M_PI, 3) * detS);
        const double expo = -0.5 * (dx.transpose() * invS * dx)(0, 0);
        return norm * std::exp(expo);
    }
};

int main() {
    // Gaussian belief parameters
    GaussianBeliefSE2 g;
    g.mu << 2.0, -1.0, 0.7;
    g.Sigma.setZero();
    g.Sigma(0, 0) = 0.2 * 0.2;
    g.Sigma(1, 1) = 0.3 * 0.3;
    g.Sigma(2, 2) = std::pow(10.0 * M_PI / 180.0, 2);

    Eigen::Vector3d x_test(2.1, -1.2, 0.75);
    std::cout << "pdf(x_test) = " << g.pdf(x_test) << "\n";

    // Grid definition
    const int Nx = 101, Ny = 101, Nt = 121;
    const double x0 = 1.0, x1 = 3.0;
    const double y0 = -2.0, y1 = 0.0;
    const double th0 = -M_PI, th1 = M_PI; // endpoint excluded for uniform bins

    std::vector<double> xs(Nx), ys(Ny), ths(Nt);
    for (int i = 0; i < Nx; ++i) xs[i] = x0 + (x1 - x0) * i / (Nx - 1);
    for (int j = 0; j < Ny; ++j) ys[j] = y0 + (y1 - y0) * j / (Ny - 1);
    for (int k = 0; k < Nt; ++k) ths[k] = th0 + (th1 - th0) * k / Nt;

    const double dx = xs[1] - xs[0];
    const double dy = ys[1] - ys[0];
    const double dth = ths[1] - ths[0];
    const double cellVol = dx * dy * dth;

    // Evaluate and normalize
    std::vector<double> b(Nx * Ny * Nt, 0.0);
    auto idx = [Ny, Nt](int i, int j, int k) { return (i * Ny + j) * Nt + k; };

    double Z = 0.0;
    for (int i = 0; i < Nx; ++i) {
        for (int j = 0; j < Ny; ++j) {
            for (int k = 0; k < Nt; ++k) {
                Eigen::Vector3d x(xs[i], ys[j], ths[k]);
                const double val = g.pdf(x);
                b[idx(i, j, k)] = val;
                Z += val * cellVol;
            }
        }
    }
    for (double& v : b) v /= Z;

    double check = 0.0;
    for (double v : b) check += v * cellVol;
    std::cout << "grid normalization check: " << check << "\n";

    // Expectation: E[x], E[y], circular mean of theta
    double ex = 0.0, ey = 0.0, esin = 0.0, ecos = 0.0;
    for (int i = 0; i < Nx; ++i) {
        for (int j = 0; j < Ny; ++j) {
            for (int k = 0; k < Nt; ++k) {
                const double w = b[idx(i, j, k)] * cellVol;
                ex += xs[i] * w;
                ey += ys[j] * w;
                esin += std::sin(ths[k]) * w;
                ecos += std::cos(ths[k]) * w;
            }
        }
    }
    const double eth = std::atan2(esin, ecos);
    std::cout << "grid E[pose] = [" << ex << ", " << ey << ", " << eth << "]\n";

    return 0;
}
      

This Java version uses Apache Commons Math for matrix inversion/determinants. This is a practical route for robotics-adjacent Java stacks (simulation tooling, embedded analytics, or ROS-bridge code).

File: Chapter6_Lesson1.java

// Chapter 6 - Lesson 1: Belief as a Probability Distribution Over Pose
// Autonomous Mobile Robots (Control Engineering)
//
// This Java example uses Apache Commons Math (linear algebra) to compute a 3D Gaussian PDF,
// discretize it on a grid, normalize by cell volume, and compute expected pose with a
// circular mean for theta.
//
// Dependency (Maven):
//   <dependency>
//     <groupId>org.apache.commons</groupId>
//     <artifactId>commons-math3</artifactId>
//     <version>3.6.1</version>
//   </dependency>

import org.apache.commons.math3.linear.LUDecomposition;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix;

public class Chapter6_Lesson1 {

    @FunctionalInterface
    interface Index3 {
        int index(int i, int j, int k);
    }

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

    private static double gaussianPdf3(double[] x, double[] mu, RealMatrix Sigma) {
        double[] dx = new double[] { x[0] - mu[0], x[1] - mu[1], wrapToPi(x[2] - mu[2]) };

        RealMatrix invS = new LUDecomposition(Sigma).getSolver().getInverse();
        double detS = new LUDecomposition(Sigma).getDeterminant();
        double norm = 1.0 / Math.sqrt(Math.pow(2.0 * Math.PI, 3.0) * detS);

        // quadratic form dx' invS dx
        double q = 0.0;
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                q += dx[i] * invS.getEntry(i, j) * dx[j];
            }
        }
        return norm * Math.exp(-0.5 * q);
    }

    public static void main(String[] args) {
        double[] mu = new double[] { 2.0, -1.0, 0.7 };
        RealMatrix Sigma = MatrixUtils.createRealMatrix(new double[][] {
            { 0.2 * 0.2, 0.0, 0.0 },
            { 0.0, 0.3 * 0.3, 0.0 },
            { 0.0, 0.0, Math.pow(10.0 * Math.PI / 180.0, 2.0) }
        });

        double[] xTest = new double[] { 2.1, -1.2, 0.75 };
        System.out.println("pdf(x_test) = " + gaussianPdf3(xTest, mu, Sigma));

        int Nx = 101, Ny = 101, Nt = 121;
        double x0 = 1.0, x1 = 3.0;
        double y0 = -2.0, y1 = 0.0;
        double th0 = -Math.PI, th1 = Math.PI;

        double[] xs = new double[Nx];
        double[] ys = new double[Ny];
        double[] ths = new double[Nt];

        for (int i = 0; i < Nx; ++i) xs[i] = x0 + (x1 - x0) * i / (Nx - 1.0);
        for (int j = 0; j < Ny; ++j) ys[j] = y0 + (y1 - y0) * j / (Ny - 1.0);
        for (int k = 0; k < Nt; ++k) ths[k] = th0 + (th1 - th0) * k / Nt; // endpoint excluded

        double dx = xs[1] - xs[0];
        double dy = ys[1] - ys[0];
        double dth = ths[1] - ths[0];
        double cellVol = dx * dy * dth;

        double[] b = new double[Nx * Ny * Nt];
        Index3 index3 = (i, j, k) -> (i * Ny + j) * Nt + k;

        double Z = 0.0;
        for (int i = 0; i < Nx; ++i) {
            for (int j = 0; j < Ny; ++j) {
                for (int k = 0; k < Nt; ++k) {
                    double[] x = new double[] { xs[i], ys[j], ths[k] };
                    double val = gaussianPdf3(x, mu, Sigma);
                    b[index3.index(i, j, k)] = val;
                    Z += val * cellVol;
                }
            }
        }
        for (int t = 0; t < b.length; ++t) b[t] /= Z;

        double check = 0.0;
        for (double v : b) check += v * cellVol;
        System.out.println("grid normalization check: " + check);

        double ex = 0.0, ey = 0.0, esin = 0.0, ecos = 0.0;
        for (int i = 0; i < Nx; ++i) {
            for (int j = 0; j < Ny; ++j) {
                for (int k = 0; k < Nt; ++k) {
                    double w = b[index3.index(i, j, k)] * cellVol;
                    ex += xs[i] * w;
                    ey += ys[j] * w;
                    esin += Math.sin(ths[k]) * w;
                    ecos += Math.cos(ths[k]) * w;
                }
            }
        }
        double eth = Math.atan2(esin, ecos);
        System.out.println("grid E[pose] = [" + ex + ", " + ey + ", " + eth + "]");
    }
}
      

MATLAB is commonly used in control engineering workflows and rapid prototyping. This script computes a grid belief, verifies normalization, computes the circular-mean heading, and programmatically creates a tiny Simulink model that illustrates normalization as a reusable block pattern.

File: Chapter6_Lesson1.m

% Chapter 6 - Lesson 1: Belief as a Probability Distribution Over Pose
% Autonomous Mobile Robots (Control Engineering)
%
% This script demonstrates:
%   1) A Gaussian belief over pose x = [x; y; theta] (theta approximated on R)
%   2) Discretization onto a grid and normalization by cell volume
%   3) Expected pose with circular mean for theta
%   4) Programmatic creation of a simple Simulink model that calls a MATLAB Function
%      to normalize a vector (illustrating how belief normalization can be embedded).

clear; clc;

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

% Gaussian belief parameters
mu = [2.0; -1.0; 0.7];
Sigma = diag([0.2^2, 0.3^2, (10*pi/180)^2]);

% Evaluate PDF at a point (manual 3D Gaussian pdf)
x_test = [2.1; -1.2; 0.75];
dx = x_test - mu;
dx(3) = wrapToPi(dx(3));
invS = inv(Sigma);
detS = det(Sigma);
normC = 1/sqrt((2*pi)^3 * detS);
pdf_test = normC * exp(-0.5 * (dx' * invS * dx));
fprintf('pdf(x_test) = %.6e\n', pdf_test);

% Grid discretization
xs = linspace(1.0, 3.0, 101);
ys = linspace(-2.0, 0.0, 101);
thetas = linspace(-pi, pi, 121+1); thetas(end) = []; % endpoint excluded

dxg = xs(2)-xs(1);
dyg = ys(2)-ys(1);
dthg = thetas(2)-thetas(1);
cellVol = dxg * dyg * dthg;

b = zeros(length(xs), length(ys), length(thetas));
for i = 1:length(xs)
    for j = 1:length(ys)
        for k = 1:length(thetas)
            x = [xs(i); ys(j); thetas(k)];
            d = x - mu;
            d(3) = wrapToPi(d(3));
            b(i,j,k) = normC * exp(-0.5 * (d' * invS * d));
        end
    end
end

Z = sum(b(:)) * cellVol;
b = b / Z;
fprintf('grid normalization check: %.12f\n', sum(b(:)) * cellVol);

% Expectation with circular mean
[X, Y, TH] = ndgrid(xs, ys, thetas);
ex = sum(X(:) .* b(:)) * cellVol;
ey = sum(Y(:) .* b(:)) * cellVol;

esin = sum(sin(TH(:)) .* b(:)) * cellVol;
ecos = sum(cos(TH(:)) .* b(:)) * cellVol;
eth = atan2(esin, ecos);

fprintf('grid E[pose] = [%.6f, %.6f, %.6f]\n', ex, ey, eth);

%% (Optional) Simulink: Create a tiny model for normalization
% The model generates a random vector u and normalizes it with a MATLAB Function block.
% This is a minimal pattern you can adapt to normalize a discrete belief vector in Simulink.

modelName = 'Chapter6_Lesson1_Simulink';
if bdIsLoaded(modelName)
    close_system(modelName, 0);
end
new_system(modelName);
open_system(modelName);

add_block('simulink/Sources/Random Number', [modelName '/Random']);
set_param([modelName '/Random'], 'Mean', '0', 'Variance', '1', 'SampleTime', '0.1');

add_block('simulink/Math Operations/Gain', [modelName '/Gain']);
set_param([modelName '/Gain'], 'Gain', '1');

add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/Normalize']);
set_param([modelName '/Normalize'], 'MATLABFcn', ...
['function y = f(u)\n' ...
'  % Normalize a scalar or vector to sum to 1 (simple example)\n' ...
'  u = abs(u);\n' ...
'  s = sum(u);\n' ...
'  if s == 0\n' ...
'    y = u;\n' ...
'  else\n' ...
'    y = u / s;\n' ...
'  end\n' ...
'end']);

add_block('simulink/Sinks/Scope', [modelName '/Scope']);

add_line(modelName, 'Random/1', 'Gain/1');
add_line(modelName, 'Gain/1', 'Normalize/1');
add_line(modelName, 'Normalize/1', 'Scope/1');

set_param(modelName, 'StopTime', '2.0');
save_system(modelName);

disp(['Created Simulink model: ' modelName '.slx']);
      

Mathematica is useful for verification, symbolic manipulation, and rapid probability calculations. This notebook builds a Gaussian belief, discretizes it, and checks normalization and expectations.

File: Chapter6_Lesson1.nb

(* Content-type: application/vnd.wolfram.mathematica *)

Notebook[{
 Cell["Chapter 6 - Lesson 1: Belief as a Probability Distribution Over Pose", "Title"],
 Cell["Autonomous Mobile Robots (Control Engineering)", "Subtitle"],

 Cell[CellGroupData[{
   Cell["1. Gaussian belief over pose x = (x, y, theta)", "Section"],
   Cell[BoxData[
     RowBox[{
       RowBox[{"mu", "=", RowBox[{"{", RowBox[{"2.0", ",", RowBox[{"-", "1.0"}], ",", "0.7"}], "}"}]}], ";",
       "\n",
       RowBox[{"Sigma", "=", RowBox[{"DiagonalMatrix", "[", RowBox[{"{", RowBox[{
         SuperscriptBox["0.2", "2"], ",",
         SuperscriptBox["0.3", "2"], ",",
         SuperscriptBox[RowBox[{"10", " ", RowBox[{"Pi", "/", "180"}]}], "2"]
       }], "}"}], "]"}]}], ";"
     }]
   ], "Input"],

   Cell["Define wrap-to-pi and a Gaussian pdf with wrapped angle difference (approximation).", "Text"],
   Cell[BoxData[
     RowBox[{
       RowBox[{"wrapToPi", "[", "a_", "]"}], ":=",
       RowBox[{
         RowBox[{"Mod", "[", RowBox[{"a", "+", "Pi", ",", RowBox[{"2", "Pi"}]}], "]"}], "-", "Pi"
       }], ";"
     }]
   ], "Input"],

   Cell[BoxData[
     RowBox[{
       RowBox[{"gaussPdf3", "[", RowBox[{"x_", ",", "mu_", ",", "Sigma_"}], "]"}], ":=",
       RowBox[{"Module", "[", RowBox[
         RowBox[{"{", RowBox[{"dx", ",", "invS", ",", "detS", ",", "normC"}], "}"}], ",",
         RowBox[{
           RowBox[{"dx", "=", RowBox[{"x", "-", "mu"}]}], ";",
           RowBox[{"dx", "=", RowBox[{"ReplacePart", "[", RowBox[{"dx", ",", RowBox[{"3", "->", RowBox[{"wrapToPi", "[", RowBox[{"dx", "[", RowBox[{"[", "3", "]"}], "]"}], "]"}]}]}], "]"}]}], ";",
           RowBox[{"invS", "=", RowBox[{"Inverse", "[", "Sigma", "]"}]}], ";",
           RowBox[{"detS", "=", RowBox[{"Det", "[", "Sigma", "]"}]}], ";",
           RowBox[{"normC", "=", RowBox[{"1", "/", RowBox[{"Sqrt", "[", RowBox[{
             SuperscriptBox[RowBox[{"2", "Pi"}], "3"], " ", "detS"
           }], "]"}]}]}], ";",
           RowBox[{"normC", " ", RowBox[{"Exp", "[", RowBox[{"-", RowBox[{"1", "/", "2"}], " ", RowBox[{"dx", ".", "invS", ".", "dx"}]}], "]"}]}]
         }]
       }], "]"}], ";"
     }]
   ], "Input"],

   Cell[BoxData[
     RowBox[{
       RowBox[{"xTest", "=", RowBox[{"{", RowBox[{"2.1", ",", RowBox[{"-", "1.2"}], ",", "0.75"}], "}"}]}], ";",
       "\n",
       RowBox[{"gaussPdf3", "[", RowBox[{"xTest", ",", "mu", ",", "Sigma"}], "]"}]
     }]
   ], "Input"]
 }], Open]],

 Cell[CellGroupData[{
   Cell["2. Discretize onto a grid and normalize", "Section"],
   Cell[BoxData[
     RowBox[{
       RowBox[{"xs", "=", RowBox[{"Subdivide", "[", RowBox[{"1.0", ",", "3.0", ",", "100"}], "]"}]}], ";",
       "\n",
       RowBox[{"ys", "=", RowBox[{"Subdivide", "[", RowBox[{"-2.0", ",", "0.0", ",", "100"}], "]"}]}], ";",
       "\n",
       RowBox[{"thetas", "=", RowBox[{"Most", "[", RowBox[{"Subdivide", "[", RowBox[{"-Pi", ",", "Pi", ",", "121"}], "]"}], "]"}]}], ";"
     }]
   ], "Input"],

   Cell[BoxData[
     RowBox[{
       RowBox[{"dx", "=", RowBox[{"xs", "[", RowBox[{"[", "2", "]"}], "]"}], "-", RowBox[{"xs", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
       "\n",
       RowBox[{"dy", "=", RowBox[{"ys", "[", RowBox[{"[", "2", "]"}], "]"}], "-", RowBox[{"ys", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
       "\n",
       RowBox[{"dth", "=", RowBox[{"thetas", "[", RowBox[{"[", "2", "]"}], "]"}], "-", RowBox[{"thetas", "[", RowBox[{"[", "1", "]"}], "]"}]}], ";",
       "\n",
       RowBox[{"cellVol", "=", RowBox[{"dx", " ", "dy", " ", "dth"}]}], ";"
     }]
   ], "Input"],

   Cell["Compute unnormalized grid values then normalize by Z = Sum(b)*cellVol.", "Text"],
   Cell[BoxData[
     RowBox[{
       RowBox[{"bUn", "=", RowBox[{"Table", "[", RowBox[{
         RowBox[{"gaussPdf3", "[", RowBox[{
           RowBox[{"{", RowBox[{"x", ",", "y", ",", "th"}], "}"}], ",", "mu", ",", "Sigma"
         }], "]"}], ",",
         RowBox[{"{", RowBox[{"x", ",", "xs"}], "}"}], ",",
         RowBox[{"{", RowBox[{"y", ",", "ys"}], "}"}], ",",
         RowBox[{"{", RowBox[{"th", ",", "thetas"}], "}"}]
       }], "]"}]}], ";",
       "\n",
       RowBox[{"Z", "=", RowBox[{"Total", "[", RowBox[{"bUn", ",", "Infinity"}], "]"}], " ", "cellVol"}], ";",
       "\n",
       RowBox[{"b", "=", RowBox[{"bUn", "/", "Z"}]}], ";",
       "\n",
       RowBox[{"Total", "[", RowBox[{"b", ",", "Infinity"}], "]"}], " ", "cellVol"
     }]
   ], "Input"]
 }], Open]]
},
WindowSize -> {1000, 700},
StyleDefinitions -> "Default.nb"
]
      

Problem 1 (Normalization on a Pose Domain): Let \( b(x,y,\theta) \): be a nonnegative function on \( \mathbb{R}^2 \times [-\pi,\pi) \). Define \( Z=\int b \) and \( \bar{b}=b/Z \). Prove that \( \bar{b} \) is a valid density if \( 0 < Z < \infty \).

Solution: This is exactly the normalization proposition in Section 3. Nonnegativity holds since \( Z > 0 \). Normalization follows:

\[ \int \bar{b}(\mathbf{x})\, d\mathbf{x} \;=\; \frac{1}{Z}\int b(\mathbf{x})\, d\mathbf{x} \;=\; \frac{Z}{Z} \;=\; 1. \]

Problem 2 (Grid Approximation Error): Suppose you approximate \( \int b(\mathbf{x})\, d\mathbf{x} \) with a grid sum \( \sum_{i,j,k} b_{ijk}\,\Delta x\,\Delta y\,\Delta \theta \). Give a sufficient condition under which the approximation converges to the true integral as grid resolution increases.

Solution: A standard sufficient condition is that \( b \) is Riemann integrable on the domain (e.g., bounded and continuous almost everywhere on any large truncated region; and tails vanish as truncation expands). Then as \( \Delta x,\Delta y,\Delta\theta \rightarrow 0 \) and truncation bounds expand, the Riemann sums converge to the integral.

Problem 3 (Circular Mean Optimality): Show that \( \mu_\theta=\operatorname{atan2}(s,c) \) with \( s=\mathbb{E}[\sin\Theta] \), \( c=\mathbb{E}[\cos\Theta] \) minimizes the chordal loss in Section 6.

Solution: From Section 6, the loss reduces to \( \ell(\phi)=2-2(c\cos\phi+s\sin\phi) \). Minimizing \( \ell \) is maximizing the inner product \( c\cos\phi+s\sin\phi \), which is maximized when \( (\cos\phi,\sin\phi) \) aligns with \( (c,s) \). Hence \( \phi^\star=\operatorname{atan2}(s,c) \).

Problem 4 (Covariance PSD): Let \( \mathbf{\Sigma}=\mathbb{E}[\tilde{\mathbf{X}}\tilde{\mathbf{X}}^\top] \). Prove \( \mathbf{\Sigma} \) is positive semidefinite.

Solution: For any \( \mathbf{v} \),

\[ \mathbf{v}^\top\mathbf{\Sigma}\mathbf{v} = \mathbb{E}\!\left[\mathbf{v}^\top\tilde{\mathbf{X}}\tilde{\mathbf{X}}^\top\mathbf{v}\right] = \mathbb{E}\!\left[(\mathbf{v}^\top\tilde{\mathbf{X}})^2\right] \ge 0. \]

Problem 5 (Deterministic Transform Check): Consider a 2D change of variables \( \mathbf{y}=g(\mathbf{x}) \) with invertible Jacobian. Using Section 7, prove that if \( b_X \) integrates to 1, then \( b_Y \) integrates to 1.

Solution: Using the change-of-variables theorem,

\[ \int b_Y(\mathbf{y})\, d\mathbf{y} = \int b_X\!\left(g^{-1}(\mathbf{y})\right)\left|\det\left(\frac{\partial g^{-1}}{\partial \mathbf{y}}\right)\right| d\mathbf{y} = \int b_X(\mathbf{x})\, d\mathbf{x} = 1. \]

Problem 6 (Programming Exercise — Robust Angle Differences): Implement a function \( \operatorname{wrap}(\alpha) \) that maps angles to \( (-\pi,\pi] \), and show (by testing random angles) that \( \operatorname{wrap}(\alpha+2\pi k)=\operatorname{wrap}(\alpha) \) for integers \( k \).

Solution: Use the pattern in the code labs (Python: modulo, C++: fmod, Java: modulo). For tests, sample random \( \alpha \) and random integers \( k \), and verify equality within floating-point tolerance.

We defined belief as a conditional probability distribution over planar pose, established normalization and expectation operators on \( \mathbb{R}^2 \times S^1 \), and derived robust summaries (circular mean for heading; PSD covariance). We also proved the deterministic change-of-variables rule for how beliefs transform through coordinate mappings induced by kinematics. These results are the mathematical substrate for Bayes filtering in Lesson 2.