In prior chapters, we treated localization as estimating the robot pose given a known map, and mapping as estimating the map given known poses. SLAM couples both: the robot must estimate its own pose while building a map from the same uncertain measurements. The coupling creates a feedback loop: \( \text{pose uncertainty} \) affects mapping quality, and map uncertainty feeds back into localization.

We describe the robot pose at time \( t \) by \( \mathbf{x}_t = [x_t, y_t, \theta_t]^\top \in \mathbb{R}^3 \) and the map by \( \mathbf{m} \), which may be a set of landmarks \( \{\mathbf{m}_i\}_{i=1}^N \) with \( \mathbf{m}_i \in \mathbb{R}^2 \), or a grid map. The robot applies controls \( \mathbf{u}_t \) (e.g., odometry increments, wheel/IMU integration outputs) and receives measurements \( \mathbf{z}_t \) (e.g., LiDAR features, range-bearing beacons, visual features).

The core question of SLAM is: given the history \( \mathbf{u}_{1:t} \) and \( \mathbf{z}_{1:t} \), what is the posterior over the robot trajectory and the map?

The most general (and conceptually clean) formulation estimates the full trajectory \( \mathbf{x}_{0:t} \) and the map \( \mathbf{m} \):

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

Under standard assumptions introduced in the probabilistic robotics foundations chapter:

• Markov motion: \( p(\mathbf{x}_t \mid \mathbf{x}_{0:t-1}, \mathbf{u}_{1:t}) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t) \)
• Static map: \( \mathbf{m} \) does not evolve (for this chapter).
• Conditional independence of measurements: \( p(\mathbf{z}_t \mid \mathbf{x}_{0:t}, \mathbf{m}, \mathbf{z}_{1:t-1}) = p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) \)

the joint distribution factorizes as a dynamic Bayesian network:

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

This factorization is the mathematical backbone of “filter-based SLAM”: inference is driven by repeating (i) a motion prediction factor and (ii) a measurement correction factor.

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

Data association as a latent variable. Often each measurement must be matched to a map entity (landmark id, feature track, etc.). Introduce an association variable \( \mathbf{c}_t \) so that \( p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}) \) becomes \( p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m}, \mathbf{c}_t) \). Then SLAM marginalizes over unknown associations:

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

We will revisit the practical consequences (false matches, perceptual aliasing, loop closure errors) in Lesson 4.

A filter does not maintain the entire trajectory posterior explicitly. Instead it maintains the current belief over the current pose and the map: \( \text{belief } b_t(\mathbf{x}_t,\mathbf{m}) \triangleq p(\mathbf{x}_t,\mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \).

The recursion consists of a prediction step (integrate out the previous pose) and an update step (apply the likelihood of the new measurement):

\[ \underbrace{\bar b_t(\mathbf{x}_t,\mathbf{m})}_{\text{prediction} } = \int p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t)\, b_{t-1}(\mathbf{x}_{t-1},\mathbf{m}) \, d\mathbf{x}_{t-1}, \]

\[ \underbrace{b_t(\mathbf{x}_t,\mathbf{m})}_{\text{update} } = \eta\; p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m})\; \bar b_t(\mathbf{x}_t,\mathbf{m}), \]

where \( \eta \) is a normalizing constant ensuring the posterior integrates to 1.

Proposition (SLAM Bayes-filter recursion). Under the Markov motion and conditional-independence assumptions in Section 2, the belief \( b_t(\mathbf{x}_t,\mathbf{m}) \) satisfies the two equations above.

Proof (compressed but rigorous).

Start from the definition: \( b_t(\mathbf{x}_t,\mathbf{m}) = p(\mathbf{x}_t,\mathbf{m}\mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) \). Apply Bayes’ rule:

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

By conditional independence, \( p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{m},\mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{m}) \). For the second factor, marginalize over \( \mathbf{x}_{t-1} \):

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

Expand the integrand using the chain rule: \( p(\mathbf{x}_t,\mathbf{m},\mathbf{x}_{t-1}\mid \cdot) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{m},\cdot)\, p(\mathbf{m},\mathbf{x}_{t-1}\mid \cdot) \). Since the motion does not depend on the map in the basic model and is Markov in the state: \( p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{m},\mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{u}_t) \). The remaining term is exactly the previous belief: \( p(\mathbf{m},\mathbf{x}_{t-1}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t-1}) = b_{t-1}(\mathbf{x}_{t-1},\mathbf{m}) \). Substituting yields the prediction integral; then multiplying by the likelihood yields the update.

flowchart TD
  A["Inputs: u_t, z_t"] --> B["Prediction: integrate motion model"]
  B --> C["Compute expected measurement from (x_t, m)"]
  C --> D["Innovation: z_t - zhat_t"]
  D --> E["Update: apply likelihood / correction"]
  E --> F["Output belief: b_t(x_t, m)"]
        

Different “filter-based SLAM” algorithms differ mainly in how they represent and approximate \( b_t(\mathbf{x}_t,\mathbf{m}) \) (Gaussian EKF, Gaussian-sum, particle factorization, etc.).

For a planar mobile robot, a standard discrete-time kinematic motion model (unicycle form) is:

\[ \mathbf{x}_t = \begin{bmatrix} x_{t-1} + v_t \Delta t \cos\theta_{t-1} \\ y_{t-1} + v_t \Delta t \sin\theta_{t-1} \\ \theta_{t-1} + \omega_t \Delta t \end{bmatrix} + \mathbf{w}_t, \quad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_t), \]

where \( \mathbf{u}_t = [v_t, \omega_t, \Delta t]^\top \) and \( \mathbf{Q}_t \) models process uncertainty (slip, encoder noise, IMU integration drift).

For a landmark map with range-bearing observations to landmark \( i \), the measurement model is:

\[ \mathbf{z}_t^{(i)} = \begin{bmatrix} r_t^{(i)} \\ \phi_t^{(i)} \end{bmatrix} = \begin{bmatrix} \sqrt{(m_{ix}-x_t)^2 + (m_{iy}-y_t)^2} \\ \operatorname{atan2}(m_{iy}-y_t, m_{ix}-x_t) - \theta_t \end{bmatrix} + \mathbf{v}_t^{(i)}, \quad \mathbf{v}_t^{(i)} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_t). \]

These two probabilistic models fully specify the SLAM Bayes filter in Section 3. EKF-SLAM arises when we assume the belief is approximately Gaussian and propagate mean/covariance under linearization (next lesson).

In filter-based SLAM, a common representation is an augmented state consisting of the robot pose and the map parameters. For a landmark map:

\[ \mathbf{y}_t \triangleq \begin{bmatrix} \mathbf{x}_t \\ \mathbf{m}_1 \\ \vdots \\ \mathbf{m}_N \end{bmatrix} \in \mathbb{R}^{3 + 2N}. \]

The belief becomes \( p(\mathbf{y}_t \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \). If modeled as Gaussian, it is summarized by \( (\boldsymbol{\mu}_t, \mathbf{P}_t) \), where \( \boldsymbol{\mu}_t \in \mathbb{R}^{3+2N} \) and \( \mathbf{P}_t \in \mathbb{R}^{(3+2N)\times(3+2N)} \). The off-diagonal blocks encode cross-correlation between robot pose and map estimates—this is the key coupling unique to SLAM.

For occupancy-grid maps, \( \mathbf{m} \) is high-dimensional (one variable per cell). Classical filter-based SLAM in this chapter is typically taught with landmark maps because the augmented-state math is tractable and highlights coupling cleanly. Grid-based SLAM often uses different approximations (e.g., particle filters with map updates), which we connect later when discussing FastSLAM.

A subtle but fundamental property: without an external global reference (e.g., GPS/RTK or known anchors), SLAM is invariant to a global rigid transform. This implies the “absolute” world frame is not identifiable; only relative geometry is observable.

Proposition (SE(2) gauge invariance). Consider landmark measurements that depend only on relative displacement between the robot and landmarks. Let a global rigid transform be defined by rotation \( \mathbf{R}\in\mathbb{R}^{2\times 2} \) with \( \mathbf{R}^\top\mathbf{R}=\mathbf{I} \) and translation \( \mathbf{t}\in\mathbb{R}^2 \). Define transformed variables:

\[ \tilde{\mathbf{p} }_t = \mathbf{R}\mathbf{p}_t + \mathbf{t}, \quad \tilde{\mathbf{m} }_i = \mathbf{R}\mathbf{m}_i + \mathbf{t}, \quad \tilde{\theta}_t = \theta_t + \alpha, \]

where \( \mathbf{p}_t=[x_t,y_t]^\top \) and \( \alpha \) is the rotation angle of \( \mathbf{R} \). Then the ideal range-bearing observation to landmark \( i \) remains unchanged: \( h(\tilde{\mathbf{x} }_t,\tilde{\mathbf{m} }_i)=h(\mathbf{x}_t,\mathbf{m}_i) \).

Proof. Range depends on the Euclidean norm of the relative vector: \( \|\tilde{\mathbf{m} }_i-\tilde{\mathbf{p} }_t\| = \|\mathbf{R}(\mathbf{m}_i-\mathbf{p}_t)\| = \|\mathbf{m}_i-\mathbf{p}_t\| \) since \( \mathbf{R} \) is orthonormal. Bearing depends on the angle of the relative vector expressed in the robot frame; the global rotation adds \( \alpha \) to both the landmark direction and robot heading, which cancels in \( \operatorname{atan2}(\cdot) - \theta_t \).

Consequence. To make the posterior well-posed, we impose a gauge choice such as a prior on \( \mathbf{x}_0 \) (e.g., \( \mathbf{x}_0 \sim \mathcal{N}(\hat{\mathbf{x} }_0,\mathbf{P}_0) \)) or fix a landmark/anchor. In EKF-SLAM, this affects consistency and the structure of the covariance; we return to this in Lesson 2.

The following implementations operationalize the SLAM Bayes-filter loop by maintaining an augmented state and performing predict/update steps. For a university lab setting, these examples are intentionally compact: they assume known data association and a small landmark set.

Python robotics ecosystem notes: In real AMR stacks, Python is often used for prototyping and offline evaluation (NumPy/SciPy), ROS2 tooling (rclpy), and dataset benchmarking. The EKF math shown here is pure NumPy to keep dependencies minimal.

File: Chapter11_Lesson1.py

# Chapter11_Lesson1.py
"""
Autonomous Mobile Robots (Control Engineering)
Chapter 11: SLAM I — Filter-Based SLAM
Lesson 1: The SLAM Problem Formulation

This script implements a *minimal* filter-based SLAM core using an EKF over the
augmented state y = [x, y, theta, m1x, m1y, ..., mNx, mNy].

Pedagogical choices (for Lesson 1):
- Known data association (each measurement is already matched to a landmark id).
- Simple differential-drive motion model (unicycle).
- Range-bearing landmark observations.
- Gaussian noise; standard EKF predict-update.

Note: EKF-SLAM structure/limitations are treated in Lesson 2; here we focus on
the probabilistic formulation and the recursion implemented by a filter.
"""

from __future__ import annotations
import numpy as np


def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + np.pi) % (2.0 * np.pi) - np.pi


def motion_model(x: np.ndarray, u: np.ndarray) -> np.ndarray:
    """
    Unicycle motion model.
    State: x = [px, py, theta]
    Control: u = [v, omega, dt]
    """
    px, py, th = x
    v, w, dt = u
    if abs(w) < 1e-12:
        px2 = px + v * dt * np.cos(th)
        py2 = py + v * dt * np.sin(th)
        th2 = th
    else:
        px2 = px + (v / w) * (np.sin(th + w * dt) - np.sin(th))
        py2 = py - (v / w) * (np.cos(th + w * dt) - np.cos(th))
        th2 = th + w * dt
    return np.array([px2, py2, wrap_angle(th2)], dtype=float)


def jacobian_motion_wrt_state(x: np.ndarray, u: np.ndarray) -> np.ndarray:
    """
    Jacobian Fx = d f(x,u) / d x  (3x3).
    """
    px, py, th = x
    v, w, dt = u
    Fx = np.eye(3, dtype=float)

    if abs(w) < 1e-12:
        Fx[0, 2] = -v * dt * np.sin(th)
        Fx[1, 2] =  v * dt * np.cos(th)
    else:
        th2 = th + w * dt
        Fx[0, 2] = (v / w) * (np.cos(th2) - np.cos(th))
        Fx[1, 2] = (v / w) * (np.sin(th2) - np.sin(th))
    return Fx


def measurement_model_pose_landmark(x: np.ndarray, mi: np.ndarray) -> np.ndarray:
    """
    Range-bearing measurement to landmark i.
    z = [range, bearing], bearing is relative to robot heading.
    """
    px, py, th = x
    mx, my = mi
    dx = mx - px
    dy = my - py
    r = np.sqrt(dx * dx + dy * dy)
    b = np.arctan2(dy, dx) - th
    return np.array([r, wrap_angle(b)], dtype=float)


def jacobian_measurement_wrt_pose_and_landmark(x: np.ndarray, mi: np.ndarray):
    """
    Returns (H_x, H_m) where:
    H_x = d h / d x  (2x3)
    H_m = d h / d m_i (2x2)
    """
    px, py, th = x
    mx, my = mi
    dx = mx - px
    dy = my - py
    q = dx * dx + dy * dy
    r = np.sqrt(q)

    if q < 1e-12:
        q = 1e-12
        r = np.sqrt(q)

    Hx = np.array([
        [-dx / r, -dy / r, 0.0],
        [ dy / q, -dx / q, -1.0]
    ], dtype=float)

    Hm = np.array([
        [ dx / r,  dy / r],
        [-dy / q,  dx / q]
    ], dtype=float)
    return Hx, Hm


def ekf_slam_predict(y: np.ndarray, P: np.ndarray, u: np.ndarray, Q_pose: np.ndarray):
    n = y.size
    x = y[:3].copy()
    m = y[3:].copy()

    Fx = jacobian_motion_wrt_state(x, u)
    x_pred = motion_model(x, u)

    F = np.eye(n, dtype=float)
    F[:3, :3] = Fx

    Q = np.zeros((n, n), dtype=float)
    Q[:3, :3] = Q_pose

    y_pred = np.concatenate([x_pred, m])
    P_pred = F @ P @ F.T + Q
    return y_pred, P_pred


def ekf_slam_update_landmark(y: np.ndarray, P: np.ndarray, z: np.ndarray, landmark_id: int, R_meas: np.ndarray):
    n = y.size
    x = y[:3]
    i = landmark_id
    idx = 3 + 2 * i
    mi = y[idx:idx + 2]

    z_hat = measurement_model_pose_landmark(x, mi)
    innov = np.array([z[0] - z_hat[0], wrap_angle(z[1] - z_hat[1])], dtype=float)

    Hx, Hm = jacobian_measurement_wrt_pose_and_landmark(x, mi)

    H = np.zeros((2, n), dtype=float)
    H[:, :3] = Hx
    H[:, idx:idx + 2] = Hm

    S = H @ P @ H.T + R_meas
    K = P @ H.T @ np.linalg.inv(S)

    y_upd = y + K @ innov
    y_upd[2] = wrap_angle(y_upd[2])

    I = np.eye(n, dtype=float)
    P_upd = (I - K @ H) @ P @ (I - K @ H).T + K @ R_meas @ K.T
    return y_upd, P_upd


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

    landmarks = np.array([[4.0, 2.0], [8.0, -1.0], [2.0, -3.0]], dtype=float)
    N = landmarks.shape[0]

    x_true = np.array([0.0, 0.0, 0.0], dtype=float)

    y = np.zeros(3 + 2 * N, dtype=float)
    y[:3] = np.array([0.0, 0.0, 0.0], dtype=float)
    y[3:] = np.array([3.0, 1.0, 7.0, 0.0, 1.0, -2.0], dtype=float)

    P = np.eye(y.size, dtype=float) * 1e-3
    for i in range(N):
        idx = 3 + 2 * i
        P[idx:idx + 2, idx:idx + 2] = np.eye(2) * 4.0

    Q_pose = np.diag([0.02**2, 0.02**2, (np.deg2rad(1.0))**2])
    R_meas = np.diag([0.10**2, (np.deg2rad(2.0))**2])

    U = [
        np.array([1.0,  0.10, 1.0]),
        np.array([1.0,  0.00, 1.0]),
        np.array([1.0, -0.10, 1.0]),
        np.array([1.0,  0.00, 1.0]),
    ]

    for t, u in enumerate(U, start=1):
        x_true = motion_model(x_true, u) + np.random.multivariate_normal(np.zeros(3), Q_pose)
        x_true[2] = wrap_angle(x_true[2])

        y, P = ekf_slam_predict(y, P, u, Q_pose)

        for i in range(N):
            z_true = measurement_model_pose_landmark(x_true, landmarks[i])
            z_noisy = z_true + np.random.multivariate_normal(np.zeros(2), R_meas)
            z_noisy[1] = wrap_angle(z_noisy[1])
            y, P = ekf_slam_update_landmark(y, P, z_noisy, i, R_meas)

        print(f"t={t}: pose mean = {np.round(y[:3], 3)}")


if __name__ == "__main__":
    demo()
      

C++ robotics ecosystem notes: C++ is the dominant language for production SLAM in ROS/ROS2 (real-time constraints, deterministic memory, mature linear algebra libraries). Typical dependencies: Eigen for matrices, and ROS2 message/TF stacks.

File: Chapter11_Lesson1.cpp

// Chapter11_Lesson1.cpp
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 11: SLAM I — Filter-Based SLAM
Lesson 1: The SLAM Problem Formulation

A compact EKF-SLAM core (known data association) for a 2D unicycle robot with
range-bearing landmark observations.

Build note:
  - Requires Eigen (header-only). Example compile:
    g++ -O2 -std=c++17 Chapter11_Lesson1.cpp -I /path/to/eigen -o slam_demo
*/

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

static double wrapAngle(double a) {
  const double twoPi = 2.0 * M_PI;
  a = std::fmod(a + M_PI, twoPi);
  if (a < 0) a += twoPi;
  return a - M_PI;
}

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

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

static Eigen::Matrix3d jacMotionWrtState(const Eigen::Vector3d& x, const Eigen::Vector3d& u) {
  const double th = x(2);
  const double v = u(0), w = u(1), dt = u(2);

  Eigen::Matrix3d Fx = Eigen::Matrix3d::Identity();
  if (std::abs(w) < 1e-12) {
    Fx(0,2) = -v * dt * std::sin(th);
    Fx(1,2) =  v * dt * std::cos(th);
  } else {
    const double th2 = th + w * dt;
    Fx(0,2) = (v / w) * (std::cos(th2) - std::cos(th));
    Fx(1,2) = (v / w) * (std::sin(th2) - std::sin(th));
  }
  return Fx;
}

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

static void jacMeasWrtPoseLandmark(
  const Eigen::Vector3d& x, const Eigen::Vector2d& m,
  Eigen::Matrix<double,2,3>& Hx,
  Eigen::Matrix2d& Hm
) {
  const double px = x(0), py = x(1);
  const double dx = m(0) - px;
  const double dy = m(1) - py;
  double q = dx*dx + dy*dy;
  if (q < 1e-12) q = 1e-12;
  const double r = std::sqrt(q);

  Hx << -dx/r, -dy/r, 0.0,
         dy/q, -dx/q, -1.0;

  Hm <<  dx/r,  dy/r,
        -dy/q,  dx/q;
}

int main() {
  std::vector<Eigen::Vector2d> landmarks = {
    {4.0,  2.0},
    {8.0, -1.0},
    {2.0, -3.0}
  };
  const int N = static_cast<int>(landmarks.size());
  const int n = 3 + 2*N;

  Eigen::VectorXd y(n);
  y.setZero();
  y.segment<3>(0) = Eigen::Vector3d(0.0, 0.0, 0.0);
  y.segment<6>(3) = Eigen::VectorXd::Map((double[]){3.0,1.0, 7.0,0.0, 1.0,-2.0}, 6);

  Eigen::MatrixXd P = Eigen::MatrixXd::Identity(n,n) * 1e-3;
  for (int i=0;i<N;i++) {
    P.block<2,2>(3+2*i, 3+2*i) = Eigen::Matrix2d::Identity() * 4.0;
  }

  Eigen::Matrix3d Qpose = Eigen::Vector3d(0.02*0.02, 0.02*0.02, std::pow(M_PI/180.0,2)).asDiagonal();
  Eigen::Matrix2d Rmeas = Eigen::Vector2d(0.10*0.10, std::pow(2.0*M_PI/180.0,2)).asDiagonal();

  Eigen::Vector3d u(1.0, 0.10, 1.0);

  Eigen::Vector3d x = y.segment<3>(0);
  Eigen::Matrix3d Fx = jacMotionWrtState(x, u);
  Eigen::Vector3d xPred = motionModel(x, u);

  Eigen::MatrixXd F = Eigen::MatrixXd::Identity(n,n);
  F.block<3,3>(0,0) = Fx;

  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(n,n);
  Q.block<3,3>(0,0) = Qpose;

  y.segment<3>(0) = xPred;
  P = F * P * F.transpose() + Q;

  const int lmId = 0;
  const int idx = 3 + 2*lmId;
  Eigen::Vector2d mi = y.segment<2>(idx);

  Eigen::Vector2d z = measModel(xPred, landmarks[lmId]);
  z(0) += 0.05;
  z(1) = wrapAngle(z(1) + 1.0 * M_PI/180.0);

  Eigen::Vector2d zHat = measModel(y.segment<3>(0), mi);
  Eigen::Vector2d innov(z(0) - zHat(0), wrapAngle(z(1) - zHat(1)));

  Eigen::Matrix<double,2,3> Hx;
  Eigen::Matrix2d Hm;
  jacMeasWrtPoseLandmark(y.segment<3>(0), mi, Hx, Hm);

  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2,n);
  H.block<2,3>(0,0) = Hx;
  H.block<2,2>(0,idx) = Hm;

  Eigen::Matrix2d S = H * P * H.transpose() + Rmeas;
  Eigen::MatrixXd K = P * H.transpose() * S.inverse();

  y = y + K * innov;
  y(2) = wrapAngle(y(2));

  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n,n);
  P = (I - K*H) * P * (I - K*H).transpose() + K * Rmeas * K.transpose();

  std::cout << "Updated pose mean: " << y.segment<3>(0).transpose() << "\n";
  return 0;
}
      

Java robotics ecosystem notes: Java is common in education and some robotics middleware stacks; it pairs well with EJML for linear algebra and clean OOP structuring of models.

File: Chapter11_Lesson1.java

// Chapter11_Lesson1.java
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 11: SLAM I — Filter-Based SLAM
Lesson 1: The SLAM Problem Formulation

Minimal EKF-SLAM core (known data association) using EJML.

Dependencies:
  - EJML (org.ejml:ejml-simple)
*/

import org.ejml.simple.SimpleMatrix;

public class Chapter11_Lesson1 {

    static double wrapAngle(double a) {
        double twoPi = 2.0 * Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        return a - Math.PI;
    }

    static SimpleMatrix motionModel(SimpleMatrix x, SimpleMatrix u) {
        double px = x.get(0), py = x.get(1), th = x.get(2);
        double v = u.get(0), w = u.get(1), dt = u.get(2);

        double px2, py2, th2;
        if (Math.abs(w) < 1e-12) {
            px2 = px + v * dt * Math.cos(th);
            py2 = py + v * dt * Math.sin(th);
            th2 = th;
        } else {
            double thN = th + w * dt;
            px2 = px + (v / w) * (Math.sin(thN) - Math.sin(th));
            py2 = py - (v / w) * (Math.cos(thN) - Math.cos(th));
            th2 = thN;
        }
        SimpleMatrix xp = new SimpleMatrix(3,1);
        xp.set(0, px2);
        xp.set(1, py2);
        xp.set(2, wrapAngle(th2));
        return xp;
    }

    static SimpleMatrix jacMotionWrtState(SimpleMatrix x, SimpleMatrix u) {
        double th = x.get(2);
        double v = u.get(0), w = u.get(1), dt = u.get(2);

        SimpleMatrix Fx = SimpleMatrix.identity(3);
        if (Math.abs(w) < 1e-12) {
            Fx.set(0,2, -v * dt * Math.sin(th));
            Fx.set(1,2,  v * dt * Math.cos(th));
        } else {
            double th2 = th + w * dt;
            Fx.set(0,2, (v / w) * (Math.cos(th2) - Math.cos(th)));
            Fx.set(1,2, (v / w) * (Math.sin(th2) - Math.sin(th)));
        }
        return Fx;
    }

    static SimpleMatrix measModel(SimpleMatrix x, SimpleMatrix m) {
        double px = x.get(0), py = x.get(1), th = x.get(2);
        double dx = m.get(0) - px;
        double dy = m.get(1) - py;
        double r = Math.sqrt(dx*dx + dy*dy);
        double b = wrapAngle(Math.atan2(dy, dx) - th);
        SimpleMatrix z = new SimpleMatrix(2,1);
        z.set(0, r);
        z.set(1, b);
        return z;
    }

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

        SimpleMatrix Hx = new SimpleMatrix(2,3);
        Hx.set(0,0, -dx/r); Hx.set(0,1, -dy/r); Hx.set(0,2, 0.0);
        Hx.set(1,0,  dy/q); Hx.set(1,1, -dx/q); Hx.set(1,2, -1.0);

        SimpleMatrix Hm = new SimpleMatrix(2,2);
        Hm.set(0,0,  dx/r); Hm.set(0,1,  dy/r);
        Hm.set(1,0, -dy/q); Hm.set(1,1,  dx/q);

        return new SimpleMatrix[]{Hx, Hm};
    }

    public static void main(String[] args) {
        SimpleMatrix lm0 = new SimpleMatrix(new double[][]{ {4.0},{2.0} });
        int N = 3;
        int n = 3 + 2*N;

        SimpleMatrix y = new SimpleMatrix(n,1);
        y.zero();
        y.set(0, 0.0); y.set(1, 0.0); y.set(2, 0.0);
        y.set(3, 3.0); y.set(4, 1.0);
        y.set(5, 7.0); y.set(6, 0.0);
        y.set(7, 1.0); y.set(8,-2.0);

        SimpleMatrix P = SimpleMatrix.identity(n).scale(1e-3);
        for (int i=0;i<N;i++) {
            int idx = 3 + 2*i;
            P.set(idx,   idx,   4.0);
            P.set(idx+1, idx+1, 4.0);
        }

        SimpleMatrix Qpose = new SimpleMatrix(new double[][]{
            {0.02*0.02, 0, 0},
            {0, 0.02*0.02, 0},
            {0, 0, Math.pow(Math.PI/180.0,2)}
        });

        SimpleMatrix Rmeas = new SimpleMatrix(new double[][]{
            {0.10*0.10, 0},
            {0, Math.pow(2.0*Math.PI/180.0,2)}
        });

        SimpleMatrix u = new SimpleMatrix(new double[][]{ {1.0},{0.10},{1.0} });

        SimpleMatrix x = y.extractMatrix(0,3,0,1);
        SimpleMatrix Fx = jacMotionWrtState(x, u);
        SimpleMatrix xPred = motionModel(x, u);

        SimpleMatrix F = SimpleMatrix.identity(n);
        F.insertIntoThis(0,0, Fx);

        SimpleMatrix Q = new SimpleMatrix(n,n);
        Q.zero();
        Q.insertIntoThis(0,0, Qpose);

        y.insertIntoThis(0,0, xPred);
        P = F.mult(P).mult(F.transpose()).plus(Q);

        int lmId = 0;
        int idx = 3 + 2*lmId;
        SimpleMatrix mi = y.extractMatrix(idx, idx+2, 0, 1);

        SimpleMatrix z = measModel(xPred, lm0);
        z.set(0, z.get(0) + 0.05);
        z.set(1, wrapAngle(z.get(1) + 1.0*Math.PI/180.0));

        SimpleMatrix zHat = measModel(y.extractMatrix(0,3,0,1), mi);
        SimpleMatrix innov = new SimpleMatrix(2,1);
        innov.set(0, z.get(0) - zHat.get(0));
        innov.set(1, wrapAngle(z.get(1) - zHat.get(1)));

        SimpleMatrix[] J = jacMeasWrtPoseLandmark(y.extractMatrix(0,3,0,1), mi);
        SimpleMatrix Hx = J[0];
        SimpleMatrix Hm = J[1];

        SimpleMatrix H = new SimpleMatrix(2,n);
        H.zero();
        H.insertIntoThis(0,0, Hx);
        H.insertIntoThis(0,idx, Hm);

        SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(Rmeas);
        SimpleMatrix K = P.mult(H.transpose()).mult(S.invert());

        y = y.plus(K.mult(innov));
        y.set(2, wrapAngle(y.get(2)));

        SimpleMatrix I = SimpleMatrix.identity(n);
        P = (I.minus(K.mult(H))).mult(P).mult((I.minus(K.mult(H))).transpose())
                .plus(K.mult(Rmeas).mult(K.transpose()));

        System.out.println("Updated pose mean: " + y.extractMatrix(0,3,0,1).transpose());
    }
}
      

MATLAB/Simulink ecosystem notes: MATLAB is widely used for algorithm development; the Robotics System Toolbox provides kinematics, occupancy grids, and sensor models. Simulink is useful for real-time prototyping and block-diagram integration of estimation and control. Below is a MATLAB EKF-SLAM core plus an optional script that generates a small Simulink “shell” model programmatically.

File: Chapter11_Lesson1.m

% Chapter11_Lesson1.m
% Autonomous Mobile Robots (Control Engineering)
% Chapter 11: SLAM I — Filter-Based SLAM
% Lesson 1: The SLAM Problem Formulation
%
% This file contains:
%   (A) A minimal EKF-SLAM predict/update for an augmented state.
%   (B) A helper that can (optionally) generate a simple Simulink model shell
%       that calls the EKF step inside a MATLAB Function block.

function Chapter11_Lesson1()
    rng(7);

    landmarks = [4 2; 8 -1; 2 -3];
    N = size(landmarks,1);

    x_true = [0;0;0];

    y = zeros(3+2*N,1);
    y(1:3) = [0;0;0];
    y(4:end) = [3;1; 7;0; 1;-2];

    P = eye(length(y))*1e-3;
    for i=1:N
        idx = 3 + 2*(i-1) + 1;
        P(idx:idx+1, idx:idx+1) = eye(2)*4.0;
    end

    Q_pose = diag([0.02^2, 0.02^2, (deg2rad(1.0))^2]);
    R_meas = diag([0.10^2, (deg2rad(2.0))^2]);

    U = [1.0  0.10 1.0;
         1.0  0.00 1.0;
         1.0 -0.10 1.0;
         1.0  0.00 1.0];

    for t=1:size(U,1)
        u = U(t,:).';

        x_true = motion_model(x_true, u) + mvnrnd([0 0 0], Q_pose).';
        x_true(3) = wrap_angle(x_true(3));

        [y,P] = ekf_slam_predict(y,P,u,Q_pose);

        for i=1:N
            z_true  = meas_model(x_true, landmarks(i,:).');
            z_noisy = z_true + mvnrnd([0 0], R_meas).';
            z_noisy(2) = wrap_angle(z_noisy(2));

            [y,P] = ekf_slam_update_landmark(y,P,z_noisy,i,R_meas);
        end

        fprintf('t=%d: pose mean = [%.3f %.3f %.3f]\n', t, y(1), y(2), y(3));
    end
end

function a = wrap_angle(a)
    a = mod(a + pi, 2*pi) - pi;
end

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

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

function z = meas_model(x, m)
    px=x(1); py=x(2); th=x(3);
    dx = m(1)-px;
    dy = m(2)-py;
    r = sqrt(dx^2 + dy^2);
    b = atan2(dy,dx) - th;
    z = [r; wrap_angle(b)];
end

function [Hx,Hm] = jac_meas_wrt_pose_landmark(x,m)
    px=x(1); py=x(2);
    dx = m(1)-px;
    dy = m(2)-py;
    q = dx^2 + dy^2;
    if q < 1e-12, q = 1e-12; end
    r = sqrt(q);

    Hx = [ -dx/r, -dy/r, 0;
            dy/q, -dx/q, -1 ];
    Hm = [  dx/r,  dy/r;
           -dy/q,  dx/q ];
end

function [y_pred,P_pred] = ekf_slam_predict(y,P,u,Q_pose)
    n = length(y);
    x = y(1:3);
    Fx = jac_motion_wrt_state(x,u);
    x_pred = motion_model(x,u);

    F = eye(n);
    F(1:3,1:3) = Fx;

    Q = zeros(n);
    Q(1:3,1:3) = Q_pose;

    y_pred = y;
    y_pred(1:3) = x_pred;
    P_pred = F*P*F.' + Q;
end

function Fx = jac_motion_wrt_state(x,u)
    th=x(3);
    v=u(1); w=u(2); dt=u(3);
    Fx = eye(3);

    if abs(w) < 1e-12
        Fx(1,3) = -v*dt*sin(th);
        Fx(2,3) =  v*dt*cos(th);
    else
        thN = th + w*dt;
        Fx(1,3) = (v/w)*(cos(thN)-cos(th));
        Fx(2,3) = (v/w)*(sin(thN)-sin(th));
    end
end

function [y_upd,P_upd] = ekf_slam_update_landmark(y,P,z,landmark_id,R_meas)
    n = length(y);
    i = landmark_id - 1;
    idx = 3 + 2*i + 1;
    x = y(1:3);
    mi = y(idx:idx+1);

    z_hat = meas_model(x,mi);
    innov = [z(1)-z_hat(1); wrap_angle(z(2)-z_hat(2))];

    [Hx,Hm] = jac_meas_wrt_pose_landmark(x,mi);

    H = zeros(2,n);
    H(:,1:3) = Hx;
    H(:,idx:idx+1) = Hm;

    S = H*P*H.' + R_meas;
    K = P*H.'/S;

    y_upd = y + K*innov;
    y_upd(3) = wrap_angle(y_upd(3));

    I = eye(n);
    P_upd = (I-K*H)*P*(I-K*H).' + K*R_meas*K.';
end

% Optional: generate a tiny Simulink shell (requires Simulink)
function build_simulink_shell()
    mdl = 'SLAM_BayesFilter_Shell';
    if bdIsLoaded(mdl); close_system(mdl,0); end
    new_system(mdl); open_system(mdl);

    add_block('simulink/Sources/Constant', [mdl '/u'], 'Value', '[1;0.1;1]');
    add_block('simulink/Sources/Constant', [mdl '/z'], 'Value', '[5;0]');
    add_block('simulink/Sources/Constant', [mdl '/R'], 'Value', 'diag([0.1^2,(deg2rad(2))^2])');
    add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/EKFStep']);
    add_block('simulink/Sinks/Display', [mdl '/Display']);

    set_param([mdl '/EKFStep'], 'Script', ...
        ['function y_out = f(y_in,P_in,u,z,R)\n' ...
         '% Call an EKF update/predict in MATLAB code. This is a shell; wire in states as needed.\n' ...
         'y_out = y_in; %#ok<NASGU>\n' ...
         'end\n']);

    add_line(mdl,'u/1','EKFStep/3');
    add_line(mdl,'z/1','EKFStep/4');
    add_line(mdl,'R/1','EKFStep/5');
    add_line(mdl,'EKFStep/1','Display/1');

    save_system(mdl);
end
      

Wolfram Mathematica ecosystem notes: Mathematica is strong for symbolic manipulation (derivations, Jacobians) and quick numerical experiments. Below is a plain-text notebook containing symbolic Bayes-filter statements and one EKF update demonstration.

File: Chapter11_Lesson1.nb

(* Chapter11_Lesson1.nb
Autonomous Mobile Robots (Control Engineering)
Chapter 11: SLAM I — Filter-Based SLAM
Lesson 1: The SLAM Problem Formulation

This notebook is stored as plain text. Open it in Wolfram Mathematica.
It contains:
  - Symbolic SLAM Bayes-filter recursion statements
  - A small numeric EKF-SLAM update demonstration for one landmark
*)

Notebook[{
  Cell["Chapter 11 — Lesson 1: SLAM Problem Formulation (Filter-Based)", "Title"],
  Cell["1. Bayes-filter recursion (symbolic)", "Section"],
  Cell[BoxData@ToBoxes@HoldForm[
    b[t][x_t_, m_] == p[x_t, m \[Conditioned] z[1 ;; t], u[1 ;; t]]
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    bBar[t][x_t_, m_] == Integrate[ p[x_t \[Conditioned] x[t - 1], u[t]] * b[t - 1][x[t - 1], m],
                                  {x[t - 1], -\[Infinity], \[Infinity]}]
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    b[t][x_t_, m_] \[Proportional] p[z[t] \[Conditioned] x_t, m] * bBar[t][x_t, m]
  ], "Input"],

  Cell["2. Minimal EKF step (numeric)", "Section"],
  Cell["We use a 2D pose x=[px,py,theta] and one landmark m=[mx,my].", "Text"],

  Cell[BoxData@ToBoxes@HoldForm[
    WrapAngle[a_] := Mod[a + Pi, 2 Pi] - Pi
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    MeasModel[{px_, py_, th_}, {mx_, my_}] := Module[{dx = mx - px, dy = my - py},
      {Sqrt[dx^2 + dy^2], WrapAngle[ArcTan[dx, dy] - th]}
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    Jacobians[{px_, py_, th_}, {mx_, my_}] := Module[{dx = mx - px, dy = my - py, q, r},
      q = dx^2 + dy^2; If[q < 10^-12, q = 10^-12];
      r = Sqrt[q];
      {
        { {-dx/r, -dy/r, 0}, {dy/q, -dx/q, -1} },
        { {dx/r, dy/r}, {-dy/q, dx/q} }
      }
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    EKFUpdate[y_, P_, z_, R_] := Module[{x, m, zHat, innov, Hx, Hm, H, S, K, y2, P2, I},
      x = y[[1 ;; 3]];
      m = y[[4 ;; 5]];
      zHat = MeasModel[x, m];
      innov = {z[[1]] - zHat[[1]], WrapAngle[z[[2]] - zHat[[2]]]};
      {Hx, Hm} = Jacobians[x, m];
      H = ConstantArray[0, {2, 5}];
      H[[All, 1 ;; 3]] = Hx;
      H[[All, 4 ;; 5]] = Hm;
      S = H.P.Transpose[H] + R;
      K = P.Transpose[H].Inverse[S];
      y2 = y + K.innov;
      y2[[3]] = WrapAngle[y2[[3]]];
      I = IdentityMatrix[Length[y]];
      P2 = (I - K.H).P.Transpose[(I - K.H)] + K.R.Transpose[K];
      {y2, P2}
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes@HoldForm[
    y0 = {0.0, 0.0, 0.0, 3.0, 1.0};
    P0 = DiagonalMatrix[{0.001, 0.001, (1 Degree)^2, 4.0, 4.0}];
    z  = {5.0, 0.1};
    R  = DiagonalMatrix[{0.1^2, (2 Degree)^2}];
    {y1, P1} = EKFUpdate[y0, P0, z, R];
    y1
  ], "Input"]
}]
      

Problem 1 (Derive the SLAM Bayes filter): Starting from \( p(\mathbf{x}_t,\mathbf{m}\mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) \), derive the prediction and update equations in Section 3, explicitly stating the conditional-independence assumptions you use.

Solution: Use Bayes’ rule: \( p(\mathbf{x}_t,\mathbf{m}\mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) = \eta\, p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{m},\mathbf{z}_{1:t-1},\mathbf{u}_{1:t})\, p(\mathbf{x}_t,\mathbf{m}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) \). Apply conditional independence to replace the first factor by \( p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{m}) \). For the second factor, marginalize over \( \mathbf{x}_{t-1} \) and use the Markov motion:

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

Identify the last term as the previous belief \( b_{t-1}(\mathbf{x}_{t-1},\mathbf{m}) \), obtaining the prediction; multiply by the likelihood and renormalize for the update.

Problem 2 (Gauge invariance): Show that if measurements depend only on relative displacement \( \mathbf{m}_i-\mathbf{p}_t \), then applying the same rigid transform to both the robot trajectory and landmarks leaves all ideal measurements unchanged.

Solution: Let \( \tilde{\mathbf{p} }_t=\mathbf{R}\mathbf{p}_t+\mathbf{t} \), \( \tilde{\mathbf{m} }_i=\mathbf{R}\mathbf{m}_i+\mathbf{t} \). Then \( \tilde{\mathbf{m} }_i-\tilde{\mathbf{p} }_t=\mathbf{R}(\mathbf{m}_i-\mathbf{p}_t) \). Range is invariant because orthonormal \( \mathbf{R} \) preserves norms: \( \|\tilde{\mathbf{m} }_i-\tilde{\mathbf{p} }_t\|=\|\mathbf{m}_i-\mathbf{p}_t\| \). Bearing is invariant because the global rotation adds the same angle to both the landmark direction and robot heading, which cancels in the relative bearing expression.

Problem 3 (Latent data association): Introduce an association variable \( \mathbf{c}_t \). Write the SLAM posterior as a sum over association histories \( \mathbf{c}_{1:t} \) and explain (in one paragraph) why this sum is computationally hard in practice.

Solution: The posterior is \( p(\mathbf{x}_{0:t},\mathbf{m}\mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) = \sum_{\mathbf{c}_{1:t} } p(\mathbf{x}_{0:t},\mathbf{m},\mathbf{c}_{1:t}\mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) \). The sum is hard because the number of association sequences grows combinatorially with time and with the number of candidate landmarks per measurement (a branching process), so exact marginalization is generally infeasible without strong approximations or pruning.

Problem 4 (Augmented-state dimension): For a 2D landmark map with \( N \) landmarks, what is the dimension of the augmented state \( \mathbf{y}_t \) and the number of scalar entries in the covariance \( \mathbf{P}_t \)? Give the leading-order scaling in \( N \).

Solution: The state dimension is \( d = 3 + 2N \). The covariance has \( d^2 \) scalar entries (or \( d(d+1)/2 \) unique entries due to symmetry). Leading-order scaling is \( \mathcal{O}(N^2) \) storage.

Problem 5 (Compute a predicted measurement and Jacobian): Let the robot pose be \( \mathbf{x}=[1,2,\pi/6]^\top \) and a landmark be \( \mathbf{m}=[4,3]^\top \). Compute the predicted range and bearing \( \hat{\mathbf{z} }=[\hat r,\hat\phi]^\top \). Then compute the Jacobian \( \partial \hat{\mathbf{z} }/\partial \mathbf{x} \).

Solution: First compute \( dx=4-1=3 \), \( dy=3-2=1 \), \( q=dx^2+dy^2=10 \), \( \hat r=\sqrt{10} \). Bearing is \( \hat\phi=\operatorname{atan2}(1,3)-\pi/6 \), wrapped to \( (-\pi,\pi] \). The Jacobian (from Section 7 code) is:

\[ \frac{\partial}{\partial \mathbf{x} } \begin{bmatrix}\hat r\\ \hat\phi\end{bmatrix} = \begin{bmatrix} -dx/\hat r & -dy/\hat r & 0 \\ dy/q & -dx/q & -1 \end{bmatrix}. \]

Substitute \( dx=3 \), \( dy=1 \), \( q=10 \), \( \hat r=\sqrt{10} \).

We formulated SLAM as inference over the joint posterior of robot states and map, derived the Bayes-filter recursion that drives filter-based SLAM, and connected the abstract recursion to concrete AMR motion and landmark measurement models. We also highlighted a fundamental identifiability issue (global-frame gauge freedom) that shapes priors and estimator consistency. Next, we specialize this formulation to EKF-SLAM, where the belief is approximated by a single Gaussian and propagated by linearization.