In Lesson 1, we defined the \( bel(x_t) \) as a probability distribution over the robot pose/state at time \( t \), conditioned on all data available up to time \( t \). The Bayes filter provides a recursive mechanism to propagate this belief through time as controls and sensor readings arrive.

Let the robot state be \( x_t \), the control (odometry command) be \( u_t \), and the measurement be \( z_t \). Denote histories by \( u_{1:t} = (u_1,\dots,u_t) \) and \( z_{1:t} = (z_1,\dots,z_t) \). The Bayes filter targets the posterior:

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

The core idea is to split the update into: (i) a prediction step that pushes belief forward under the motion model, and (ii) a correction step that reweights belief using the sensor likelihood and renormalizes.

flowchart TD
  A["Inputs at time t"] --> U["Control u_t"]
  A --> Z["Measurement z_t"]
  B0["Prior belief bel(x_{t-1})"] --> P["Prediction: apply motion model"]
  U --> P
  P --> BB["Predicted belief bel_bar(x_t)"]
  BB --> C["Correction: apply measurement likelihood"]
  Z --> C
  C --> B1["Posterior belief bel(x_t) (normalized)"]
        

Two conditional independence assumptions (made explicit in Section 2) are sufficient to derive the recursion: a first-order Markov property for the state transition and conditional independence of measurements given the current state.

The Bayes filter rests on two standard modeling assumptions for mobile robots:

Assumption A (Controlled Markov dynamics): the state is first-order Markov given the control:

\[ p(x_t \mid x_{0:t-1}, u_{1:t}) \;=\; p(x_t \mid x_{t-1}, u_t). \]

Assumption B (Conditional independence of measurements): the measurement depends on the current state only:

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

These are structural assumptions; their practical validity (and failure modes) will be examined later (Chapter 6, Lesson 4). For now, we use them as the axioms that define the Bayes filter recursion.

Theorem (Bayes filter recursion). Under Assumptions A–B, the posterior belief satisfies:

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

where \( \eta \) is a normalization constant ensuring \( \int bel(x_t)\,dx_t = 1 \). For discrete state spaces (e.g., grid cells), the integral becomes a sum:

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

We now prove this theorem carefully, using only Bayes’ rule and the law of total probability.

Start from the definition: \( bel(x_t)=p(x_t \mid z_{1:t}, u_{1:t}) \). Using Bayes’ rule:

\[ 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 Assumption B, \( p(z_t \mid x_t, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t) \). Let the denominator be the normalization constant \( \eta^{-1} \):

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

The remaining term \( p(x_t \mid z_{1:t-1}, u_{1:t}) \) is the predicted belief (often denoted \( \overline{bel}(x_t) \)). Apply the law of total probability over \( x_{t-1} \):

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

Factor the joint: \( p(x_t, x_{t-1} \mid \cdot) = p(x_t \mid x_{t-1}, \cdot)\, p(x_{t-1} \mid \cdot) \):

\[ 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}. \]

Now apply Assumption A and the fact that \( x_{t-1} \) is determined before \( u_t \) is applied: \( 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}) = bel(x_{t-1}) \), and \( p(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t) \). Therefore:

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

Substitute into the earlier Bayes’ rule expression to obtain the Bayes filter recursion:

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

Normalization constant and existence. Define:

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

If \( bel(x_{t-1}) \) is a valid probability density/mass function and the motion model \( p(x_t \mid x_{t-1}, u_t) \) is a conditional density/mass function, then the inner integral yields a nonnegative predicted density. Multiplying by \( p(z_t \mid x_t) \ge 0 \) preserves nonnegativity, and choosing \( \eta \) makes the result integrate/sum to 1.

Corollary (Bayes filter preserves probability mass). If \( bel(x_{t-1}) \) integrates to 1, then \( bel(x_t) \) integrates to 1 after applying \( \eta \).

For a finite state space with \( N \) states, collect beliefs into a probability vector \( \mathbf{b}_t \in \mathbb{R}^N \), where \( (\mathbf{b}_t)_i = bel(x_t=i) \). Define a control-indexed transition matrix \( \mathbf{T}_t \in \mathbb{R}^{N \times N} \) by: \( (\mathbf{T}_t)_{ij} = p(x_t=i \mid x_{t-1}=j, u_t) \). Then the prediction step is:

\[ \overline{\mathbf{b}}_t \;=\; \mathbf{T}_t \mathbf{b}_{t-1}. \]

Let the measurement likelihood vector be \( \boldsymbol{\ell}_t \in \mathbb{R}^N \) with \( (\boldsymbol{\ell}_t)_i = p(z_t \mid x_t=i) \). The correction step is elementwise multiplication:

\[ \mathbf{b}_t \;=\; \eta \, \big(\boldsymbol{\ell}_t \odot \overline{\mathbf{b}}_t\big), \quad \eta^{-1} \;=\; \mathbf{1}^\top \big(\boldsymbol{\ell}_t \odot \overline{\mathbf{b}}_t\big). \]

Proposition (stochasticity and nonnegativity). If every column of \( \mathbf{T}_t \) sums to 1 and entries are nonnegative (i.e., a column-stochastic matrix), then \( \overline{\mathbf{b}}_t \) is a valid probability vector whenever \( \mathbf{b}_{t-1} \) is.

Proof. Nonnegativity: \( \overline{\mathbf{b}}_t = \mathbf{T}_t \mathbf{b}_{t-1} \) is a nonnegative linear combination of nonnegative columns. Sum-to-one:

\[ \mathbf{1}^\top \overline{\mathbf{b}}_t \;=\; \mathbf{1}^\top \mathbf{T}_t \mathbf{b}_{t-1} \;=\; \big(\mathbf{1}^\top \mathbf{T}_t\big)\mathbf{b}_{t-1}. \]

Column-stochasticity implies \( \mathbf{1}^\top \mathbf{T}_t = \mathbf{1}^\top \), hence \( \mathbf{1}^\top \overline{\mathbf{b}}_t = \mathbf{1}^\top \mathbf{b}_{t-1} = 1 \). The correction step preserves nonnegativity and enforces sum-to-one via \( \eta \). \( \square \)

This matrix form is particularly useful for implementation and complexity analysis: naïvely, \( \mathbf{T}_t \mathbf{b}_{t-1} \) costs \( \mathcal{O}(N^2) \), but if \( \mathbf{T}_t \) is sparse/banded (typical in local motion on grids), the cost reduces to \( \mathcal{O}(Nk) \) where \( k \) is the number of nonzeros per column.

The Bayes filter is often easier to remember as a factorization of the joint distribution, followed by marginalization. Under Assumptions A–B, the controlled hidden Markov factorization is:

\[ p(x_{0:t}, z_{1:t} \mid u_{1:t}) \;=\; p(x_0)\, \prod_{k=1}^{t} p(x_k \mid x_{k-1}, u_k)\, \prod_{k=1}^{t} p(z_k \mid x_k). \]

The posterior belief \( bel(x_t) \) is obtained by marginalizing out \( x_{0:t-1} \) and conditioning on \( z_{1:t} \).

flowchart LR
  X0["x_0"] --> X1["x_1"] --> X2["x_2"] --> XT["x_t"]
  Z1["z_1"] --> X1
  Z2["z_2"] --> X2
  ZT["z_t"] --> XT
  U1["u_1"] --> X1
  U2["u_2"] --> X2
  UT["u_t"] --> XT
        

The recursion arises because the posterior at \( t-1 \) summarizes all past information \( (z_{1:t-1}, u_{1:t-1}) \), enabling a one-step prediction and correction at time \( t \).

Python implementations commonly use \( \texttt{numpy} \) for vectorized prediction and update. In a robotics stack, this logic typically lives inside a node (e.g., ROS2 rclpy) that receives \( u_t \) and \( z_t \) messages and publishes the belief state.

File: Chapter6_Lesson2.py

"""
Chapter6_Lesson2.py
Bayes Filters for Mobile Robots (discrete-state version)

This module implements the Bayes filter recursion for a finite state space:
  bel_t = eta * p(z_t | x_t) * sum_{x_{t-1}} p(x_t | x_{t-1}, u_t) * bel_{t-1}

It also includes a small 1D corridor demo (grid localization) to illustrate
prediction + update steps.
"""

from __future__ import annotations

from dataclasses import dataclass
from typing import Iterable, Tuple

import numpy as np


def normalize(p: np.ndarray, eps: float = 1e-12) -> np.ndarray:
    """Normalize a nonnegative vector to sum to 1."""
    s = float(np.sum(p))
    if s < eps:
        # If everything is (near) zero, fall back to uniform to avoid NaNs.
        return np.ones_like(p) / p.size
    return p / s


@dataclass
class DiscreteBayesFilter:
    """
    Discrete Bayes filter on N states.

    Transition matrix T has shape (N, N) with:
        T[i, j] = P(x_t = i | x_{t-1} = j, u_t)
    """
    N: int

    def predict(self, bel_prev: np.ndarray, T: np.ndarray) -> np.ndarray:
        bel_prev = np.asarray(bel_prev, dtype=float).reshape(self.N)
        T = np.asarray(T, dtype=float).reshape(self.N, self.N)
        # bel_bar[i] = sum_j T[i,j] bel_prev[j]
        bel_bar = T @ bel_prev
        return normalize(bel_bar)

    def update(self, bel_bar: np.ndarray, likelihood: np.ndarray) -> np.ndarray:
        bel_bar = np.asarray(bel_bar, dtype=float).reshape(self.N)
        likelihood = np.asarray(likelihood, dtype=float).reshape(self.N)
        bel = likelihood * bel_bar
        return normalize(bel)

    def step(self, bel_prev: np.ndarray, T: np.ndarray, likelihood: np.ndarray) -> np.ndarray:
        bel_bar = self.predict(bel_prev, T)
        return self.update(bel_bar, likelihood)


def make_shift_transition(N: int, delta: int,
                          p_exact: float = 0.8,
                          p_undershoot: float = 0.1,
                          p_overshoot: float = 0.1) -> np.ndarray:
    """
    A simple probabilistic motion model on a 1D ring (mod N).
    Returns T[i,j] = P(x_t=i | x_{t-1}=j, u_t=delta)
    """
    assert abs((p_exact + p_undershoot + p_overshoot) - 1.0) < 1e-9
    T = np.zeros((N, N), dtype=float)
    for j in range(N):
        i_exact = (j + delta) % N
        i_under = (j + delta - 1) % N
        i_over = (j + delta + 1) % N
        T[i_exact, j] += p_exact
        T[i_under, j] += p_undershoot
        T[i_over, j] += p_overshoot
    return T


def landmark_likelihood(N: int, z: int, landmarks: Iterable[int],
                        p_hit: float = 0.9, p_false: float = 0.1) -> np.ndarray:
    """
    Binary measurement model:
      z=1 means "detect landmark", z=0 means "no landmark".
    """
    L = np.full(N, p_false if z == 1 else (1.0 - p_false), dtype=float)
    landmarks = set(int(k) for k in landmarks)
    for x in landmarks:
        L[x % N] = p_hit if z == 1 else (1.0 - p_hit)
    return L


def demo_1d_corridor(seed: int = 0) -> Tuple[np.ndarray, np.ndarray]:
    """
    Small demo: localization on a 1D ring with 20 cells and two landmarks.
    Returns bel_history and truth trajectory.
    """
    rng = np.random.default_rng(seed)
    N = 20
    bf = DiscreteBayesFilter(N=N)

    bel = np.ones(N) / N
    landmarks = [3, 14]
    controls = [1, 1, 1, 2, 1, 1, 2, 1]

    truth = [rng.integers(0, N)]
    zs = []

    for u in controls:
        x_prev = truth[-1]
        r = rng.random()
        if r < 0.8:
            x = (x_prev + u) % N
        elif r < 0.9:
            x = (x_prev + u - 1) % N
        else:
            x = (x_prev + u + 1) % N
        truth.append(int(x))

        at_landmark = (x in landmarks)
        if at_landmark:
            z = 1 if rng.random() < 0.9 else 0
        else:
            z = 1 if rng.random() < 0.1 else 0
        zs.append(int(z))

    bel_hist = [bel.copy()]
    for u, z in zip(controls, zs):
        Tmat = make_shift_transition(N, delta=u)
        L = landmark_likelihood(N, z=z, landmarks=landmarks)
        bel = bf.step(bel, Tmat, L)
        bel_hist.append(bel.copy())

    return np.vstack(bel_hist), np.array(truth, dtype=int)


if __name__ == "__main__":
    bel_hist, truth = demo_1d_corridor(seed=4)
    print("Final belief (top 5 states):")
    idx = np.argsort(-bel_hist[-1])[:5]
    for i in idx:
        print(f"  state {i:2d}: {bel_hist[-1, i]:.4f}")
    print("Truth trajectory:", truth.tolist())
      

Practical note: when likelihoods become extremely small, use log space: store \( \log \boldsymbol{\ell}_t \) and compute with log-sum-exp, then exponentiate and normalize.

C++ is the common deployment language for real-time robotics. A typical production implementation would use Eigen for linear algebra and integrate with a middleware layer (e.g., ROS2 rclcpp). Here, we implement the recursion directly with std::vector for clarity.

File: Chapter6_Lesson2.cpp

/*
Chapter6_Lesson2.cpp
Bayes Filters for Mobile Robots (discrete-state version)
*/

#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <random>
#include <stdexcept>
#include <vector>

static std::vector<double> normalize(const std::vector<double>& p, double eps = 1e-12) {
    double s = std::accumulate(p.begin(), p.end(), 0.0);
    std::vector<double> out(p.size(), 0.0);
    if (s < eps) {
        double u = 1.0 / static_cast<double>(p.size());
        std::fill(out.begin(), out.end(), u);
        return out;
    }
    for (size_t i = 0; i < p.size(); ++i) out[i] = p[i] / s;
    return out;
}

static std::vector<double> predict(const std::vector<double>& bel_prev,
                                   const std::vector<std::vector<double>>& T) {
    const size_t N = bel_prev.size();
    std::vector<double> bel_bar(N, 0.0);
    for (size_t i = 0; i < N; ++i) {
        double s = 0.0;
        for (size_t j = 0; j < N; ++j) s += T[i][j] * bel_prev[j];
        bel_bar[i] = s;
    }
    return normalize(bel_bar);
}

static std::vector<double> update(const std::vector<double>& bel_bar,
                                  const std::vector<double>& likelihood) {
    const size_t N = bel_bar.size();
    std::vector<double> bel(N, 0.0);
    for (size_t i = 0; i < N; ++i) bel[i] = likelihood[i] * bel_bar[i];
    return normalize(bel);
}

static std::vector<std::vector<double>> make_shift_transition(size_t N, int delta,
                                                              double p_exact = 0.8,
                                                              double p_under = 0.1,
                                                              double p_over  = 0.1) {
    std::vector<std::vector<double>> T(N, std::vector<double>(N, 0.0));
    auto modN = [N](long long k) -> size_t {
        long long r = k % static_cast<long long>(N);
        if (r < 0) r += static_cast<long long>(N);
        return static_cast<size_t>(r);
    };
    for (size_t j = 0; j < N; ++j) {
        size_t i_exact = modN(static_cast<long long>(j) + delta);
        size_t i_under = modN(static_cast<long long>(j) + delta - 1);
        size_t i_over  = modN(static_cast<long long>(j) + delta + 1);
        T[i_exact][j] += p_exact;
        T[i_under][j] += p_under;
        T[i_over][j]  += p_over;
    }
    return T;
}

static std::vector<double> landmark_likelihood(size_t N, int z,
                                               const std::vector<int>& landmarks,
                                               double p_hit = 0.9,
                                               double p_false = 0.1) {
    std::vector<double> L(N, (z == 1) ? p_false : (1.0 - p_false));
    for (int lm : landmarks) {
        size_t x = static_cast<size_t>(((lm % static_cast<int>(N)) + static_cast<int>(N)) % static_cast<int>(N));
        L[x] = (z == 1) ? p_hit : (1.0 - p_hit);
    }
    return L;
}

int main() {
    const size_t N = 20;
    std::vector<double> bel(N, 1.0 / static_cast<double>(N));
    std::vector<int> landmarks = {3, 14};
    std::vector<int> controls  = {1, 1, 1, 2, 1, 1, 2, 1};

    std::mt19937 rng(4);
    std::uniform_real_distribution<double> unif(0.0, 1.0);
    std::uniform_int_distribution<int> unif_state(0, static_cast<int>(N) - 1);

    int x = unif_state(rng);

    // Simulate measurements
    std::vector<int> zs;
    for (int u : controls) {
        double r = unif(rng);
        if (r < 0.8) x = (x + u) % static_cast<int>(N);
        else if (r < 0.9) x = (x + u - 1 + static_cast<int>(N)) % static_cast<int>(N);
        else x = (x + u + 1) % static_cast<int>(N);

        bool at_landmark = (x == landmarks[0] || x == landmarks[1]);
        int z = 0;
        if (at_landmark) z = (unif(rng) < 0.9) ? 1 : 0;
        else z = (unif(rng) < 0.1) ? 1 : 0;
        zs.push_back(z);
    }

    // Filter
    for (size_t t = 0; t < controls.size(); ++t) {
        auto T = make_shift_transition(N, controls[t]);
        auto L = landmark_likelihood(N, zs[t], landmarks);
        bel = update(predict(bel, T), L);
    }

    // Report top-5
    std::vector<size_t> idx(N);
    std::iota(idx.begin(), idx.end(), 0);
    std::sort(idx.begin(), idx.end(), [&](size_t a, size_t b){ return bel[a] > bel[b]; });

    std::cout << "Final belief (top 5 states):\n";
    for (int k = 0; k < 5; ++k) {
        size_t i = idx[k];
        std::cout << "  state " << std::setw(2) << i << ": " << std::fixed << std::setprecision(4) << bel[i] << "\n";
    }
    return 0;
}
      

Java appears in some industrial AMR stacks (especially enterprise integrations). Numerically, one may use Apache Commons Math; here we implement arrays directly to emphasize the Bayes recursion mechanics.

File: Chapter6_Lesson2.java

/*
Chapter6_Lesson2.java
Bayes Filters for Mobile Robots (discrete-state version)
*/

import java.util.Arrays;
import java.util.Random;

public class Chapter6_Lesson2 {

    static double[] normalize(double[] p) {
        double s = 0.0;
        for (double v : p) s += v;
        if (s < 1e-12) {
            double[] u = new double[p.length];
            Arrays.fill(u, 1.0 / p.length);
            return u;
        }
        double[] out = new double[p.length];
        for (int i = 0; i < p.length; i++) out[i] = p[i] / s;
        return out;
    }

    static double[] predict(double[] belPrev, double[][] T) {
        int N = belPrev.length;
        double[] belBar = new double[N];
        for (int i = 0; i < N; i++) {
            double s = 0.0;
            for (int j = 0; j < N; j++) s += T[i][j] * belPrev[j];
            belBar[i] = s;
        }
        return normalize(belBar);
    }

    static double[] update(double[] belBar, double[] L) {
        int N = belBar.length;
        double[] bel = new double[N];
        for (int i = 0; i < N; i++) bel[i] = L[i] * belBar[i];
        return normalize(bel);
    }

    static int mod(int a, int n) {
        int r = a % n;
        return (r < 0) ? r + n : r;
    }

    static double[][] makeShiftTransition(int N, int delta, double pExact, double pUnder, double pOver) {
        double[][] T = new double[N][N];
        for (int j = 0; j < N; j++) {
            int iExact = mod(j + delta, N);
            int iUnder = mod(j + delta - 1, N);
            int iOver  = mod(j + delta + 1, N);
            T[iExact][j] += pExact;
            T[iUnder][j] += pUnder;
            T[iOver][j]  += pOver;
        }
        return T;
    }

    static double[] landmarkLikelihood(int N, int z, int[] landmarks, double pHit, double pFalse) {
        double[] L = new double[N];
        double base = (z == 1) ? pFalse : (1.0 - pFalse);
        Arrays.fill(L, base);
        for (int lm : landmarks) {
            int x = mod(lm, N);
            L[x] = (z == 1) ? pHit : (1.0 - pHit);
        }
        return L;
    }

    public static void main(String[] args) {
        int N = 20;
        double[] bel = new double[N];
        Arrays.fill(bel, 1.0 / N);

        int[] landmarks = new int[]{3, 14};
        int[] controls  = new int[]{1, 1, 1, 2, 1, 1, 2, 1};

        Random rng = new Random(4);
        int x = rng.nextInt(N);

        int[] zs = new int[controls.length];
        for (int t = 0; t < controls.length; t++) {
            int u = controls[t];
            double r = rng.nextDouble();
            if (r < 0.8) x = mod(x + u, N);
            else if (r < 0.9) x = mod(x + u - 1, N);
            else x = mod(x + u + 1, N);

            boolean atLandmark = (x == landmarks[0] || x == landmarks[1]);
            int z;
            if (atLandmark) z = (rng.nextDouble() < 0.9) ? 1 : 0;
            else z = (rng.nextDouble() < 0.1) ? 1 : 0;
            zs[t] = z;
        }

        for (int t = 0; t < controls.length; t++) {
            double[][] T = makeShiftTransition(N, controls[t], 0.8, 0.1, 0.1);
            double[] L = landmarkLikelihood(N, zs[t], landmarks, 0.9, 0.1);
            bel = update(predict(bel, T), L);
        }

        Integer[] idx = new Integer[N];
        for (int i = 0; i < N; i++) idx[i] = i;
        Arrays.sort(idx, (a, b) -> Double.compare(bel[b], bel[a]));

        System.out.println("Final belief (top 5 states):");
        for (int k = 0; k < 5; k++) {
            int i = idx[k];
            System.out.printf("  state %2d: %.4f%n", i, bel[i]);
        }
    }
}
      

MATLAB is common for algorithm prototyping and for rapid verification. In deployment-like workflows, the belief update can be embedded into a Simulink model via a MATLAB Function block that executes the prediction and correction steps at each sample time. Below we provide a MATLAB reference implementation (and a corridor demo).

File: Chapter6_Lesson2.m

% Chapter6_Lesson2.m
% Bayes Filters for Mobile Robots (discrete-state version)

function Chapter6_Lesson2()
    rng(4);

    N = 20;
    bel = ones(N,1) / N;

    landmarks = [3; 14]; % 0-indexed landmark positions
    controls  = [1 1 1 2 1 1 2 1];

    x = randi([0, N-1]);
    zs = zeros(numel(controls),1);

    % Simulate measurements from truth
    for t = 1:numel(controls)
        u = controls(t);
        r = rand();
        if r < 0.8
            x = mod(x + u, N);
        elseif r < 0.9
            x = mod(x + u - 1, N);
        else
            x = mod(x + u + 1, N);
        end

        at_landmark = any(x == mod(landmarks, N));
        if at_landmark
            z = double(rand() < 0.9);
        else
            z = double(rand() < 0.1);
        end
        zs(t) = z;
    end

    % Filter
    for t = 1:numel(controls)
        u = controls(t);
        z = zs(t);

        T = make_shift_transition(N, u, 0.8, 0.1, 0.1);
        L = landmark_likelihood(N, z, landmarks, 0.9, 0.1);

        bel_bar = normalize(T * bel);
        bel = normalize(L .* bel_bar);
    end

    [~, idx] = sort(bel, 'descend');
    fprintf('Final belief (top 5 states):\n');
    for k = 1:5
        i = idx(k) - 1;
        fprintf('  state %2d: %.4f\n', i, bel(idx(k)));
    end
end

function p = normalize(p)
    s = sum(p);
    if s < 1e-12
        p = ones(size(p)) / numel(p);
    else
        p = p / s;
    end
end

function T = make_shift_transition(N, delta, p_exact, p_under, p_over)
    T = zeros(N, N);
    for j = 0:(N-1)
        i_exact = mod(j + delta, N);
        i_under = mod(j + delta - 1, N);
        i_over  = mod(j + delta + 1, N);
        T(i_exact+1, j+1) = T(i_exact+1, j+1) + p_exact;
        T(i_under+1, j+1) = T(i_under+1, j+1) + p_under;
        T(i_over+1,  j+1) = T(i_over+1,  j+1) + p_over;
    end
end

function L = landmark_likelihood(N, z, landmarks, p_hit, p_false)
    if z == 1
        base = p_false;
    else
        base = 1.0 - p_false;
    end
    L = base * ones(N, 1);
    for k = 1:numel(landmarks)
        x = mod(landmarks(k), N);
        if z == 1
            L(x+1) = p_hit;
        else
            L(x+1) = 1.0 - p_hit;
        end
    end
end
      

Simulink embedding guideline (conceptual): implement \( \mathbf{b}_{t-1} \) as a unit delay state; compute \( \overline{\mathbf{b}}_t=\mathbf{T}_t\mathbf{b}_{t-1} \) and \( \mathbf{b}_t=\eta(\boldsymbol{\ell}_t \odot \overline{\mathbf{b}}_t) \) inside a MATLAB Function block; feed in \( u_t \), \( z_t \) as inputs.

Mathematica is convenient for symbolic manipulation and rapid experimentation with probabilistic recursions. The code below implements the discrete prediction and update steps and runs the same 1D corridor demo.

File: Chapter6_Lesson2.nb

(* Chapter6_Lesson2.nb
   Bayes Filters for Mobile Robots (discrete-state version)
*)

Normalize[p_] := Module[{s = Total[p]},
  If[s < 10^-12, ConstantArray[1/Length[p], Length[p]], p/s]
];

Predict[belPrev_, T_] := Normalize[T . belPrev];
Update[belBar_, L_] := Normalize[L * belBar];

MakeShiftTransition[N_, delta_, pExact_, pUnder_, pOver_] := Module[{T, mod},
  T = ConstantArray[0.0, {N, N}];
  mod = Function[k, Mod[k, N]];
  Do[
    T[[mod[j + delta] + 1, j + 1]] += pExact;
    T[[mod[j + delta - 1] + 1, j + 1]] += pUnder;
    T[[mod[j + delta + 1] + 1, j + 1]] += pOver;
    ,
    {j, 0, N - 1}
  ];
  T
];

LandmarkLikelihood[N_, z_, landmarks_, pHit_, pFalse_] := Module[{L, base},
  base = If[z == 1, pFalse, 1.0 - pFalse];
  L = ConstantArray[base, N];
  Do[
    L[[Mod[lm, N] + 1]] = If[z == 1, pHit, 1.0 - pHit];
    ,
    {lm, landmarks}
  ];
  L
];

SeedRandom[4];
N = 20;
bel = ConstantArray[1.0/N, N];
landmarks = {3, 14};
controls = {1, 1, 1, 2, 1, 1, 2, 1};
x = RandomInteger[{0, N - 1}];
zs = {};

Do[
  u = controls[[t]];
  r = RandomReal[];
  x = If[r < 0.8, Mod[x + u, N], If[r < 0.9, Mod[x + u - 1, N], Mod[x + u + 1, N]]];
  atLm = MemberQ[landmarks, x];
  z = If[atLm, If[RandomReal[] < 0.9, 1, 0], If[RandomReal[] < 0.1, 1, 0]];
  zs = Append[zs, z];
  ,
  {t, 1, Length[controls]}
];

Do[
  u = controls[[t]];
  z = zs[[t]];
  T = MakeShiftTransition[N, u, 0.8, 0.1, 0.1];
  L = LandmarkLikelihood[N, z, landmarks, 0.9, 0.1];
  bel = Update[Predict[bel, T], L];
  ,
  {t, 1, Length[controls]}
];

Print["Final belief top-5 states:"];
idx = Reverse[Ordering[bel]];
Do[
  Print["state ", idx[[k]] - 1, " : ", NumberForm[bel[[idx[[k]]]], {Infinity, 4}]];
  ,
  {k, 1, 5}
];
      

Problem 1 (Derive the Bayes filter recursion): Starting from \( bel(x_t)=p(x_t \mid z_{1:t},u_{1:t}) \), use Bayes’ rule and the law of total probability to derive the recursion \( bel(x_t)=\eta\,p(z_t\mid x_t)\int p(x_t\mid x_{t-1},u_t)\,bel(x_{t-1})\,dx_{t-1} \) under Assumptions A–B.

Solution: Apply Bayes’ rule:

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

By Assumption B, \( p(z_t \mid x_t, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t) \). Then marginalize:

\[ 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}. \]

Apply Assumption A and remove the irrelevant conditioning on \( u_t \) for the already-realized \( x_{t-1} \), yielding:

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

Substitute back to get the recursion. \( \square \)

Problem 2 (Normalization constant): Show that choosing \( \eta^{-1} = \int p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_t) bel(x_{t-1}) dx_{t-1} dx_t \) ensures \( \int bel(x_t)\,dx_t=1 \).

Solution: Define the unnormalized posterior:

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

Then \( bel(x_t)=\eta \tilde{bel}(x_t) \). Integrate:

\[ \int bel(x_t)\,dx_t \;=\; \eta \int \tilde{bel}(x_t)\,dx_t \;=\; \eta \, \eta^{-1} \;=\; 1. \]

Problem 3 (Discrete matrix form): For a finite state space, prove that the Bayes filter can be written as \( \mathbf{b}_t = \eta (\boldsymbol{\ell}_t \odot (\mathbf{T}_t\mathbf{b}_{t-1})) \), and show that if \( \mathbf{T}_t \) is column-stochastic and \( \mathbf{b}_{t-1} \) is a probability vector, then \( \mathbf{T}_t\mathbf{b}_{t-1} \) is a probability vector.

Solution: The sum form is:

\[ bel(i) \;=\; \eta \, \ell(i) \sum_j T_{ij} b_{t-1}(j). \]

This is precisely \( \mathbf{b}_t=\eta(\boldsymbol{\ell}_t \odot (\mathbf{T}_t\mathbf{b}_{t-1})) \). For probability preservation of prediction:

\[ \mathbf{1}^\top(\mathbf{T}_t\mathbf{b}_{t-1}) \;=\; (\mathbf{1}^\top \mathbf{T}_t)\mathbf{b}_{t-1} \;=\; \mathbf{1}^\top \mathbf{b}_{t-1} \;=\; 1, \]

where \( \mathbf{1}^\top\mathbf{T}_t=\mathbf{1}^\top \) holds for column-stochastic \( \mathbf{T}_t \). Nonnegativity is immediate from nonnegative entries. \( \square \)

Problem 4 (Complexity of prediction): If \( \mathbf{T}_t \) is dense, show prediction costs \( \mathcal{O}(N^2) \). If each column has at most \( k \) nonzeros, show prediction costs \( \mathcal{O}(Nk) \).

Solution: Dense multiply computes each of \( N \) outputs as a sum over \( N \) terms, costing \( N\cdot N \) multiply-adds. If each column contains at most \( k \) nonzero transitions (local motion), each output accumulates at most \( k \) contributions per previous-state column, giving \( \mathcal{O}(Nk) \) operations.

Problem 5 (Numerical underflow and log domain): Suppose likelihoods are extremely small so that floating-point multiplication underflows. Define \( s_i = \log \ell_i + \log \overline{b}_i \). Show that a numerically stable normalization can be computed via:

\[ b_i \;=\; \frac{\exp(s_i - s_{\max})}{\sum_j \exp(s_j - s_{\max})}, \quad s_{\max} = \max_j s_j. \]

Solution: The unnormalized posterior is \( \tilde{b}_i=\ell_i \overline{b}_i \), hence \( \log \tilde{b}_i = s_i \). Subtracting \( s_{\max} \) does not change the normalized ratio because it cancels between numerator and denominator, but prevents exponentials from underflowing. \( \square \)

We derived the Bayes filter recursion from conditional independence and marginalization, proved that prediction and correction preserve probabilistic validity (with normalization), expressed the recursion in discrete matrix form, and implemented a discrete Bayes filter loop across multiple languages. This recursion is the foundational estimator that later chapters instantiate with specific representations and approximations.