In Lesson 2 (EKF-SLAM), we represented the joint state as \( \mathbf{s}_t = [\mathbf{x}_t^\top, \mathbf{m}^\top]^\top \), where \( \mathbf{x}_t \) is robot pose and \( \mathbf{m} \) concatenates all landmark states. The EKF maintained a dense covariance across all landmarks, yielding per-step update cost that grows roughly as \( \mathcal{O}(M^2) \) or worse (depending on implementation and number of observed landmarks), and it is sensitive to linearization error and inconsistency when the posterior is multi-modal.

FastSLAM addresses these issues by:

• Representing the robot trajectory distribution with particles: \( \mathbf{x}_{1:t}^{(i)} \) (path hypothesis).
• Conditioning on each sampled path, representing each landmark posterior with an EKF (FastSLAM 1.0) or a small PF (FastSLAM 2.0 variants).
• Exploiting a conditional independence structure that makes landmark posteriors independent given the path.

The crucial idea is Rao–Blackwellization: if a joint posterior factors as \( p(A,B \mid \cdot) = p(A \mid \cdot)\, p(B \mid A, \cdot) \), and we can integrate/compute \( p(B \mid A,\cdot) \) analytically, then sampling only \( A \) reduces Monte Carlo variance and often reduces computational burden. In SLAM, the “hard” part is the robot path; the “easier” part becomes landmark estimation conditional on that path.

We will assume (for this lesson) that each measurement is already associated with a landmark ID; the probabilistic consequences of wrong association and how to manage it will be introduced in Lesson 4.

Let: \( \mathbf{x}_{1:t} = (\mathbf{x}_1,\dots,\mathbf{x}_t) \) be the robot trajectory, \( \mathbf{m} = \{ \mathbf{m}_1,\dots,\mathbf{m}_M \} \) the map landmarks, controls \( \mathbf{u}_{1:t} \), and observations \( \mathbf{z}_{1:t} \). The full SLAM posterior is:

\[ p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}). \]

FastSLAM relies on the following factorization:

\[ p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) = p(\mathbf{x}_{1:t} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \prod_{k=1}^{M} p(\mathbf{m}_k \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}). \]

The product form says: given the robot path \( \mathbf{x}_{1:t} \), the landmarks become conditionally independent. This is not magic; it is a consequence of:

• Landmarks are static and do not influence each other (no map dynamics coupling).
• Measurements at time \( t \) depend on \( \mathbf{x}_t \) and the observed landmark only.
• Given the entire trajectory, each landmark’s posterior depends only on the subset of observations that reference it.

2.1 Proof sketch (conditional independence)

Assume each measurement \( \mathbf{z}_t \) decomposes into components \( \mathbf{z}_t^{(k)} \) that refer to landmark \( k \) (or empty if unseen), and the measurement model factorizes:

\[ p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) = \prod_{k=1}^{M} p(\mathbf{z}_t^{(k)} \mid \mathbf{x}_t, \mathbf{m}_k). \]

Using Bayes rule and the Markov motion model:

\[ p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \propto p(\mathbf{x}_{1:t} \mid \mathbf{u}_{1:t}) \; p(\mathbf{m}) \; \prod_{\tau=1}^{t} p(\mathbf{z}_\tau \mid \mathbf{x}_\tau, \mathbf{m}). \]

If the prior over landmarks factorizes as independent: \( p(\mathbf{m}) = \prod_{k=1}^{M} p(\mathbf{m}_k) \), then substituting the factorized likelihood gives:

\[ p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \propto p(\mathbf{x}_{1:t} \mid \mathbf{u}_{1:t}) \prod_{k=1}^{M} \left( p(\mathbf{m}_k)\prod_{\tau=1}^{t} p(\mathbf{z}_\tau^{(k)} \mid \mathbf{x}_\tau, \mathbf{m}_k) \right). \]

Now, conditioning on \( \mathbf{x}_{1:t} \) makes the term in parentheses depend only on landmark \( k \), hence the landmark posteriors separate, yielding: \( p(\mathbf{m} \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) = \prod_k p(\mathbf{m}_k \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) \).

2.2 Rao–Blackwellization: why it reduces variance

Let \( \theta = (\theta_A,\theta_B) \) be a latent variable (here: \( \theta_A=\mathbf{x}_{1:t} \), \( \theta_B=\mathbf{m} \)). For any integrable function \( f(\theta_A,\theta_B) \), define the Rao–Blackwellized estimator:

\[ \hat{I}_{RB} = \frac{1}{N}\sum_{i=1}^{N} \mathbb{E}\big[f(\theta_A^{(i)},\theta_B)\mid \theta_A^{(i)}, \text{data}\big], \quad \theta_A^{(i)} \sim p(\theta_A\mid \text{data}). \]

Compared to the naive Monte Carlo estimator that samples both \( \theta_A \) and \( \theta_B \), the law of total variance implies:

\[ \operatorname{Var}\!\left(\mathbb{E}[f(\theta_A,\theta_B)\mid \theta_A]\right) \le \operatorname{Var}\!\left(f(\theta_A,\theta_B)\right). \]

Therefore, integrating (analytically) the landmark uncertainty conditional on each trajectory typically yields more stable estimates than sampling landmarks directly.

FastSLAM 1.0 maintains \( N \) particles. Particle \( i \) at time \( t \) stores:

• Trajectory (or current pose only, plus enough history to update landmarks): \( \mathbf{x}_t^{(i)} \) and conceptually \( \mathbf{x}_{1:t}^{(i)} \).
• Particle weight \( w_t^{(i)} \).
• A set of landmark EKFs: for each landmark \( k \), mean \( \boldsymbol{\mu}_{k,t}^{(i)} \) and covariance \( \mathbf{\Sigma}_{k,t}^{(i)} \).

3.1 Motion model sampling

We use a (unicycle) motion model: \( \mathbf{x}_t = g(\mathbf{x}_{t-1}, \mathbf{u}_t) + \boldsymbol{\epsilon}_t \), with \( \boldsymbol{\epsilon}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_t) \). Each particle samples: \( \mathbf{x}_t^{(i)} \sim p(\mathbf{x}_t \mid \mathbf{x}_{t-1}^{(i)}, \mathbf{u}_t) \).

3.2 Landmark EKF update given a particle pose

For a landmark \( \mathbf{m}_k \in \mathbb{R}^2 \) and a range-bearing measurement \( \mathbf{z}_{t} = [r_t,\phi_t]^\top \):

\[ \mathbf{z}_t = h(\mathbf{x}_t, \mathbf{m}_k) + \mathbf{v}_t,\quad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_t). \]

Define \( \hat{\mathbf{z} }_t = h(\mathbf{x}_t^{(i)}, \boldsymbol{\mu}_{k,t-1}^{(i)}) \), innovation \( \mathbf{\nu}_t = \mathbf{z}_t - \hat{\mathbf{z} }_t \) (bearing wrapped), and Jacobian \( \mathbf{H}_t = \frac{\partial h}{\partial \mathbf{m} } \big|_{\mathbf{x}_t^{(i)}, \boldsymbol{\mu}_{k,t-1}^{(i)} } \). Then the EKF equations are:

\[ \mathbf{S}_t = \mathbf{H}_t \mathbf{\Sigma}_{k,t-1}^{(i)} \mathbf{H}_t^\top + \mathbf{Q}_t,\quad \mathbf{K}_t = \mathbf{\Sigma}_{k,t-1}^{(i)} \mathbf{H}_t^\top \mathbf{S}_t^{-1}, \]

\[ \boldsymbol{\mu}_{k,t}^{(i)} = \boldsymbol{\mu}_{k,t-1}^{(i)} + \mathbf{K}_t \mathbf{\nu}_t,\quad \mathbf{\Sigma}_{k,t}^{(i)} = (\mathbf{I} - \mathbf{K}_t \mathbf{H}_t)\mathbf{\Sigma}_{k,t-1}^{(i)}. \]

3.3 Importance weights (measurement likelihood)

In FastSLAM 1.0, we typically use the proposal \( q(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t, \mathbf{z}_t) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t) \), i.e., sample from motion only. Then the importance weight update simplifies to the measurement likelihood:

\[ w_t^{(i)} \propto w_{t-1}^{(i)} \, p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}, \mathbf{m}^{(i)}). \]

Under conditional landmark independence and known association, if at time \( t \) we observe a set \( \mathcal{K}_t \subseteq \{1,\dots,M\} \) of landmarks, then:

\[ p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}, \mathbf{m}^{(i)}) = \prod_{k \in \mathcal{K}_t} p(\mathbf{z}_t^{(k)} \mid \mathbf{x}_t^{(i)}, \mathbf{m}_k^{(i)}). \]

With Gaussian EKF predictive distribution: \( \mathbf{\nu}_t^{(k)} \sim \mathcal{N}(\mathbf{0}, \mathbf{S}_t^{(k)}) \), the per-landmark likelihood becomes:

\[ p(\mathbf{z}_t^{(k)} \mid \mathbf{x}_t^{(i)}, \mathbf{m}_k^{(i)}) \approx \frac{1}{\sqrt{(2\pi)^2|\mathbf{S}_t^{(k)}|} } \exp\!\left(-\tfrac{1}{2}(\mathbf{\nu}_t^{(k)})^\top (\mathbf{S}_t^{(k)})^{-1}\mathbf{\nu}_t^{(k)}\right). \]

3.4 Resampling and degeneracy

Like standard particle filters (Chapter 8), FastSLAM can suffer from weight degeneracy. A common criterion uses the effective sample size:

\[ N_{\text{eff} } = \frac{1}{\sum_{i=1}^{N} (w_t^{(i)})^2}. \]

When \( N_{\text{eff} } < \alpha N \) (e.g., \( \alpha = 0.5 \)), resample particles (systematic / stratified / multinomial).

graph TD
  A[Inputs] --> B[Sample pose for each particle]
  B --> C[Update landmark EKFs]
  C --> D[Update particle weights]
  D --> E[Normalize weights]
  E --> F[Compute Neff]
  F --> G{Neff low}
  G --> H[Resample particles]
  G --> I[Keep particles]
  H --> J[Output estimate]
  I --> J
  

For a 2D pose \( \mathbf{x}_t = [x_t, y_t, \theta_t]^\top \) and landmark \( \mathbf{m}_k = [m_x, m_y]^\top \), define: \( \Delta x = m_x - x_t \), \( \Delta y = m_y - y_t \), and \( q = (\Delta x)^2 + (\Delta y)^2 \). The measurement function is:

\[ h(\mathbf{x}_t,\mathbf{m}_k) = \begin{bmatrix} \sqrt{q} \\ \operatorname{atan2}(\Delta y,\Delta x) - \theta_t \end{bmatrix}. \]

The EKF in FastSLAM 1.0 typically linearizes w.r.t. the landmark state, so we need: \( \mathbf{H}_t = \frac{\partial h}{\partial \mathbf{m} } \). Using standard derivatives:

\[ \mathbf{H}_t = \begin{bmatrix} \frac{\Delta x}{\sqrt{q} } & \frac{\Delta y}{\sqrt{q} } \\ -\frac{\Delta y}{q} & \frac{\Delta x}{q} \end{bmatrix}. \]

Initialization. If landmark \( k \) is first observed with measurement \( \mathbf{z}_t = [r,\phi]^\top \) (bearing relative to robot heading), then the inverse sensor model gives an initial mean:

\[ \boldsymbol{\mu}_{k,t}^{(i)} = \begin{bmatrix} x_t^{(i)} + r\cos(\theta_t^{(i)} + \phi) \\ y_t^{(i)} + r\sin(\theta_t^{(i)} + \phi) \end{bmatrix}. \]

A common approximation for the initial covariance uses \( \mathbf{\Sigma} \approx \mathbf{J}\mathbf{Q}\mathbf{J}^\top \), where \( \mathbf{J} = \left(\frac{\partial h}{\partial \mathbf{m} }\right)^{-1} \) evaluated at the initialized landmark (equivalently \( \mathbf{J} = \frac{\partial \mathbf{m} }{\partial \mathbf{z} } \)).

flowchart TD
  A["SLAM posterior"] --> B["Sample only robot path x_1:t with particles"]
  B --> C["Condition on path -> each landmark independent"]
  C --> D["For each particle: run M small EKFs for landmarks"]
  D --> E["Map estimate is mixture over particle maps"]
        

Computational scaling. If at time \( t \) you observe \( K_t \) landmarks, FastSLAM 1.0 updates only those EKFs in each particle, giving per-step complexity approximately: \( \mathcal{O}(N K_t) \) (each EKF update is constant-size in 2D landmark state). This is a major advantage over EKF-SLAM’s global covariance updates.

Path degeneracy. Even when landmark estimation is analytic, particle sets can collapse to few path hypotheses. This motivates better proposals (FastSLAM 2.0 uses measurement-informed proposals) and good resampling strategies.

Consistency vs linearization. Linearization remains in the landmark EKFs, but the high-dimensional coupling of EKF-SLAM is removed, and the particle approximation can represent multi-modality in robot trajectory.

What we are intentionally not doing yet. Data association uncertainty can dominate performance in real AMR environments; we postpone it to Lesson 4 to keep the probabilistic derivation clean.

The following Python code implements FastSLAM 1.0 for a 2D unicycle robot with range-bearing landmarks and known association. It includes a tiny simulator to generate measurements and a systematic resampler. Libraries you should know in robotics Python stacks: numpy, scipy (optional), matplotlib.

# Chapter11_Lesson3.py
"""
FastSLAM 1.0 (Rao–Blackwellized Particle Filter SLAM) — minimal educational implementation.

Assumptions (kept simple for teaching):
- 2D planar robot with state x = [px, py, theta]
- Known data association (each observation includes landmark id)
- Range-bearing landmark sensor with Gaussian noise
- Motion model: unicycle with noisy controls

This script:
1) Simulates a small world with point landmarks.
2) Runs FastSLAM 1.0 with N particles, each maintaining per-landmark EKF.
3) Plots ground-truth and estimated trajectory.

Dependencies: numpy, matplotlib (optional for plotting)
"""

from __future__ import annotations
import math
import numpy as np

try:
    import matplotlib.pyplot as plt
    _HAS_PLT = True
except Exception:
    _HAS_PLT = False

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

def motion_model(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Deterministic unicycle step."""
    px, py, th = x
    v, w = u
    if abs(w) < 1e-9:
        px2 = px + v * dt * math.cos(th)
        py2 = py + v * dt * math.sin(th)
        th2 = th
    else:
        px2 = px + (v / w) * (math.sin(th + w * dt) - math.sin(th))
        py2 = py - (v / w) * (math.cos(th + w * dt) - math.cos(th))
        th2 = th + w * dt
    return np.array([px2, py2, wrap_angle(th2)], dtype=float)

def noisy_motion(x: np.ndarray, u: np.ndarray, dt: float, R_u: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Sample a motion step by perturbing controls (v,w)."""
    u_noisy = u + rng.multivariate_normal(mean=np.zeros(2), cov=R_u)
    return motion_model(x, u_noisy, dt)

def h_landmark(x: np.ndarray, m: np.ndarray) -> np.ndarray:
    """Range-bearing measurement model to landmark m in world frame."""
    px, py, th = x
    dx = m[0] - px
    dy = m[1] - py
    q = dx*dx + dy*dy
    r = math.sqrt(q)
    b = wrap_angle(math.atan2(dy, dx) - th)
    return np.array([r, b], dtype=float)

def H_landmark(x: np.ndarray, m: np.ndarray) -> np.ndarray:
    """Jacobian dh/dm for landmark position m=[mx,my]."""
    px, py, th = x
    dx = m[0] - px
    dy = m[1] - py
    q = dx*dx + dy*dy
    r = math.sqrt(q)
    # Avoid division by zero
    q = max(q, 1e-12)
    r = max(r, 1e-6)
    return np.array([[dx/r, dy/r],
                     [-dy/q, dx/q]], dtype=float)

def inv_measurement(x: np.ndarray, z: np.ndarray) -> np.ndarray:
    """Inverse sensor model: given pose x and measurement z=[r,b], return landmark position."""
    px, py, th = x
    r, b = float(z[0]), float(z[1])
    ang = th + b
    mx = px + r * math.cos(ang)
    my = py + r * math.sin(ang)
    return np.array([mx, my], dtype=float)

def gaussian_likelihood(v: np.ndarray, S: np.ndarray) -> float:
    """Evaluate N(v;0,S) with small numerical safeguards."""
    # 2D only
    S = 0.5 * (S + S.T)
    detS = float(np.linalg.det(S))
    detS = max(detS, 1e-18)
    invS = np.linalg.inv(S)
    e = float(v.T @ invS @ v)
    norm = 1.0 / (2.0 * math.pi * math.sqrt(detS))
    return norm * math.exp(-0.5 * e)

class LandmarkEKF:
    def __init__(self):
        self.seen = False
        self.mu = np.zeros(2, dtype=float)
        self.Sigma = np.eye(2, dtype=float) * 1e6

class Particle:
    def __init__(self, n_landmarks: int):
        self.x = np.zeros(3, dtype=float)
        self.w = 1.0
        self.lms = [LandmarkEKF() for _ in range(n_landmarks)]

def systematic_resample(particles: list[Particle], rng: np.random.Generator) -> list[Particle]:
    """Systematic resampling on normalized weights."""
    N = len(particles)
    ws = np.array([p.w for p in particles], dtype=float)
    ws = ws / max(ws.sum(), 1e-12)
    cdf = np.cumsum(ws)

    u0 = rng.uniform(0.0, 1.0 / N)
    idxs = []
    j = 0
    for i in range(N):
        u = u0 + i / N
        while u > cdf[j]:
            j += 1
            if j >= N:
                j = N - 1
                break
        idxs.append(j)

    # Deep copy particles (including landmark EKFs)
    new_ps = []
    for j in idxs:
        pj = particles[j]
        pnew = Particle(len(pj.lms))
        pnew.x = pj.x.copy()
        pnew.w = 1.0 / N
        for k, lm in enumerate(pj.lms):
            pnew.lms[k].seen = lm.seen
            pnew.lms[k].mu = lm.mu.copy()
            pnew.lms[k].Sigma = lm.Sigma.copy()
        new_ps.append(pnew)
    return new_ps

def fastslam_1_step(particles: list[Particle],
                    u: np.ndarray,
                    z_list: list[tuple[int, np.ndarray]],
                    dt: float,
                    R_u: np.ndarray,
                    Q_z: np.ndarray,
                    rng: np.random.Generator) -> list[Particle]:
    """
    One FastSLAM 1.0 step:
    1) sample motion for each particle
    2) per particle: update per-landmark EKFs, and multiply importance weight by measurement likelihood
    3) normalize + resample if ESS low
    """
    N = len(particles)

    # 1) motion sampling
    for p in particles:
        p.x = noisy_motion(p.x, u, dt, R_u, rng)

    # 2) measurement updates
    for p in particles:
        for lm_id, z in z_list:
            lm = p.lms[lm_id]
            if not lm.seen:
                # initialize landmark EKF from inverse measurement
                lm.mu = inv_measurement(p.x, z)
                H = H_landmark(p.x, lm.mu)
                # Sigma = (H^{-1}) Q (H^{-1})^T
                J = np.linalg.inv(H)
                lm.Sigma = J @ Q_z @ J.T
                lm.seen = True
                continue

            zhat = h_landmark(p.x, lm.mu)
            v = np.array([z[0] - zhat[0], wrap_angle(z[1] - zhat[1])], dtype=float)
            H = H_landmark(p.x, lm.mu)
            S = H @ lm.Sigma @ H.T + Q_z
            K = lm.Sigma @ H.T @ np.linalg.inv(S)
            lm.mu = lm.mu + K @ v
            lm.Sigma = (np.eye(2) - K @ H) @ lm.Sigma

            # importance weight multiplier
            p.w *= gaussian_likelihood(v, S)

    # 3) normalize
    ws = np.array([p.w for p in particles], dtype=float)
    s = float(ws.sum())
    if s <= 1e-30:
        # catastrophic degeneracy; reset to uniform
        for p in particles:
            p.w = 1.0 / N
    else:
        for p in particles:
            p.w /= s

    # effective sample size
    ws = np.array([p.w for p in particles], dtype=float)
    ess = 1.0 / float(np.sum(ws * ws))
    if ess < 0.5 * N:
        particles = systematic_resample(particles, rng)

    return particles

def estimate_pose(particles: list[Particle]) -> np.ndarray:
    """Weighted mean pose (angle via circular mean)."""
    ws = np.array([p.w for p in particles], dtype=float)
    ws = ws / max(ws.sum(), 1e-12)
    xs = np.array([p.x for p in particles], dtype=float)
    px = float(np.sum(ws * xs[:, 0]))
    py = float(np.sum(ws * xs[:, 1]))
    # circular mean for theta
    c = float(np.sum(ws * np.cos(xs[:, 2])))
    s = float(np.sum(ws * np.sin(xs[:, 2])))
    th = math.atan2(s, c)
    return np.array([px, py, th], dtype=float)

def main():
    rng = np.random.default_rng(42)

    # World (landmarks)
    landmarks = np.array([[2.0, 2.0],
                          [8.0, 1.0],
                          [6.5, 7.0],
                          [1.0, 8.0],
                          [9.0, 9.0]], dtype=float)
    M = landmarks.shape[0]

    # Simulation settings
    T = 150
    dt = 0.1
    max_range = 7.0

    # True motion & sensor noise
    R_u = np.diag([0.05**2, (2.0*math.pi/180.0)**2])  # noise on [v,w]
    Q_z = np.diag([0.15**2, (3.0*math.pi/180.0)**2])  # noise on [range,bearing]

    # Control sequence: gentle turns
    controls = []
    for t in range(T):
        v = 0.8
        w = 0.25 * math.sin(0.05 * t)
        controls.append(np.array([v, w], dtype=float))

    # Generate ground truth + observations
    x_true = np.array([0.0, 0.0, 0.0], dtype=float)
    xs_true = [x_true.copy()]
    Z = []  # list per time: [(id, z), ...]
    for t in range(T):
        x_true = noisy_motion(x_true, controls[t], dt, R_u, rng)
        xs_true.append(x_true.copy())

        z_list = []
        for i in range(M):
            zhat = h_landmark(x_true, landmarks[i])
            if zhat[0] <= max_range:
                z = zhat + rng.multivariate_normal(mean=np.zeros(2), cov=Q_z)
                z[1] = wrap_angle(float(z[1]))
                z_list.append((i, z))
        Z.append(z_list)

    # FastSLAM particles
    N = 200
    particles = [Particle(M) for _ in range(N)]
    for p in particles:
        p.x = np.array([0.0, 0.0, 0.0], dtype=float)
        p.w = 1.0 / N

    xs_est = []
    for t in range(T):
        particles = fastslam_1_step(particles, controls[t], Z[t], dt, R_u, Q_z, rng)
        xs_est.append(estimate_pose(particles))

    xs_true = np.array(xs_true[1:], dtype=float)
    xs_est = np.array(xs_est, dtype=float)

    print("Final true pose:", xs_true[-1])
    print("Final est  pose:", xs_est[-1])

    # Extract best particle's landmark map for visualization
    best = max(particles, key=lambda p: p.w)
    est_lms = []
    for i in range(M):
        if best.lms[i].seen:
            est_lms.append(best.lms[i].mu.copy())
        else:
            est_lms.append(np.array([np.nan, np.nan]))
    est_lms = np.array(est_lms, dtype=float)

    if _HAS_PLT:
        plt.figure()
        plt.plot(xs_true[:, 0], xs_true[:, 1], label="true traj")
        plt.plot(xs_est[:, 0], xs_est[:, 1], label="FastSLAM mean traj")
        plt.scatter(landmarks[:, 0], landmarks[:, 1], marker="x", label="true landmarks")
        plt.scatter(est_lms[:, 0], est_lms[:, 1], marker="o", label="best particle landmarks")
        plt.axis("equal")
        plt.grid(True)
        plt.legend()
        plt.title("FastSLAM 1.0 demo (known data association)")
        plt.show()
    else:
        print("matplotlib not available; skipping plot.")

if __name__ == "__main__":
    main()

Notes for students:

• This is FastSLAM 1.0: proposal = motion model. It works but can be inefficient when measurements are informative (weights become peaky).
• Each particle stores its own landmark EKFs; this is the Rao–Blackwellization structure.
• We used known landmark IDs to isolate the RBPF mechanics. Data association is addressed in Lesson 4.

In C++, the most common linear algebra library for robotics is Eigen. The code below implements the FastSLAM 1.0 update loop (motion sampling, landmark EKFs, weights, and systematic resampling). Hook the observation list to your simulator (or later, a ROS2 pipeline).

// Chapter11_Lesson3.cpp
/*
FastSLAM 1.0 (Rao–Blackwellized PF-SLAM) — compact C++ reference implementation.

Educational notes:
- 2D pose: (x,y,theta)
- Known landmark IDs in observations
- Each particle maintains EKF (mean+cov) per landmark
- Uses Eigen for linear algebra (recommended for robotics)

Build (example):
  g++ -O2 -std=c++17 Chapter11_Lesson3.cpp -I /usr/include/eigen3 -o fastslam_demo

This file focuses on the algorithmic core (no ROS, no visualization).
*/

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

static double wrapAngle(double a) {
  while (a <= -M_PI) a += 2.0 * M_PI;
  while (a > M_PI) a -= 2.0 * M_PI;
  return a;
}

struct LandmarkEKF {
  bool seen = false;
  Eigen::Vector2d mu = Eigen::Vector2d::Zero();
  Eigen::Matrix2d Sigma = 1e6 * Eigen::Matrix2d::Identity();
};

struct Particle {
  Eigen::Vector3d x = Eigen::Vector3d::Zero();
  double w = 1.0;
  std::vector<LandmarkEKF> lms;

  explicit Particle(int M) : lms(M) {}
};

struct Obs {
  int id;
  Eigen::Vector2d z; // [range, bearing]
};

static Eigen::Vector3d motionModel(const Eigen::Vector3d& x, const Eigen::Vector2d& u, double dt) {
  double px = x(0), py = x(1), th = x(2);
  double v = u(0), w = u(1);

  Eigen::Vector3d x2;
  if (std::abs(w) < 1e-9) {
    x2(0) = px + v * dt * std::cos(th);
    x2(1) = py + v * dt * std::sin(th);
    x2(2) = th;
  } else {
    x2(0) = px + (v / w) * (std::sin(th + w * dt) - std::sin(th));
    x2(1) = py - (v / w) * (std::cos(th + w * dt) - std::cos(th));
    x2(2) = th + w * dt;
  }
  x2(2) = wrapAngle(x2(2));
  return x2;
}

static Eigen::Vector2d hLandmark(const Eigen::Vector3d& x, const Eigen::Vector2d& m) {
  double px = x(0), py = x(1), th = x(2);
  double dx = m(0) - px;
  double dy = m(1) - py;
  double q = dx*dx + dy*dy;
  double r = std::sqrt(std::max(q, 1e-12));
  double b = wrapAngle(std::atan2(dy, dx) - th);
  return Eigen::Vector2d(r, b);
}

static Eigen::Matrix2d H_landmark(const Eigen::Vector3d& x, const Eigen::Vector2d& m) {
  double px = x(0), py = x(1), th = x(2);
  (void)th;
  double dx = m(0) - px;
  double dy = m(1) - py;
  double q = std::max(dx*dx + dy*dy, 1e-12);
  double r = std::sqrt(std::max(q, 1e-12));
  Eigen::Matrix2d H;
  H << dx/r, dy/r,
      -dy/q, dx/q;
  return H;
}

static Eigen::Vector2d invMeasurement(const Eigen::Vector3d& x, const Eigen::Vector2d& z) {
  double px = x(0), py = x(1), th = x(2);
  double r = z(0), b = z(1);
  double ang = th + b;
  return Eigen::Vector2d(px + r * std::cos(ang), py + r * std::sin(ang));
}

static double gaussLikelihood(const Eigen::Vector2d& v, const Eigen::Matrix2d& S) {
  Eigen::Matrix2d Ssym = 0.5 * (S + S.transpose());
  double detS = std::max(Ssym.determinant(), 1e-18);
  Eigen::Matrix2d invS = Ssym.inverse();
  double e = v.transpose() * invS * v;
  double norm = 1.0 / (2.0 * M_PI * std::sqrt(detS));
  return norm * std::exp(-0.5 * e);
}

static void normalizeWeights(std::vector<Particle>& ps) {
  double sumw = 0.0;
  for (auto& p : ps) sumw += p.w;
  if (sumw < 1e-30) {
    double w0 = 1.0 / ps.size();
    for (auto& p : ps) p.w = w0;
    return;
  }
  for (auto& p : ps) p.w /= sumw;
}

static double effectiveSampleSize(const std::vector<Particle>& ps) {
  double s2 = 0.0;
  for (const auto& p : ps) s2 += p.w * p.w;
  return (s2 > 1e-18) ? 1.0 / s2 : 0.0;
}

static std::vector<Particle> systematicResample(const std::vector<Particle>& ps, std::mt19937& gen) {
  int N = (int)ps.size();
  std::vector<double> cdf(N, 0.0);
  cdf[0] = ps[0].w;
  for (int i = 1; i < N; ++i) cdf[i] = cdf[i-1] + ps[i].w;

  std::uniform_real_distribution<double> uni(0.0, 1.0 / N);
  double u0 = uni(gen);

  std::vector<Particle> out;
  out.reserve(N);

  int j = 0;
  for (int i = 0; i < N; ++i) {
    double u = u0 + (double)i / (double)N;
    while (j < N-1 && u > cdf[j]) j++;
    Particle pnew = ps[j]; // copy includes landmarks
    pnew.w = 1.0 / N;
    out.push_back(std::move(pnew));
  }
  return out;
}

static void fastslamStep(std::vector<Particle>& ps,
                         const Eigen::Vector2d& u,
                         const std::vector<Obs>& obs,
                         double dt,
                         const Eigen::Matrix2d& R_u,
                         const Eigen::Matrix2d& Q_z,
                         std::mt19937& gen) {
  std::normal_distribution<double> n01(0.0, 1.0);

  // 1) motion sampling by perturbing controls
  for (auto& p : ps) {
    Eigen::Vector2d noise;
    noise(0) = n01(gen);
    noise(1) = n01(gen);
    Eigen::Matrix2d L = R_u.llt().matrixL();
    Eigen::Vector2d u_noisy = u + L * noise;
    p.x = motionModel(p.x, u_noisy, dt);
  }

  // 2) landmark EKF updates + weights
  for (auto& p : ps) {
    for (const auto& o : obs) {
      auto& lm = p.lms[o.id];
      if (!lm.seen) {
        lm.mu = invMeasurement(p.x, o.z);
        Eigen::Matrix2d H = H_landmark(p.x, lm.mu);
        Eigen::Matrix2d J = H.inverse();
        lm.Sigma = J * Q_z * J.transpose();
        lm.seen = true;
        continue;
      }

      Eigen::Vector2d zhat = hLandmark(p.x, lm.mu);
      Eigen::Vector2d v;
      v(0) = o.z(0) - zhat(0);
      v(1) = wrapAngle(o.z(1) - zhat(1));

      Eigen::Matrix2d H = H_landmark(p.x, lm.mu);
      Eigen::Matrix2d S = H * lm.Sigma * H.transpose() + Q_z;
      Eigen::Matrix2d K = lm.Sigma * H.transpose() * S.inverse();
      lm.mu = lm.mu + K * v;
      lm.Sigma = (Eigen::Matrix2d::Identity() - K * H) * lm.Sigma;

      p.w *= gaussLikelihood(v, S);
    }
  }

  normalizeWeights(ps);

  double ess = effectiveSampleSize(ps);
  if (ess < 0.5 * ps.size()) {
    ps = systematicResample(ps, gen);
  }
}

int main() {
  // Minimal sanity check run (no observations). Hook to a simulator for real tests.
  const int M = 5;
  const int N = 100;
  const int T = 10;
  const double dt = 0.1;

  Eigen::Matrix2d R_u = Eigen::Matrix2d::Zero();
  R_u(0,0) = 0.05 * 0.05;
  R_u(1,1) = std::pow(2.0 * M_PI / 180.0, 2);

  Eigen::Matrix2d Q_z = Eigen::Matrix2d::Zero();
  Q_z(0,0) = 0.15 * 0.15;
  Q_z(1,1) = std::pow(3.0 * M_PI / 180.0, 2);

  std::vector<Particle> ps;
  ps.reserve(N);
  for (int i = 0; i < N; ++i) {
    Particle p(M);
    p.x = Eigen::Vector3d(0.0, 0.0, 0.0);
    p.w = 1.0 / N;
    ps.push_back(p);
  }

  std::mt19937 gen(42);

  for (int t = 0; t < T; ++t) {
    Eigen::Vector2d u(0.8, 0.1);
    std::vector<Obs> obs;

    fastslamStep(ps, u, obs, dt, R_u, Q_z, gen);

    Eigen::Vector3d mean = Eigen::Vector3d::Zero();
    double c = 0.0, s = 0.0;
    for (const auto& p : ps) {
      mean(0) += p.w * p.x(0);
      mean(1) += p.w * p.x(1);
      c += p.w * std::cos(p.x(2));
      s += p.w * std::sin(p.x(2));
    }
    mean(2) = std::atan2(s, c);

    std::cout << "t=" << t << " mean pose: " << mean.transpose() << "\n";
  }

  return 0;
}

In Java, EJML is a common choice for small matrix operations in estimation. The following file mirrors the same FastSLAM 1.0 structure as the Python/C++ versions. Plug in your observation generator (or later, a robotics middleware interface).

// Chapter11_Lesson3.java
/*
FastSLAM 1.0 (Rao–Blackwellized PF-SLAM) — compact Java reference implementation.

Educational notes:
- 2D pose: (x,y,theta)
- Known landmark IDs in observations
- Each particle maintains EKF (mean+cov) per landmark
- Uses EJML for matrices (recommended for robotics/estimation in Java)

Gradle/Maven dependency (EJML):
  org.ejml:ejml-simple:0.43 (or similar)

This file focuses on algorithmic structure. Hook it to your simulator/sensors.
*/

import org.ejml.simple.SimpleMatrix;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

public class Chapter11_Lesson3 {

    static double wrapAngle(double a) {
        while (a <= -Math.PI) a += 2.0 * Math.PI;
        while (a > Math.PI) a -= 2.0 * Math.PI;
        return a;
    }

    static class LandmarkEKF {
        boolean seen = false;
        SimpleMatrix mu = new SimpleMatrix(2, 1);               // [mx; my]
        SimpleMatrix Sigma = SimpleMatrix.identity(2).scale(1e6);
    }

    static class Particle {
        double x, y, th;
        double w;
        LandmarkEKF[] lms;

        Particle(int M) {
            x = 0; y = 0; th = 0;
            w = 1.0;
            lms = new LandmarkEKF[M];
            for (int i = 0; i < M; i++) lms[i] = new LandmarkEKF();
        }

        Particle deepCopy() {
            Particle p = new Particle(lms.length);
            p.x = x; p.y = y; p.th = th;
            p.w = w;
            for (int i = 0; i < lms.length; i++) {
                p.lms[i].seen = lms[i].seen;
                p.lms[i].mu = lms[i].mu.copy();
                p.lms[i].Sigma = lms[i].Sigma.copy();
            }
            return p;
        }
    }

    static class Obs {
        int id;
        double range;
        double bearing;
        Obs(int id, double range, double bearing) {
            this.id = id;
            this.range = range;
            this.bearing = bearing;
        }
    }

    static double[] motionModel(double[] x, double v, double w, double dt) {
        double px = x[0], py = x[1], th = x[2];
        double px2, py2, th2;

        if (Math.abs(w) < 1e-9) {
            px2 = px + v * dt * Math.cos(th);
            py2 = py + v * dt * Math.sin(th);
            th2 = th;
        } else {
            px2 = px + (v / w) * (Math.sin(th + w * dt) - Math.sin(th));
            py2 = py - (v / w) * (Math.cos(th + w * dt) - Math.cos(th));
            th2 = th + w * dt;
        }
        return new double[]{px2, py2, wrapAngle(th2)};
    }

    static double[] invMeasurement(double[] x, double range, double bearing) {
        double px = x[0], py = x[1], th = x[2];
        double ang = th + bearing;
        return new double[]{px + range * Math.cos(ang), py + range * Math.sin(ang)};
    }

    static double[] hLandmark(double[] x, double[] m) {
        double px = x[0], py = x[1], th = x[2];
        double dx = m[0] - px;
        double dy = m[1] - py;
        double q = dx * dx + dy * dy;
        double r = Math.sqrt(Math.max(q, 1e-12));
        double b = wrapAngle(Math.atan2(dy, dx) - th);
        return new double[]{r, b};
    }

    static SimpleMatrix H_landmark(double[] x, double[] m) {
        double px = x[0], py = x[1];
        double dx = m[0] - px;
        double dy = m[1] - py;
        double q = Math.max(dx * dx + dy * dy, 1e-12);
        double r = Math.sqrt(Math.max(q, 1e-12));

        // dh/dm
        return new SimpleMatrix(new double[][]{
                {dx / r, dy / r},
                {-dy / q, dx / q}
        });
    }

    static double gaussLikelihood(SimpleMatrix v, SimpleMatrix S) {
        // 2D Gaussian N(v;0,S)
        SimpleMatrix Ssym = S.plus(S.transpose()).scale(0.5);
        double detS = Math.max(Ssym.determinant(), 1e-18);
        SimpleMatrix invS = Ssym.invert();
        double e = v.transpose().mult(invS).mult(v).get(0, 0);
        double norm = 1.0 / (2.0 * Math.PI * Math.sqrt(detS));
        return norm * Math.exp(-0.5 * e);
    }

    static void normalizeWeights(List<Particle> ps) {
        double sum = 0.0;
        for (Particle p : ps) sum += p.w;
        if (sum < 1e-30) {
            double w0 = 1.0 / ps.size();
            for (Particle p : ps) p.w = w0;
        } else {
            for (Particle p : ps) p.w /= sum;
        }
    }

    static double effectiveSampleSize(List<Particle> ps) {
        double s2 = 0.0;
        for (Particle p : ps) s2 += p.w * p.w;
        return (s2 > 1e-18) ? 1.0 / s2 : 0.0;
    }

    static List<Particle> systematicResample(List<Particle> ps, Random rng) {
        int N = ps.size();
        double[] cdf = new double[N];
        cdf[0] = ps.get(0).w;
        for (int i = 1; i < N; i++) cdf[i] = cdf[i - 1] + ps.get(i).w;

        double u0 = rng.nextDouble() / N;
        List<Particle> out = new ArrayList<>(N);

        int j = 0;
        for (int i = 0; i < N; i++) {
            double u = u0 + (double) i / N;
            while (j < N - 1 && u > cdf[j]) j++;
            Particle pnew = ps.get(j).deepCopy();
            pnew.w = 1.0 / N;
            out.add(pnew);
        }
        return out;
    }

    static void fastslamStep(List<Particle> ps,
                             double v, double w, double dt,
                             List<Obs> obs,
                             SimpleMatrix R_u, SimpleMatrix Q_z,
                             Random rng) {
        int N = ps.size();

        // 1) sample motion by perturbing controls (v,w)
        for (Particle p : ps) {
            double dv = rng.nextGaussian() * Math.sqrt(R_u.get(0, 0));
            double dw = rng.nextGaussian() * Math.sqrt(R_u.get(1, 1));
            double[] x2 = motionModel(new double[]{p.x, p.y, p.th}, v + dv, w + dw, dt);
            p.x = x2[0]; p.y = x2[1]; p.th = x2[2];
        }

        // 2) update landmark EKFs and weights
        for (Particle p : ps) {
            for (Obs o : obs) {
                LandmarkEKF lm = p.lms[o.id];

                double[] x = new double[]{p.x, p.y, p.th};
                double[] z = new double[]{o.range, o.bearing};

                if (!lm.seen) {
                    double[] m0 = invMeasurement(x, o.range, o.bearing);
                    lm.mu.set(0, 0, m0[0]);
                    lm.mu.set(1, 0, m0[1]);

                    SimpleMatrix H = H_landmark(x, m0);
                    SimpleMatrix J = H.invert();
                    lm.Sigma = J.mult(Q_z).mult(J.transpose());
                    lm.seen = true;
                    continue;
                }

                double[] muArr = new double[]{lm.mu.get(0, 0), lm.mu.get(1, 0)};
                double[] zhat = hLandmark(x, muArr);

                double vr = z[0] - zhat[0];
                double vb = wrapAngle(z[1] - zhat[1]);
                SimpleMatrix vvec = new SimpleMatrix(new double[][]{ {vr}, {vb} });

                SimpleMatrix H = H_landmark(x, muArr);
                SimpleMatrix S = H.mult(lm.Sigma).mult(H.transpose()).plus(Q_z);
                SimpleMatrix K = lm.Sigma.mult(H.transpose()).mult(S.invert());

                lm.mu = lm.mu.plus(K.mult(vvec));
                lm.Sigma = SimpleMatrix.identity(2).minus(K.mult(H)).mult(lm.Sigma);

                p.w *= gaussLikelihood(vvec, S);
            }
        }

        normalizeWeights(ps);

        double ess = effectiveSampleSize(ps);
        if (ess < 0.5 * N) {
            List<Particle> res = systematicResample(ps, rng);
            ps.clear();
            ps.addAll(res);
        }
    }

    public static void main(String[] args) {
        int M = 5;    // number of landmarks
        int N = 200;  // particles
        int T = 20;
        double dt = 0.1;

        // Noise covariances
        SimpleMatrix R_u = SimpleMatrix.identity(2);
        R_u.set(0, 0, 0.05 * 0.05);
        R_u.set(1, 1, Math.pow(2.0 * Math.PI / 180.0, 2));

        SimpleMatrix Q_z = SimpleMatrix.identity(2);
        Q_z.set(0, 0, 0.15 * 0.15);
        Q_z.set(1, 1, Math.pow(3.0 * Math.PI / 180.0, 2));

        List<Particle> ps = new ArrayList<>(N);
        for (int i = 0; i < N; i++) {
            Particle p = new Particle(M);
            p.w = 1.0 / N;
            ps.add(p);
        }

        Random rng = new Random(42);

        for (int t = 0; t < T; t++) {
            double v = 0.8;
            double w = 0.1 * Math.sin(0.1 * t);

            // Demo uses empty observations. Hook your simulator here.
            List<Obs> obs = new ArrayList<>();

            fastslamStep(ps, v, w, dt, obs, R_u, Q_z, rng);

            // Weighted mean pose
            double mx = 0, my = 0, c = 0, s = 0;
            for (Particle p : ps) {
                mx += p.w * p.x;
                my += p.w * p.y;
                c += p.w * Math.cos(p.th);
                s += p.w * Math.sin(p.th);
            }
            double mth = Math.atan2(s, c);

            System.out.printf("t=%d mean pose: (%.3f, %.3f, %.3f)%n", t, mx, my, mth);
        }
    }
}

MATLAB is widely used in estimation courses because matrix operations are concise. The script below runs a small FastSLAM 1.0 demo similar to the Python implementation. For Simulink, you typically wrap the particle update into a MATLAB Function block and feed it control and measurement buses (this is described after the script).

% Chapter11_Lesson3.m
% FastSLAM 1.0 (Rao–Blackwellized Particle Filter SLAM) — educational MATLAB script.
%
% Assumptions:
% - 2D pose [x;y;theta]
% - Known data association (each observation includes landmark id)
% - Range-bearing sensor; Gaussian noise
%
% This script includes:
% 1) Simple simulator
% 2) FastSLAM update loop with per-landmark EKF inside each particle
%
% Note: This is designed for clarity more than performance.

function Chapter11_Lesson3()
    rng(42);

    % Landmarks in world
    L = [2 2;
         8 1;
         6.5 7;
         1 8;
         9 9];
    M = size(L,1);

    % Settings
    T = 150; dt = 0.1; maxRange = 7.0;

    % Noise covariances
    R_u = diag([0.05^2, (2*pi/180)^2]);      % noise on [v,w]
    Q_z = diag([0.15^2, (3*pi/180)^2]);      % noise on [range,bearing]

    % Controls
    U = zeros(2,T);
    for t=1:T
        U(:,t) = [0.8; 0.25*sin(0.05*t)];
    end

    % Ground truth simulation + observations (cell array)
    xTrue = [0;0;0];
    xTrueHist = zeros(3,T);
    Z = cell(T,1);

    for t=1:T
        xTrue = noisyMotion(xTrue, U(:,t), dt, R_u);
        xTrueHist(:,t) = xTrue;

        obs = [];
        for i=1:M
            zhat = hLandmark(xTrue, L(i,:)');
            if zhat(1) <= maxRange
                z = zhat + mvnrnd([0 0], Q_z)';
                z(2) = wrapAngle(z(2));
                obs = [obs; i, z(1), z(2)]; %#ok<AGROW>
            end
        end
        Z{t} = obs;
    end

    % FastSLAM particles
    N = 200;
    particles = initParticles(N, M);

    xEstHist = zeros(3,T);

    for t=1:T
        particles = fastslamStep(particles, U(:,t), Z{t}, dt, R_u, Q_z);
        xEstHist(:,t) = estimatePose(particles);
    end

    fprintf('Final true pose: [%.3f %.3f %.3f]\n', xTrueHist(:,end));
    fprintf('Final est  pose: [%.3f %.3f %.3f]\n', xEstHist(:,end));

    figure; hold on; grid on; axis equal;
    plot(xTrueHist(1,:), xTrueHist(2,:), 'LineWidth', 1.5);
    plot(xEstHist(1,:), xEstHist(2,:), 'LineWidth', 1.5);
    scatter(L(:,1), L(:,2), 80, 'x', 'LineWidth', 2);
    legend('True', 'FastSLAM mean', 'Landmarks');
    title('FastSLAM 1.0 demo (known data association)');
end

% ------------------------- Core Functions -------------------------

function particles = initParticles(N, M)
    particles = struct();
    for i=1:N
        particles(i).x = [0;0;0];
        particles(i).w = 1.0/N;
        particles(i).lmSeen = false(M,1);
        particles(i).lmMu = zeros(2,M);
        particles(i).lmSigma = repmat(eye(2)*1e6, 1, 1, M);
    end
end

function particles = fastslamStep(particles, u, obs, dt, R_u, Q_z)
    N = numel(particles);

    % 1) motion sampling
    for i=1:N
        particles(i).x = noisyMotion(particles(i).x, u, dt, R_u);
    end

    % 2) landmark EKFs + weights
    for i=1:N
        x = particles(i).x;
        for k=1:size(obs,1)
            id = obs(k,1);
            z = obs(k,2:3)';

            if ~particles(i).lmSeen(id)
                m0 = invMeasurement(x, z);
                particles(i).lmMu(:,id) = m0;
                H = H_landmark(x, m0);
                J = inv(H);
                particles(i).lmSigma(:,:,id) = J * Q_z * J';
                particles(i).lmSeen(id) = true;
                continue;
            end

            mu = particles(i).lmMu(:,id);
            Sigma = particles(i).lmSigma(:,:,id);

            zhat = hLandmark(x, mu);
            v = [z(1)-zhat(1); wrapAngle(z(2)-zhat(2))];

            H = H_landmark(x, mu);
            S = H*Sigma*H' + Q_z;
            K = Sigma*H' / S;
            mu2 = mu + K*v;
            Sigma2 = (eye(2)-K*H)*Sigma;

            particles(i).lmMu(:,id) = mu2;
            particles(i).lmSigma(:,:,id) = Sigma2;

            particles(i).w = particles(i).w * gaussLike(v, S);
        end
    end

    % 3) normalize and resample if needed
    ws = [particles.w]';
    sw = sum(ws);
    if sw < 1e-30
        for i=1:N, particles(i).w = 1.0/N; end
    else
        for i=1:N, particles(i).w = particles(i).w / sw; end
    end

    ws = [particles.w]';
    ess = 1.0 / sum(ws.^2);
    if ess < 0.5*N
        particles = systematicResample(particles);
    end
end

function xhat = estimatePose(particles)
    ws = [particles.w]';
    ws = ws / max(sum(ws), 1e-12);

    X = reshape([particles.x], 3, [])';
    px = sum(ws .* X(:,1));
    py = sum(ws .* X(:,2));
    c = sum(ws .* cos(X(:,3)));
    s = sum(ws .* sin(X(:,3)));
    th = atan2(s, c);

    xhat = [px; py; th];
end

function parts = systematicResample(particles)
    N = numel(particles);
    ws = [particles.w]';
    ws = ws / max(sum(ws), 1e-12);
    cdf = cumsum(ws);

    u0 = rand() / N;
    idx = zeros(N,1);
    j = 1;
    for i=1:N
        u = u0 + (i-1)/N;
        while (j < N) && (u > cdf(j)), j = j + 1; end
        idx(i) = j;
    end

    parts = particles(idx);
    for i=1:N, parts(i).w = 1.0/N; end
end

% ---------------------- Models and Utilities ----------------------

function x2 = noisyMotion(x, u, dt, R_u)
    uNoisy = u + mvnrnd([0 0], R_u)';
    x2 = motionModel(x, uNoisy, dt);
end

function x2 = motionModel(x, u, dt)
    px = x(1); py = x(2); th = x(3);
    v = u(1); w = u(2);

    if abs(w) < 1e-9
        px2 = px + v*dt*cos(th);
        py2 = py + v*dt*sin(th);
        th2 = th;
    else
        px2 = px + (v/w)*(sin(th+w*dt)-sin(th));
        py2 = py - (v/w)*(cos(th+w*dt)-cos(th));
        th2 = th + w*dt;
    end
    x2 = [px2; py2; wrapAngle(th2)];
end

function z = hLandmark(x, m)
    px = x(1); py = x(2); th = x(3);
    dx = m(1) - px;
    dy = m(2) - py;
    q = dx^2 + dy^2;
    r = sqrt(max(q, 1e-12));
    b = wrapAngle(atan2(dy, dx) - th);
    z = [r; b];
end

function H = H_landmark(x, m)
    px = x(1); py = x(2);
    dx = m(1) - px;
    dy = m(2) - py;
    q = max(dx^2 + dy^2, 1e-12);
    r = sqrt(max(q, 1e-12));
    H = [dx/r, dy/r;
         -dy/q, dx/q];
end

function m = invMeasurement(x, z)
    px = x(1); py = x(2); th = x(3);
    r = z(1); b = z(2);
    ang = th + b;
    m = [px + r*cos(ang); py + r*sin(ang)];
end

function p = gaussLike(v, S)
    S = 0.5*(S+S');
    detS = max(det(S), 1e-18);
    invS = inv(S);
    e = v'*invS*v;
    p = (1/(2*pi*sqrt(detS))) * exp(-0.5*e);
end

function a = wrapAngle(a)
    while a <= -pi, a = a + 2*pi; end
    while a >  pi, a = a - 2*pi; end
end

Simulink embedding (conceptual)

• Create a MATLAB Function block named FastSLAM_Update that stores particles in persistent variables.
• Inputs: \( \mathbf{u}_t \) (2×1), \( \mathbf{z}_t \) (Nx3 bus: [id, range, bearing]) and \( dt \).
• Outputs: pose estimate \( \hat{\mathbf{x} }_t \) and optionally landmark means for the best particle.
• Keep covariance matrices as fixed-size arrays (2×2×M) to satisfy code generation constraints.

The notebook below is a compact Wolfram Language implementation of the FastSLAM 1.0 update. In practice, you would extend the “minimal example setup” to include a simulator for observations (as done in Python/MATLAB).

(* Chapter11_Lesson3.nb *)
Notebook[{
  Cell["FastSLAM 1.0 (Rao–Blackwellized Particle Filter SLAM) — Wolfram Language demo", "Title"],
  Cell[TextData[{
    "This notebook is a compact educational implementation of FastSLAM 1.0 in 2D.\n",
    "Assumptions: known data association; range-bearing sensor; Gaussian noise; unicycle motion.\n"
  }], "Text"],

  Cell["1. Utilities", "Section"],
  Cell[BoxData[
    ToBoxes[
      ClearAll[wrap, motionModel, hLandmark, HLandmark, invMeas, gaussLike];
      wrap[a_] := Module[{x = a},
        While[x <= -Pi, x += 2 Pi];
        While[x > Pi, x -= 2 Pi];
        x
      ];

      motionModel[{px_, py_, th_}, {v_, w_}, dt_] := Module[{px2, py2, th2},
        If[Abs[w] < 10^-9,
          px2 = px + v dt Cos[th];
          py2 = py + v dt Sin[th];
          th2 = th;,
          px2 = px + (v/w) (Sin[th + w dt] - Sin[th]);
          py2 = py - (v/w) (Cos[th + w dt] - Cos[th]);
          th2 = th + w dt;
        ];
        {px2, py2, wrap[th2]}
      ];

      hLandmark[{px_, py_, th_}, {mx_, my_}] := Module[{dx, dy, q, r, b},
        dx = mx - px; dy = my - py;
        q = dx^2 + dy^2; r = Sqrt[Max[q, 10^-12]];
        b = wrap[ArcTan[dx, dy] - th];
        {r, b}
      ];

      HLandmark[{px_, py_, th_}, {mx_, my_}] := Module[{dx, dy, q, r},
        dx = mx - px; dy = my - py;
        q = Max[dx^2 + dy^2, 10^-12];
        r = Sqrt[Max[q, 10^-12]];
        { {dx/r, dy/r}, {-dy/q, dx/q} }
      ];

      invMeas[{px_, py_, th_}, {r_, b_}] := Module[{ang},
        ang = th + b;
        {px + r Cos[ang], py + r Sin[ang]}
      ];

      gaussLike[v_, S_] := Module[{Ssym, detS, invS, e},
        Ssym = (S + Transpose[S])/2;
        detS = Max[Det[Ssym], 10^-18];
        invS = Inverse[Ssym];
        e = v.invS.v;
        (1/(2 Pi Sqrt[detS])) Exp[-1/2 e]
      ];
    ]
  ], "Input"],

  Cell["2. One FastSLAM step (core update)", "Section"],
  Cell[BoxData[
    ToBoxes[
      ClearAll[fastslamStep];
      fastslamStep[particles_, u_, obs_, dt_, Ru_, Qz_, rng_] := Module[
        {ps = particles, N, mvn, normalize, ess, resample},
        N = Length[ps];
        mvn[cov_] := RandomVariate[MultinormalDistribution[{0, 0}, cov]];

        (* motion sampling *)
        ps = ps /. p_Association :> Module[{x = p["x"], un = u + mvn[Ru]},
          AssociateTo[p, "x" -> motionModel[x, un, dt]]; p
        ];

        (* measurement updates *)
        ps = ps /. p_Association :> Module[{x = p["x"], w = p["w"], lms = p["lms"], o, id, z, lm, mu, Sigma, zhat, v, H, S, K},
          Do[
            {id, z} = o;
            lm = lms[[id]];
            If[! lm["seen"],
              mu = invMeas[x, z];
              H = HLandmark[x, mu];
              Sigma = Inverse[H].Qz.Transpose[Inverse[H]];
              lm = <|"seen" -> True, "mu" -> mu, "Sigma" -> Sigma|>;
              lms[[id]] = lm;,
              mu = lm["mu"]; Sigma = lm["Sigma"];
              zhat = hLandmark[x, mu];
              v = {z[[1]] - zhat[[1]], wrap[z[[2]] - zhat[[2]]]};
              H = HLandmark[x, mu];
              S = H.Sigma.Transpose[H] + Qz;
              K = Sigma.Transpose[H].Inverse[S];
              mu = mu + K.v;
              Sigma = (IdentityMatrix[2] - K.H).Sigma;
              lm = <|"seen" -> True, "mu" -> mu, "Sigma" -> Sigma|>;
              lms[[id]] = lm;
              w = w * gaussLike[v, S];
            ],
            {o, obs}
          ];
          AssociateTo[p, "w" -> w, "lms" -> lms]; p
        ];

        (* normalize weights *)
        normalize = Total[ps[[All, "w"]]];
        If[normalize <= 10^-30,
          ps = ps /. p_Association :> (AssociateTo[p, "w" -> 1.0/N]; p);,
          ps = ps /. p_Association :> (AssociateTo[p, "w" -> p["w"]/normalize]; p);
        ];

        (* ESS + systematic resample *)
        ess = 1.0/Total[(ps[[All, "w"]]]^2];
        resample[ps0_] := Module[{cdf, u0, new = {}, j = 1, u},
          cdf = Accumulate[ps0[[All, "w"]]];
          u0 = RandomReal[{0, 1/N}];
          Do[
            u = u0 + (i - 1)/N;
            While[j < N && u > cdf[[j]], j++];
            AppendTo[new, ps0[[j]]],
            {i, 1, N}
          ];
          new = new /. p_Association :> (AssociateTo[p, "w" -> 1.0/N]; p);
          new
        ];
        If[ess < 0.5 N, ps = resample[ps]];
        ps
      ];
    ]
  ], "Input"],

  Cell["3. Minimal example setup (no plotting)", "Section"],
  Cell[BoxData[
    ToBoxes[
      SeedRandom[42];
      M = 5; N = 50;
      Ru = DiagonalMatrix[{0.05^2, (2 Pi/180)^2}];
      Qz = DiagonalMatrix[{0.15^2, (3 Pi/180)^2}];

      initParticle[] := <|
        "x" -> {0.0, 0.0, 0.0},
        "w" -> 1.0/N,
        "lms" -> Table[<|"seen" -> False, "mu" -> {0.0, 0.0}, "Sigma" -> 10^6 IdentityMatrix[2]|>, {M}]
      |>;

      particles = Table[initParticle[], {N}];

      (* One step with empty observation list (hook your simulator here) *)
      u = {0.8, 0.1}; dt = 0.1;
      particles = fastslamStep[particles, u, {}, dt, Ru, Qz, RandomReal[]];

      meanPose = Module[{ws = particles[[All, "w"]], xs = particles[[All, "x"]], px, py, c, s},
        px = Total[ws * xs[[All, 1]]];
        py = Total[ws * xs[[All, 2]]];
        c  = Total[ws * Cos[xs[[All, 3]]]];
        s  = Total[ws * Sin[xs[[All, 3]]]];
        {px, py, ArcTan[c, s]}
      ];
      meanPose
    ]
  ], "Input"]
},
WindowTitle -> "Chapter11_Lesson3.nb"
]

Problem 1 (Posterior factorization): Assume independent landmark priors \( p(\mathbf{m}) = \prod_{k=1}^{M} p(\mathbf{m}_k) \) and factorized measurement likelihood \( p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) = \prod_{k=1}^{M} p(\mathbf{z}_t^{(k)} \mid \mathbf{x}_t, \mathbf{m}_k) \). Show that \( p(\mathbf{m} \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) = \prod_{k=1}^{M} p(\mathbf{m}_k \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) \).

Solution: Starting from: \( p(\mathbf{m} \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) \propto p(\mathbf{m})\prod_{\tau=1}^t p(\mathbf{z}_\tau \mid \mathbf{x}_\tau,\mathbf{m}) \), substitute the factorized prior and likelihood: \( p(\mathbf{m} \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t}) \propto \prod_{k=1}^{M}\left(p(\mathbf{m}_k)\prod_{\tau=1}^t p(\mathbf{z}_\tau^{(k)} \mid \mathbf{x}_\tau,\mathbf{m}_k)\right) \). The right-hand side is a product of terms, each depending only on \( \mathbf{m}_k \). Normalizing yields the product of marginals, proving conditional independence.

Problem 2 (Weight update under motion proposal): Suppose FastSLAM samples \( \mathbf{x}_t^{(i)} \sim p(\mathbf{x}_t \mid \mathbf{x}_{t-1}^{(i)}, \mathbf{u}_t) \). Derive the importance weight recursion for particle \( i \) and show why it reduces to the measurement likelihood.

Solution: In general, \( w_t^{(i)} \propto w_{t-1}^{(i)}\frac{p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}, \mathbf{m}^{(i)})p(\mathbf{x}_t^{(i)} \mid \mathbf{x}_{t-1}^{(i)},\mathbf{u}_t)}{q(\mathbf{x}_t^{(i)} \mid \mathbf{x}_{t-1}^{(i)},\mathbf{u}_t,\mathbf{z}_t)} \). If \( q = p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{u}_t) \), the motion terms cancel, giving \( w_t^{(i)} \propto w_{t-1}^{(i)} p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}, \mathbf{m}^{(i)}) \).

Problem 3 (EKF innovation covariance): For a landmark EKF update with measurement Jacobian \( \mathbf{H}_t \), landmark covariance \( \mathbf{\Sigma} \), and sensor noise \( \mathbf{Q} \), derive the innovation covariance \( \mathbf{S} \) and explain its role in both EKF update and particle weighting.

Solution: Linearizing \( \mathbf{z} \approx h(\cdot) + \mathbf{H}(\mathbf{m}-\boldsymbol{\mu}) + \mathbf{v} \) with \( \mathbf{m}-\boldsymbol{\mu} \sim \mathcal{N}(\mathbf{0},\mathbf{\Sigma}) \) and \( \mathbf{v}\sim\mathcal{N}(\mathbf{0},\mathbf{Q}) \), the innovation \( \mathbf{\nu} \) has covariance \( \mathbf{S} = \mathbf{H}\mathbf{\Sigma}\mathbf{H}^\top + \mathbf{Q} \). In EKF, \( \mathbf{S} \) determines the Kalman gain and how much the landmark is corrected. In FastSLAM weighting, \( \mathbf{S} \) defines the Gaussian predictive likelihood of the innovation, hence how plausible the observation is for that particle.

Problem 4 (Effective sample size): Show that if all particle weights are equal \( w_i = 1/N \), then \( N_{\text{eff} } = N \), and if one particle has weight 1 and the rest 0, then \( N_{\text{eff} } = 1 \).

Solution: By definition \( N_{\text{eff} } = 1/\sum_i w_i^2 \). If all equal: \( \sum_i w_i^2 = N(1/N^2)=1/N \), hence \( N_{\text{eff} }=N \). If one is 1: \( \sum_i w_i^2 = 1 \), hence \( N_{\text{eff} }=1 \).

Problem 5 (Complexity comparison): Provide a concise complexity argument comparing one update step of EKF-SLAM (dense covariance) versus FastSLAM 1.0 (per-landmark EKFs) when \( K_t \) landmarks are observed at time \( t \).

Solution: In EKF-SLAM, the joint covariance is size roughly \( (3+2M)\times(3+2M) \). Even if only \( K_t \) measurements arrive, updating a dense covariance generally involves operations that scale at least linearly with \( M \) per measurement, and the full matrix maintenance can be quadratic in \( M \) (implementation-dependent). In FastSLAM 1.0, each particle updates only the EKFs for the observed landmarks; each landmark EKF update is constant-sized (2×2), so per particle cost is \( \mathcal{O}(K_t) \), and overall \( \mathcal{O}(N K_t) \), plus \( \mathcal{O}(N) \) resampling.

FastSLAM reframes filter-based SLAM by sampling the robot trajectory with a particle filter and analytically tracking landmark uncertainty via per-landmark EKFs conditioned on each trajectory. The key enabler is the Rao–Blackwellized factorization: landmarks become conditionally independent given the path, converting a high-dimensional joint filter into many small filters. FastSLAM 1.0 uses the motion model as its proposal and therefore updates particle weights via the landmark predictive likelihoods; resampling is triggered by weight degeneracy measured through \( N_{\text{eff} } \).