In Chapter 9 Lesson 1 we built dense maps (occupancy grids) by maintaining a probability for each cell. Feature-based mapping instead uses a sparse set of landmarks such as points (corners, poles), line segments (walls), or fiducial markers. A landmark map is often represented as \( \mathcal{M} = \{ \mathbf{m}_1, \dots, \mathbf{m}_N \} \), where each landmark state is, for 2D point landmarks, \( \mathbf{m}_i \in \mathbb{R}^2 \).

Compared with occupancy grids, landmark maps:

• have memory and compute that scale with the number of landmarks ( \(N\)), not the number of grid cells;
• enable geometric queries (e.g., line-of-sight, map matching) using explicit primitives;
• require data association: deciding which measurement corresponds to which landmark.

flowchart TD
  A["Inputs: pose x_k (from odom/IMU/localization), sensor scan z_k"] --> B["Extract candidate features from scan"]
  B --> C["For each feature: predict measurement \nfrom each existing landmark"]
  C --> D["Compute innovation and Mahalanobis distance"]
  D --> E{"Association passes gate?"}
  E -->|"yes"| F["Update landmark estimate \n(Gaussian update)"]
  E -->|"no"| G["Initialize new landmark \nfrom inverse sensor model"]
  F --> H["Map management: merge/prune, store covariance"]
  G --> H
  H --> I["Output: landmark map M = [mu_i, Sigma_i]"]
        

Throughout this lesson we adopt a mapping-with-known-poses viewpoint: the pose sequence \( \mathbf{x}_{1:k} \) is treated as given (from Chapters 5–8 localization/odometry processing), and we estimate only the map. The joint estimation problem (SLAM) is deferred to Chapters 11–12.

Let the robot pose at time \(k\) be \( \mathbf{x}_k = (x_k, y_k, \theta_k) \) in SE(2) coordinates (already introduced in earlier chapters). A standard 2D range–bearing sensor model for observing a point landmark \( \mathbf{m}_i = (m^x_i, m^y_i) \) is:

\[ \mathbf{z}_{k,i} = \begin{bmatrix} r_{k,i} \\ \varphi_{k,i} \end{bmatrix} = \begin{bmatrix} \sqrt{(m^x_i - x_k)^2 + (m^y_i - y_k)^2} \\ \operatorname{wrap}\!\left( \operatorname{atan2}(m^y_i - y_k,\; m^x_i - x_k) - \theta_k \right) \end{bmatrix} + \mathbf{v}_{k,i}, \quad \mathbf{v}_{k,i} \sim \mathcal{N}(\mathbf{0},\mathbf{R}) \]

The inverse model (used for initialization) maps a pose and a measurement to a landmark location:

\[ \mathbf{m}_i = \begin{bmatrix} x_k + r_{k,i}\cos(\theta_k + \varphi_{k,i}) \\ y_k + r_{k,i}\sin(\theta_k + \varphi_{k,i}) \end{bmatrix}. \]

We will use the following modeling assumptions (typical in AMR mapping):

• Conditional independence of measurements: given \( \mathbf{x}_k \) and the corresponding landmark state, the noise terms are independent across different observed landmarks.
• Static map: landmarks do not move during mapping (dynamic objects are handled later under robustness topics).

The mapping goal is the posterior over the map after measurements up to time \(k\): \( p(\mathcal{M} \mid \mathbf{z}_{1:k}, \mathbf{x}_{1:k}) \). Under known poses, Bayes’ rule gives the recursive update:

\[ p(\mathcal{M} \mid \mathbf{z}_{1:k}, \mathbf{x}_{1:k}) \propto p(\mathbf{z}_k \mid \mathcal{M}, \mathbf{x}_k)\; p(\mathcal{M} \mid \mathbf{z}_{1:k-1}, \mathbf{x}_{1:k-1}). \]

A central advantage of landmark maps is that (under common sensor models) the posterior can factorize over landmarks.

Proposition (Per-landmark factorization under conditional independence). Assume (i) the prior over landmarks factorizes as \( p(\mathcal{M}) = \prod_{i=1}^N p(\mathbf{m}_i) \), and (ii) at time \(k\) each measurement term depends on exactly one landmark and the pose, with independent noise. Then the posterior factorizes: \( p(\mathcal{M} \mid \mathbf{z}_{1:k}, \mathbf{x}_{1:k}) = \prod_{i=1}^N p(\mathbf{m}_i \mid \mathbf{z}_{1:k}, \mathbf{x}_{1:k}) \).

Proof. By Bayes’ rule and the measurement independence assumption, \( p(\mathbf{z}_k \mid \mathcal{M}, \mathbf{x}_k) = \prod_{(k,i)\in\mathcal{O}_k} p(\mathbf{z}_{k,i} \mid \mathbf{m}_i, \mathbf{x}_k) \), where \( \mathcal{O}_k \) is the set of observed landmark indices at time \(k\). If the prior factorizes, then the unnormalized posterior is a product of factors, each involving only one \( \mathbf{m}_i \). Renormalizing preserves the product structure, yielding the factorized posterior. □

This result motivates the common implementation strategy: keep a Gaussian belief for each landmark separately (mean and covariance), updated whenever that landmark is observed. The remaining challenge is data association (Section 5): determining which landmark index \(i\) each measurement corresponds to.

Suppose we maintain for landmark \(i\) a Gaussian belief \( p(\mathbf{m}_i \mid \cdot) \approx \mathcal{N}(\boldsymbol{\mu}_i, \mathbf{\Sigma}_i) \). For a nonlinear measurement \( \mathbf{z} = \mathbf{h}(\mathbf{x}_k, \mathbf{m}_i) + \mathbf{v} \), we linearize around the current mean: \( \mathbf{h}(\mathbf{x}_k, \mathbf{m}_i) \approx \mathbf{h}(\mathbf{x}_k, \boldsymbol{\mu}_i) + \mathbf{H}_i(\mathbf{m}_i - \boldsymbol{\mu}_i) \), where \( \mathbf{H}_i = \frac{\partial \mathbf{h} }{\partial \mathbf{m} }\big|_{\boldsymbol{\mu}_i} \).

For range–bearing, the Jacobian w.r.t. landmark coordinates is:

\[ \mathbf{H}_i = \begin{bmatrix} \frac{m^x_i - x_k}{r} & \frac{m^y_i - y_k}{r} \\ -\frac{m^y_i - y_k}{r^2} & \frac{m^x_i - x_k}{r^2} \end{bmatrix}, \quad r^2 = (m^x_i - x_k)^2 + (m^y_i - y_k)^2. \]

Define prediction and innovation: \( \hat{\mathbf{z} } = \mathbf{h}(\mathbf{x}_k, \boldsymbol{\mu}_i) \) and \( \mathbf{y} = \mathbf{z} - \hat{\mathbf{z} } \) (with angle wrapped). The EKF-style update is:

\[ \mathbf{S} = \mathbf{H}_i\mathbf{\Sigma}_i\mathbf{H}_i^\top + \mathbf{R}, \qquad \mathbf{K} = \mathbf{\Sigma}_i\mathbf{H}_i^\top\mathbf{S}^{-1}, \]

\[ \boldsymbol{\mu}_i \leftarrow \boldsymbol{\mu}_i + \mathbf{K}\mathbf{y}, \qquad \mathbf{\Sigma}_i \leftarrow (\mathbf{I} - \mathbf{K}\mathbf{H}_i)\mathbf{\Sigma}_i. \]

Why is this statistically meaningful? In the linear-Gaussian case \( \mathbf{z} = \mathbf{H}\mathbf{m} + \mathbf{v} \) with \( \mathbf{m}\sim\mathcal{N}(\boldsymbol{\mu},\mathbf{\Sigma}) \), the posterior is exactly Gaussian and the above update equals the posterior mean/covariance. Under nonlinear \( \mathbf{h} \), the EKF update is the exact linear-Gaussian posterior of the locally linearized model.

MAP interpretation (sketch). For linear Gaussian models, the negative log posterior is a quadratic form:

\[ -\log p(\mathbf{m} \mid \mathbf{z}) = \tfrac{1}{2}\|\mathbf{m}-\boldsymbol{\mu}\|_{\mathbf{\Sigma}^{-1} }^2 + \tfrac{1}{2}\|\mathbf{z}-\mathbf{H}\mathbf{m}\|_{\mathbf{R}^{-1} }^2 + c, \]

whose minimizer satisfies the normal equations and yields the Kalman gain update. (In Section 11 you will see the same quadratic structure again in pose-graph and factor-graph SLAM.)

Data association selects the landmark index \(i\) for each observed feature. A common baseline is nearest neighbor in measurement space with statistical gating. Given an observation \( \mathbf{z} \) and a landmark hypothesis \(i\), define innovation \( \mathbf{y}_i \) and innovation covariance \( \mathbf{S}_i \) as in Section 4. The squared Mahalanobis distance is:

\[ d_i^2 = \mathbf{y}_i^\top \mathbf{S}_i^{-1} \mathbf{y}_i. \]

Gate (chi-square test). Under the Gaussian hypothesis (correct association, linearized model), \( d_i^2 \) is approximately \( \chi^2 \)-distributed with degrees of freedom equal to the measurement dimension (2 for range–bearing). Thus, for confidence level \( \alpha \), accept association \(i\) if \( d_i^2 \le \chi^2_{2,\alpha} \). For example, \( \chi^2_{2,0.99} \approx 9.21 \).

flowchart TD
  Z["New feature measurement z"] --> P["For each landmark i: compute zhat_i and innovation y_i"]
  P --> S["Compute S_i = H_i P_i H_i^T + R"]
  S --> D["Compute d_i^2 = y_i^T S_i^{-1} y_i"]
  D --> M["Select i* = argmin d_i^2"]
  M --> G{"d_i*^2 <= gate?"}
  G -->|"yes"| U["Update landmark i* \n(Gaussian update)"]
  G -->|"no"| N["Initialize a new landmark \nfrom inverse model"]
        

Important practical note. The gate protects against gross mismatches but does not resolve ambiguous environments. More powerful association methods (e.g., joint compatibility, multi-hypothesis tracking) build on the same likelihood structure and will be revisited when we address SLAM data association in Chapter 11.

When an observation fails gating against all existing landmarks, we initialize a new landmark by inverse projection. A principled Gaussian initialization can propagate measurement noise through the inverse model using Jacobians. Let \( \mathbf{m} = \mathbf{g}(\mathbf{x}_k, \mathbf{z}) \) be the inverse mapping (Section 2). For small noise, the landmark covariance can be approximated by:

\[ \mathbf{\Sigma}_{\text{init} } \approx \mathbf{G}_z\,\mathbf{R}\,\mathbf{G}_z^\top + \mathbf{G}_x\,\mathbf{Q}_x\,\mathbf{G}_x^\top, \]

where \( \mathbf{G}_z = \frac{\partial \mathbf{g} }{\partial \mathbf{z} } \), \( \mathbf{G}_x = \frac{\partial \mathbf{g} }{\partial \mathbf{x} } \), and \( \mathbf{Q}_x \) is pose uncertainty (from localization). In this lesson’s code we keep the pose fixed and use a conservative constant initialization \( \mathbf{\Sigma}_{\text{init} } = \sigma_0^2\mathbf{I} \) to keep the implementation focused.

With per-landmark Gaussians, the dominant costs are:

• Association search: naïve nearest-neighbor checks all landmarks: \( \mathcal{O}(N) \) per measurement. Spatial indexing (k-d trees, grid hashing) can reduce average cost.
• Update cost: each EKF landmark update is constant time for fixed measurement dimension (here 2D): \( \mathcal{O}(1) \).

Map management policies include:

• Pruning: remove landmarks with persistently high uncertainty or never re-observed.
• Merging: if two landmarks become statistically indistinguishable, merge using Gaussian fusion (requires care with correlation).
• Outlier rejection: use robust gating and consistency checks to prevent corruption.

The following file simulates a robot path, generates noisy range–bearing observations, performs nearest-neighbor association with chi-square gating, initializes new landmarks, and updates landmark means/covariances. Libraries commonly used in robotics Python workflows include numpy, scipy, matplotlib, and (in ROS 2) message and geometry utilities.

Chapter9_Lesson3.py

#!/usr/bin/env python3
# Chapter9_Lesson3.py
# Feature-Based Mapping (Landmarks) with known poses:
# - Range-bearing sensor model
# - Nearest-neighbor data association with chi-square gating
# - Landmark initialization and per-landmark EKF updates
#
# Dependencies: numpy, matplotlib (optional for plotting)

import math
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 h_range_bearing(x: np.ndarray, m: np.ndarray) -> np.ndarray:
    """Measurement model z = [range, bearing] for pose x=(x,y,theta) and landmark m=(mx,my)."""
    dx = m[0] - x[0]
    dy = m[1] - x[1]
    r = math.sqrt(dx*dx + dy*dy)
    b = wrap_angle(math.atan2(dy, dx) - x[2])
    return np.array([r, b], dtype=float)

def H_landmark_jacobian(x: np.ndarray, m: np.ndarray) -> np.ndarray:
    """Jacobian dh/dm for range-bearing measurement w.r.t landmark coords (mx,my)."""
    dx = m[0] - x[0]
    dy = m[1] - x[1]
    q = dx*dx + dy*dy
    r = math.sqrt(q)
    # Avoid division by zero in degenerate cases
    eps = 1e-12
    q = max(q, eps)
    r = max(r, eps)
    H = np.array([
        [dx / r,          dy / r],
        [-dy / q,         dx / q]
    ], dtype=float)
    return H

def ekf_update_landmark(mu: np.ndarray, Sigma: np.ndarray, x: np.ndarray, z: np.ndarray, R: np.ndarray):
    """Per-landmark EKF update: (mu,Sigma) updated given pose x and measurement z."""
    zhat = h_range_bearing(x, mu)
    y = z - zhat
    y[1] = wrap_angle(y[1])

    H = H_landmark_jacobian(x, mu)
    S = H @ Sigma @ H.T + R
    K = Sigma @ H.T @ np.linalg.inv(S)

    mu_new = mu + K @ y
    Sigma_new = (np.eye(2) - K @ H) @ Sigma
    return mu_new, Sigma_new, y, S

def mahalanobis2(y: np.ndarray, S: np.ndarray) -> float:
    """Squared Mahalanobis distance y^T S^{-1} y."""
    return float(y.T @ np.linalg.inv(S) @ y)

def initialize_landmark(x: np.ndarray, z: np.ndarray, sigma0: float = 1.0):
    """Inverse range-bearing projection with conservative covariance."""
    r, b = float(z[0]), float(z[1])
    mx = x[0] + r * math.cos(x[2] + b)
    my = x[1] + r * math.sin(x[2] + b)
    mu = np.array([mx, my], dtype=float)
    Sigma = (sigma0**2) * np.eye(2, dtype=float)
    return mu, Sigma

def simulate_world(seed: int = 7):
    rng = np.random.default_rng(seed)
    # True landmarks (unknown to robot)
    true_landmarks = np.array([
        [5.0,  4.0],
        [12.0, 1.0],
        [10.0, 8.0],
        [2.0,  9.0],
        [15.0, 6.0],
    ], dtype=float)

    # Robot poses (assumed known for this lesson): a simple arc
    K = 140
    poses = []
    x, y, th = 0.0, 0.0, 0.0
    v = 0.15
    w = 0.02
    dt = 1.0
    for _ in range(K):
        x += v * math.cos(th) * dt
        y += v * math.sin(th) * dt
        th = wrap_angle(th + w * dt)
        poses.append([x, y, th])
    poses = np.array(poses, dtype=float)

    # Sensor parameters
    R = np.diag([0.15**2, (np.deg2rad(2.0))**2])  # range, bearing noise
    max_range = 8.0
    fov = np.deg2rad(140.0)  # +/- 70 degrees

    # Generate measurements: at each time, observe any landmark in range and FOV
    measurements = []  # list of list of (z, true_id)
    for k in range(K):
        xk = poses[k]
        zk = []
        for i, m in enumerate(true_landmarks):
            z_true = h_range_bearing(xk, m)
            if z_true[0] > max_range:
                continue
            if abs(z_true[1]) > fov/2:
                continue
            noise = rng.multivariate_normal(mean=np.zeros(2), cov=R)
            z_noisy = z_true + noise
            z_noisy[1] = wrap_angle(z_noisy[1])
            zk.append((z_noisy, i))
        measurements.append(zk)

    return poses, true_landmarks, measurements, R

def feature_based_mapping_known_poses(poses, measurements, R, gate_chi2: float = 9.21):
    # Map representation: list of (mu_i, Sigma_i, seen_count)
    mus = []
    Sigmas = []
    counts = []

    for k, xk in enumerate(poses):
        for (z, _) in measurements[k]:
            if len(mus) == 0:
                mu0, S0 = initialize_landmark(xk, z, sigma0=1.5)
                mus.append(mu0)
                Sigmas.append(S0)
                counts.append(1)
                continue

            # Nearest-neighbor association in Mahalanobis sense
            best_i = -1
            best_d2 = float("inf")
            best_cache = None

            for i in range(len(mus)):
                mu_i = mus[i]
                Sigma_i = Sigmas[i]
                mu_upd, Sigma_upd, y, S = ekf_update_landmark(mu_i, Sigma_i, xk, z, R)
                d2 = mahalanobis2(y, S)
                if d2 < best_d2:
                    best_d2 = d2
                    best_i = i
                    best_cache = (mu_upd, Sigma_upd, y, S)

            # Gate
            if best_d2 <= gate_chi2:
                mu_upd, Sigma_upd, _, _ = best_cache
                mus[best_i] = mu_upd
                Sigmas[best_i] = Sigma_upd
                counts[best_i] += 1
            else:
                mu0, S0 = initialize_landmark(xk, z, sigma0=1.5)
                mus.append(mu0)
                Sigmas.append(S0)
                counts.append(1)

    return np.array(mus), np.array(Sigmas), np.array(counts)

def main():
    poses, true_landmarks, measurements, R = simulate_world(seed=4)
    mus, Sigmas, counts = feature_based_mapping_known_poses(poses, measurements, R, gate_chi2=9.21)

    print("True landmarks (unknown to algorithm):")
    print(true_landmarks)
    print("\nEstimated landmarks (mu) and observation counts:")
    for i in range(len(mus)):
        print(f"  i={i:2d}, mu={mus[i]}, count={counts[i]}")

    # Optional visualization
    try:
        import matplotlib.pyplot as plt
        plt.figure()
        plt.plot(poses[:,0], poses[:,1], label="robot path")
        plt.scatter(true_landmarks[:,0], true_landmarks[:,1], marker="x", label="true landmarks")
        plt.scatter(mus[:,0], mus[:,1], marker="o", label="estimated landmarks")
        plt.axis("equal")
        plt.grid(True)
        plt.legend()
        plt.title("Feature-based mapping (known poses) — landmarks")
        plt.show()
    except Exception as e:
        print("Plot skipped:", e)

if __name__ == "__main__":
    main()
      

In C++ robotics stacks, Eigen is the de-facto linear algebra library; mapping pipelines often interact with ROS 2, PCL (for point clouds), and optimization libraries (Ceres, g2o) in later SLAM chapters. The following program implements the same per-landmark EKF update and nearest-neighbor gating in a minimal, single file.

Chapter9_Lesson3.cpp

/*
Chapter9_Lesson3.cpp
Feature-Based Mapping (Landmarks) with known poses:
- Range-bearing measurement model
- Nearest-neighbor association with chi-square gating
- Per-landmark EKF updates

Build (example):
  g++ -O2 -std=c++17 Chapter9_Lesson3.cpp -I/path/to/eigen -o lesson3
*/

#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <limits>

#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::Vector2d hRangeBearing(const Eigen::Vector3d& x, const Eigen::Vector2d& m) {
    double dx = m(0) - x(0);
    double dy = m(1) - x(1);
    double r = std::sqrt(dx*dx + dy*dy);
    double b = wrapAngle(std::atan2(dy, dx) - x(2));
    return Eigen::Vector2d(r, b);
}

static Eigen::Matrix<double,2,2> H_landmark(const Eigen::Vector3d& x, const Eigen::Vector2d& m) {
    double dx = m(0) - x(0);
    double dy = m(1) - x(1);
    double q = dx*dx + dy*dy;
    double r = std::sqrt(std::max(q, 1e-12));
    q = std::max(q, 1e-12);

    Eigen::Matrix<double,2,2> H;
    H(0,0) = dx / r;
    H(0,1) = dy / r;
    H(1,0) = -dy / q;
    H(1,1) =  dx / q;
    return H;
}

struct Landmark {
    Eigen::Vector2d mu;
    Eigen::Matrix2d Sigma;
    int count;
};

static void ekfUpdate(Landmark& lm, const Eigen::Vector3d& x, const Eigen::Vector2d& z, const Eigen::Matrix2d& R,
                      Eigen::Vector2d& innovation, Eigen::Matrix2d& S_out) {
    Eigen::Vector2d zhat = hRangeBearing(x, lm.mu);
    Eigen::Vector2d y = z - zhat;
    y(1) = wrapAngle(y(1));

    Eigen::Matrix2d H = H_landmark(x, lm.mu);
    Eigen::Matrix2d S = H * lm.Sigma * H.transpose() + R;
    Eigen::Matrix2d K = lm.Sigma * H.transpose() * S.inverse();

    lm.mu = lm.mu + K * y;
    lm.Sigma = (Eigen::Matrix2d::Identity() - K * H) * lm.Sigma;

    innovation = y;
    S_out = S;
}

static double maha2(const Eigen::Vector2d& y, const Eigen::Matrix2d& S) {
    return (y.transpose() * S.inverse() * y)(0,0);
}

static Landmark initLandmark(const Eigen::Vector3d& x, const Eigen::Vector2d& z, double sigma0=1.5) {
    double r = z(0);
    double b = z(1);
    double mx = x(0) + r * std::cos(x(2) + b);
    double my = x(1) + r * std::sin(x(2) + b);
    Landmark lm;
    lm.mu = Eigen::Vector2d(mx, my);
    lm.Sigma = (sigma0*sigma0) * Eigen::Matrix2d::Identity();
    lm.count = 1;
    return lm;
}

int main() {
    // True landmarks
    std::vector<Eigen::Vector2d> trueLM = {
        {5.0, 4.0}, {12.0, 1.0}, {10.0, 8.0}, {2.0, 9.0}, {15.0, 6.0}
    };

    // Simulated poses (known)
    const int K = 140;
    std::vector<Eigen::Vector3d> poses;
    poses.reserve(K);
    double x=0, y=0, th=0;
    double v=0.15, w=0.02, dt=1.0;
    for (int k=0; k<K; ++k) {
        x += v * std::cos(th) * dt;
        y += v * std::sin(th) * dt;
        th = wrapAngle(th + w * dt);
        poses.push_back(Eigen::Vector3d(x,y,th));
    }

    // Sensor noise
    Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
    R(0,0) = 0.15*0.15;
    R(1,1) = std::pow(2.0*M_PI/180.0, 2);

    double maxRange = 8.0;
    double fov = 140.0 * M_PI/180.0;

    // Random generator for measurement noise
    std::mt19937 gen(4);
    std::normal_distribution<double> n01(0.0, 1.0);

    auto sampleNoise = [&]() {
        // Simple diagonal covariance sampling
        Eigen::Vector2d e;
        e(0) = std::sqrt(R(0,0)) * n01(gen);
        e(1) = std::sqrt(R(1,1)) * n01(gen);
        return e;
    };

    // Mapping
    std::vector<Landmark> map;
    const double gateChi2 = 9.21; // approx chi2_{2,0.99}

    for (int k=0; k<K; ++k) {
        const auto& xk = poses[k];

        // Generate observations at time k
        std::vector<Eigen::Vector2d> obs;
        for (size_t i=0; i<trueLM.size(); ++i) {
            Eigen::Vector2d zTrue = hRangeBearing(xk, trueLM[i]);
            if (zTrue(0) > maxRange) continue;
            if (std::abs(zTrue(1)) > fov/2.0) continue;
            Eigen::Vector2d z = zTrue + sampleNoise();
            z(1) = wrapAngle(z(1));
            obs.push_back(z);
        }

        for (const auto& z : obs) {
            if (map.empty()) {
                map.push_back(initLandmark(xk, z, 1.5));
                continue;
            }

            int bestIdx = -1;
            double bestD2 = std::numeric_limits<double>::infinity();
            Landmark bestUpdated;
            Eigen::Vector2d bestY;
            Eigen::Matrix2d bestS;

            for (size_t i=0; i<map.size(); ++i) {
                Landmark temp = map[i];
                Eigen::Vector2d yInnov;
                Eigen::Matrix2d S;
                ekfUpdate(temp, xk, z, R, yInnov, S);
                double d2 = maha2(yInnov, S);
                if (d2 < bestD2) {
                    bestD2 = d2;
                    bestIdx = static_cast<int>(i);
                    bestUpdated = temp;
                    bestY = yInnov;
                    bestS = S;
                }
            }

            if (bestD2 <= gateChi2) {
                map[bestIdx] = bestUpdated;
                map[bestIdx].count += 1;
            } else {
                map.push_back(initLandmark(xk, z, 1.5));
            }
        }
    }

    std::cout << "True landmarks (unknown to algorithm):\n";
    for (const auto& m : trueLM) {
        std::cout << "  [" << m(0) << ", " << m(1) << "]\n";
    }

    std::cout << "\nEstimated landmarks:\n";
    for (size_t i=0; i<map.size(); ++i) {
        std::cout << "  i=" << i
                  << " mu=[" << map[i].mu(0) << ", " << map[i].mu(1) << "]"
                  << " count=" << map[i].count << "\n";
    }

    return 0;
}
      

Java robotics software frequently uses matrix libraries such as EJML; here we keep the implementation self-contained by coding 2×2 algebra explicitly. The algorithm remains identical: innovation, Mahalanobis gating, and EKF update.

Chapter9_Lesson3.java

/*
Chapter9_Lesson3.java
Feature-Based Mapping (Landmarks) with known poses:
- Range-bearing measurement model
- Nearest-neighbor association with chi-square gating
- Per-landmark EKF updates
No external libraries; 2x2 matrices implemented manually.
*/

import java.util.*;
import static java.lang.Math.*;

public class Chapter9_Lesson3 {

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

    static class Vec2 {
        double a, b;
        Vec2(double a, double b) { this.a = a; this.b = b; }
        Vec2 add(Vec2 o) { return new Vec2(a + o.a, b + o.b); }
        Vec2 sub(Vec2 o) { return new Vec2(a - o.a, b - o.b); }
    }

    static class Vec3 {
        double x, y, th;
        Vec3(double x, double y, double th) { this.x = x; this.y = y; this.th = th; }
    }

    static class Mat2 {
        double m00, m01, m10, m11;
        Mat2(double m00, double m01, double m10, double m11) {
            this.m00 = m00; this.m01 = m01; this.m10 = m10; this.m11 = m11;
        }
        static Mat2 I() { return new Mat2(1,0,0,1); }
        Mat2 add(Mat2 o) { return new Mat2(m00+o.m00, m01+o.m01, m10+o.m10, m11+o.m11); }
        Mat2 sub(Mat2 o) { return new Mat2(m00-o.m00, m01-o.m01, m10-o.m10, m11-o.m11); }
        Mat2 mul(Mat2 o) {
            return new Mat2(
                m00*o.m00 + m01*o.m10,
                m00*o.m01 + m01*o.m11,
                m10*o.m00 + m11*o.m10,
                m10*o.m01 + m11*o.m11
            );
        }
        Vec2 mul(Vec2 v) {
            return new Vec2(m00*v.a + m01*v.b, m10*v.a + m11*v.b);
        }
        Mat2 T() { return new Mat2(m00, m10, m01, m11); }
        double det() { return m00*m11 - m01*m10; }
        Mat2 inv() {
            double d = det();
            if (abs(d) < 1e-12) d = (d >= 0 ? 1e-12 : -1e-12);
            return new Mat2(m11/d, -m01/d, -m10/d, m00/d);
        }
    }

    static Vec2 hRangeBearing(Vec3 x, Vec2 m) {
        double dx = m.a - x.x;
        double dy = m.b - x.y;
        double r = sqrt(dx*dx + dy*dy);
        double b = wrapAngle(atan2(dy, dx) - x.th);
        return new Vec2(r, b);
    }

    static Mat2 H_landmark(Vec3 x, Vec2 m) {
        double dx = m.a - x.x;
        double dy = m.b - x.y;
        double q = dx*dx + dy*dy;
        q = max(q, 1e-12);
        double r = sqrt(q);
        // [dx/r, dy/r; -dy/q, dx/q]
        return new Mat2(dx/r, dy/r, -dy/q, dx/q);
    }

    static class Landmark {
        Vec2 mu;
        Mat2 Sigma;
        int count;
        Landmark(Vec2 mu, Mat2 Sigma) { this.mu = mu; this.Sigma = Sigma; this.count = 1; }
    }

    static Landmark initLandmark(Vec3 x, Vec2 z, double sigma0) {
        double r = z.a;
        double b = z.b;
        double mx = x.x + r * cos(x.th + b);
        double my = x.y + r * sin(x.th + b);
        Mat2 S0 = new Mat2(sigma0*sigma0, 0, 0, sigma0*sigma0);
        return new Landmark(new Vec2(mx, my), S0);
    }

    static class UpdateCache {
        Landmark updated;
        Vec2 y;
        Mat2 S;
        double d2;
        UpdateCache(Landmark updated, Vec2 y, Mat2 S, double d2) {
            this.updated = updated; this.y = y; this.S = S; this.d2 = d2;
        }
    }

    static UpdateCache ekfUpdateCandidate(Landmark lm, Vec3 x, Vec2 z, Mat2 R) {
        Vec2 zhat = hRangeBearing(x, lm.mu);
        Vec2 y = z.sub(zhat);
        y.b = wrapAngle(y.b);

        Mat2 H = H_landmark(x, lm.mu);
        Mat2 S = H.mul(lm.Sigma).mul(H.T()).add(R);
        Mat2 K = lm.Sigma.mul(H.T()).mul(S.inv());

        Vec2 Ky = K.mul(y);
        Vec2 muNew = lm.mu.add(Ky);

        Mat2 I = Mat2.I();
        Mat2 SigmaNew = (I.sub(K.mul(H))).mul(lm.Sigma);

        // d^2 = y^T S^{-1} y
        Mat2 Sinv = S.inv();
        Vec2 t = Sinv.mul(y);
        double d2 = y.a*t.a + y.b*t.b;

        Landmark upd = new Landmark(muNew, SigmaNew);
        upd.count = lm.count;
        return new UpdateCache(upd, y, S, d2);
    }

    public static void main(String[] args) {
        // True landmarks (unknown to algorithm)
        Vec2[] trueLM = new Vec2[] {
            new Vec2(5.0, 4.0),
            new Vec2(12.0, 1.0),
            new Vec2(10.0, 8.0),
            new Vec2(2.0, 9.0),
            new Vec2(15.0, 6.0)
        };

        // Known poses
        int K = 140;
        ArrayList<Vec3> poses = new ArrayList<>(K);
        double x=0, y=0, th=0;
        double v=0.15, w=0.02, dt=1.0;
        for (int k=0; k<K; k++) {
            x += v*cos(th)*dt;
            y += v*sin(th)*dt;
            th = wrapAngle(th + w*dt);
            poses.add(new Vec3(x,y,th));
        }

        // Sensor noise
        Mat2 R = new Mat2(0.15*0.15, 0, 0, pow(toRadians(2.0),2));
        double maxRange = 8.0;
        double fov = toRadians(140.0);

        Random rng = new Random(4);
        java.util.function.DoubleSupplier n01 = () -> {
            // Box-Muller
            double u1 = max(rng.nextDouble(), 1e-12);
            double u2 = rng.nextDouble();
            return sqrt(-2.0*log(u1)) * cos(2.0*PI*u2);
        };

        java.util.function.Supplier<Vec2> sampleNoise = () -> {
            double e0 = sqrt(R.m00) * n01.getAsDouble();
            double e1 = sqrt(R.m11) * n01.getAsDouble();
            return new Vec2(e0, e1);
        };

        ArrayList<Landmark> map = new ArrayList<>();
        double gateChi2 = 9.21;

        for (int k=0; k<K; k++) {
            Vec3 xk = poses.get(k);

            // Observations at time k
            ArrayList<Vec2> obs = new ArrayList<>();
            for (Vec2 m : trueLM) {
                Vec2 zTrue = hRangeBearing(xk, m);
                if (zTrue.a > maxRange) continue;
                if (abs(zTrue.b) > fov/2.0) continue;
                Vec2 z = zTrue.add(sampleNoise.get());
                z.b = wrapAngle(z.b);
                obs.add(z);
            }

            for (Vec2 z : obs) {
                if (map.isEmpty()) {
                    map.add(initLandmark(xk, z, 1.5));
                    continue;
                }

                int bestIdx = -1;
                double bestD2 = Double.POSITIVE_INFINITY;
                UpdateCache best = null;

                for (int i=0; i<map.size(); i++) {
                    Landmark cand = map.get(i);
                    UpdateCache cache = ekfUpdateCandidate(cand, xk, z, R);
                    if (cache.d2 < bestD2) {
                        bestD2 = cache.d2;
                        bestIdx = i;
                        best = cache;
                    }
                }

                if (bestD2 <= gateChi2) {
                    Landmark upd = best.updated;
                    upd.count = map.get(bestIdx).count + 1;
                    map.set(bestIdx, upd);
                } else {
                    map.add(initLandmark(xk, z, 1.5));
                }
            }
        }

        System.out.println("True landmarks (unknown to algorithm):");
        for (Vec2 m : trueLM) {
            System.out.printf("  [%.3f, %.3f]%n", m.a, m.b);
        }

        System.out.println("\nEstimated landmarks:");
        for (int i=0; i<map.size(); i++) {
            Landmark lm = map.get(i);
            System.out.printf("  i=%2d mu=[%.3f, %.3f] count=%d%n", i, lm.mu.a, lm.mu.b, lm.count);
        }
    }
}
      

MATLAB provides fast prototyping for estimation algorithms. In AMR contexts, the Robotics System Toolbox and Sensor Fusion and Tracking Toolbox provide EKF and sensor interfaces; the script below is a transparent from-scratch implementation aligned with the math above.

Chapter9_Lesson3.m

% Chapter9_Lesson3.m
% Feature-Based Mapping (Landmarks) with known poses:
% - Range-bearing measurement model
% - Nearest-neighbor association with chi-square gating
% - Per-landmark EKF updates

clear; clc;

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

hRangeBearing = @(x, m) [ ...
    hypot(m(1)-x(1), m(2)-x(2)); ...
    wrapAngle(atan2(m(2)-x(2), m(1)-x(1)) - x(3)) ...
];

H_landmark = @(x, m) jacH(x, m);

% ---- Simulation world ----
trueLM = [ ...
    5  4; ...
    12 1; ...
    10 8; ...
    2  9; ...
    15 6 ...
];

K = 140;
poses = zeros(K, 3);
x=0; y=0; th=0;
v=0.15; w=0.02; dt=1.0;
for k=1:K
    x = x + v*cos(th)*dt;
    y = y + v*sin(th)*dt;
    th = wrapAngle(th + w*dt);
    poses(k,:) = [x y th];
end

R = diag([0.15^2, deg2rad(2.0)^2]);
maxRange = 8.0;
fov = deg2rad(140.0);

% Measurements: cell array, measurements{k} is Mx2 matrix of [r, bearing]
measurements = cell(K,1);
rng(4);
for k=1:K
    xk = poses(k,:)';
    Z = [];
    for i=1:size(trueLM,1)
        zTrue = hRangeBearing(xk, trueLM(i,:)');
        if zTrue(1) > maxRange, continue; end
        if abs(zTrue(2)) > fov/2, continue; end
        noise = [sqrt(R(1,1))*randn; sqrt(R(2,2))*randn];
        z = zTrue + noise;
        z(2) = wrapAngle(z(2));
        Z = [Z; z']; %#ok<AGROW>
    end
    measurements{k} = Z;
end

% ---- Mapping ----
mus = zeros(0,2);
Sigmas = zeros(2,2,0);
counts = zeros(0,1);

gateChi2 = 9.21; % approx chi2_{2,0.99}

for k=1:K
    xk = poses(k,:)';
    Zk = measurements{k};
    for j=1:size(Zk,1)
        z = Zk(j,:)';
        if isempty(mus)
            [mu0, S0] = initLandmark(xk, z, 1.5);
            mus = [mus; mu0']; %#ok<AGROW>
            Sigmas(:,:,end+1) = S0; %#ok<SAGROW>
            counts(end+1,1) = 1; %#ok<AGROW>
            continue;
        end

        bestD2 = inf;
        bestIdx = -1;
        bestMu = [];
        bestS = [];

        for i=1:size(mus,1)
            mu = mus(i,:)';
            Sigma = Sigmas(:,:,i);
            [muU, SigmaU, y, S] = ekfUpdate(mu, Sigma, xk, z, R, hRangeBearing, H_landmark, wrapAngle);
            d2 = y'*(S\y);
            if d2 < bestD2
                bestD2 = d2;
                bestIdx = i;
                bestMu = muU;
                bestS = SigmaU;
            end
        end

        if bestD2 <= gateChi2
            mus(bestIdx,:) = bestMu';
            Sigmas(:,:,bestIdx) = bestS;
            counts(bestIdx) = counts(bestIdx) + 1;
        else
            [mu0, S0] = initLandmark(xk, z, 1.5);
            mus = [mus; mu0']; %#ok<AGROW>
            Sigmas(:,:,end+1) = S0; %#ok<SAGROW>
            counts(end+1,1) = 1; %#ok<AGROW>
        end
    end
end

disp("True landmarks:");
disp(trueLM);
disp("Estimated landmarks (mu) and counts:");
disp([mus counts]);

% Plot
figure; hold on; grid on; axis equal;
plot(poses(:,1), poses(:,2), 'LineWidth', 1.2);
scatter(trueLM(:,1), trueLM(:,2), 60, 'x', 'LineWidth', 1.5);
scatter(mus(:,1), mus(:,2), 50, 'o', 'LineWidth', 1.2);
legend('robot path', 'true landmarks', 'estimated landmarks');
title('Feature-based mapping (known poses) — landmarks');

% ---- Local functions ----
function H = jacH(x, m)
    dx = m(1) - x(1);
    dy = m(2) - x(2);
    q = dx*dx + dy*dy;
    q = max(q, 1e-12);
    r = sqrt(q);
    H = [dx/r,  dy/r; ...
         -dy/q, dx/q];
end

function [mu, Sigma] = initLandmark(x, z, sigma0)
    r = z(1); b = z(2);
    mx = x(1) + r*cos(x(3) + b);
    my = x(2) + r*sin(x(3) + b);
    mu = [mx; my];
    Sigma = (sigma0^2) * eye(2);
end

function [muNew, SigmaNew, y, S] = ekfUpdate(mu, Sigma, x, z, R, hFun, HFun, wrapAngle)
    zhat = hFun(x, mu);
    y = z - zhat;
    y(2) = wrapAngle(y(2));
    H = HFun(x, mu);
    S = H*Sigma*H' + R;
    K = Sigma*H' / S;
    muNew = mu + K*y;
    SigmaNew = (eye(2) - K*H)*Sigma;
end
      

The next file builds a simple Simulink model programmatically (so it can be version-controlled as a script). It streams pose and measurement time series from the workspace into a MATLAB Function block that performs a single-landmark EKF update.

Chapter9_Lesson3_Simulink.m

% Chapter9_Lesson3_Simulink.m
% Programmatically create a minimal Simulink model demonstrating a per-landmark EKF update.
%
% Notes:
% - This model is pedagogical: it updates a single landmark state (mu,Sigma) driven by pose and measurement streams.
% - The logic is implemented in a MATLAB Function block for transparency.

clear; clc;

modelName = 'Chapter9_Lesson3_Simulink_Model';
if bdIsLoaded(modelName)
    close_system(modelName, 0);
end
if exist([modelName '.slx'], 'file')
    delete([modelName '.slx']);
end

new_system(modelName);
open_system(modelName);

% Create basic blocks
add_block('simulink/Sources/From Workspace', [modelName '/Pose'], 'Position', [40 60 180 90]);
add_block('simulink/Sources/From Workspace', [modelName '/Meas'], 'Position', [40 130 180 160]);
add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/EKF_Update'], 'Position', [260 70 450 160]);
add_block('simulink/Sinks/To Workspace', [modelName '/Mu_out'], 'Position', [520 70 660 100]);
add_block('simulink/Sinks/To Workspace', [modelName '/Sigma_out'], 'Position', [520 130 660 160]);

set_param([modelName '/Pose'], 'VariableName', 'pose_ts');
set_param([modelName '/Meas'], 'VariableName', 'meas_ts');
set_param([modelName '/Mu_out'], 'VariableName', 'mu_hist');
set_param([modelName '/Sigma_out'], 'VariableName', 'Sigma_hist');

% Connect lines
add_line(modelName, 'Pose/1', 'EKF_Update/1');
add_line(modelName, 'Meas/1', 'EKF_Update/2');
add_line(modelName, 'EKF_Update/1', 'Mu_out/1');
add_line(modelName, 'EKF_Update/2', 'Sigma_out/1');

% Define time series inputs
K = 60;
dt = 0.1;
t = (0:K-1)'*dt;

% Pose stream: [x y theta]
pose = zeros(K,3);
x=0; y=0; th=0;
v=0.2; w=0.05;
for k=1:K
    x = x + v*cos(th)*dt;
    y = y + v*sin(th)*dt;
    th = wrapToPi(th + w*dt);
    pose(k,:) = [x y th];
end

% Assume a fixed true landmark for measurement generation
mTrue = [6.0; 3.0];
R = diag([0.10^2, deg2rad(1.5)^2]);
meas = zeros(K,2);
rng(1);
for k=1:K
    dx = mTrue(1) - pose(k,1);
    dy = mTrue(2) - pose(k,2);
    r = hypot(dx,dy);
    b = wrapToPi(atan2(dy,dx) - pose(k,3));
    noise = [sqrt(R(1,1))*randn; sqrt(R(2,2))*randn];
    z = [r; b] + noise;
    z(2) = wrapToPi(z(2));
    meas(k,:) = z';
end

pose_ts = timeseries(pose, t);
meas_ts = timeseries(meas, t);

% Configure MATLAB Function block code
code = [
"function [mu, Sigma] = f(pose_in, meas_in)\n" ...
"% pose_in: [x y theta], meas_in: [r bearing]\n" ...
"persistent mu_k Sigma_k\n" ...
"if isempty(mu_k)\n" ...
"    % Initialize from first measurement, conservative covariance\n" ...
"    x = pose_in(1); y = pose_in(2); th = pose_in(3);\n" ...
"    r = meas_in(1); b = meas_in(2);\n" ...
"    mu_k = [x + r*cos(th+b); y + r*sin(th+b)];\n" ...
"    Sigma_k = 1.0^2 * eye(2);\n" ...
"end\n" ...
"\n" ...
"% EKF update\n" ...
"x = pose_in(1); y = pose_in(2); th = pose_in(3);\n" ...
"mx = mu_k(1); my = mu_k(2);\n" ...
"dx = mx - x; dy = my - y;\n" ...
"q = dx*dx + dy*dy; q = max(q, 1e-12);\n" ...
"rhat = sqrt(q);\n" ...
"bhat = wrapToPi(atan2(dy,dx) - th);\n" ...
"zhat = [rhat; bhat];\n" ...
"z = meas_in(:);\n" ...
"innov = z - zhat;\n" ...
"innov(2) = wrapToPi(innov(2));\n" ...
"\n" ...
"H = [dx/rhat, dy/rhat; -dy/q, dx/q];\n" ...
"R = diag([0.10^2, deg2rad(1.5)^2]);\n" ...
"S = H*Sigma_k*H' + R;\n" ...
"K = Sigma_k*H'/S;\n" ...
"mu_k = mu_k + K*innov;\n" ...
"Sigma_k = (eye(2) - K*H)*Sigma_k;\n" ...
"\n" ...
"mu = mu_k;\n" ...
"Sigma = Sigma_k;\n" ...
"end\n"
];

set_param([modelName '/EKF_Update'], 'Script', code);

% Simulation settings
set_param(modelName, 'StopTime', num2str(t(end)));
save_system(modelName);

disp("Simulink model created: " + modelName + ".slx");
disp("Run simulation: sim(modelName)");
      

Mathematica is useful for symbolic derivations (Jacobians, linearizations) and rapid validation of estimation equations. The following notebook-style script defines the range–bearing model, Jacobian, EKF update, and runs a small simulation.

Chapter9_Lesson3.nb

(* Chapter9_Lesson3.nb (Mathematica script content)
   Feature-Based Mapping (Landmarks) with known poses:
   - Symbolic Jacobian for range-bearing
   - Numeric EKF update
*)

ClearAll["Global`*"];

wrapAngle[a_] := Module[{b = Mod[a + Pi, 2 Pi] - Pi}, b];

hRangeBearing[x_, m_] := Module[{dx, dy, r, b},
  dx = m[[1]] - x[[1]];
  dy = m[[2]] - x[[2]];
  r = Sqrt[dx^2 + dy^2];
  b = wrapAngle[ArcTan[dx, dy] - x[[3]]]; (* ArcTan[x,y] in Mathematica is atan2(y,x) style *)
  {r, b}
];

(* Symbolic Jacobian w.r.t. landmark coords *)
dx = mx - x;
dy = my - y;
q = dx^2 + dy^2;
r = Sqrt[q];
phi = ArcTan[dx, dy] - th;

hSym = {r, phi};
Hsym = D[hSym, { {mx, my} }] // Simplify;
Print["Symbolic H = dh/dm:"];
Print[Hsym];

(* EKF update step for a landmark *)
ekfUpdate[mu_, Sigma_, xpose_, z_, R_] := Module[
  {zhat, yinnov, H, S, K, muNew, SigmaNew},
  zhat = hRangeBearing[xpose, mu];
  yinnov = z - zhat;
  yinnov[[2]] = wrapAngle[yinnov[[2]]];

  (* Numeric Jacobian *)
  H = Module[{dxn, dyn, qn, rn},
    dxn = mu[[1]] - xpose[[1]];
    dyn = mu[[2]] - xpose[[2]];
    qn = Max[dxn^2 + dyn^2, 10^-12];
    rn = Sqrt[qn];
    { {dxn/rn, dyn/rn}, {-dyn/qn, dxn/qn} }
  ];

  S = H . Sigma . Transpose[H] + R;
  K = Sigma . Transpose[H] . Inverse[S];
  muNew = mu + K . yinnov;
  SigmaNew = (IdentityMatrix[2] - K . H) . Sigma;
  {muNew, SigmaNew, yinnov, S}
];

(* Small numeric demo *)
R = DiagonalMatrix[{0.15^2, (2 Degree)^2}];
mu0 = {4.0, 2.0};
Sigma0 = 1.5^2 IdentityMatrix[2];
xpose = {1.0, 0.5, 0.2};

z = hRangeBearing[xpose, {5.0, 4.0}] + RandomVariate[MultinormalDistribution[{0, 0}, R]];
z[[2]] = wrapAngle[z[[2]]];

{mu1, Sigma1, innov, S} = ekfUpdate[mu0, Sigma0, xpose, z, R];
Print["Initial mu0 = ", mu0];
Print["Updated mu1 = ", mu1];
Print["Innovation = ", innov];
Print["Sigma1 = ", Sigma1 // MatrixForm];
      

Problem 1 (Inverse Sensor Model): A robot at pose \( \mathbf{x}_k=(x_k,y_k,\theta_k) \) observes a point landmark with range–bearing \( \mathbf{z}=(r,\varphi) \). Derive the landmark position \( \mathbf{m} \) in the global frame.

Solution: Using basic planar geometry:

\[ \mathbf{m} = \begin{bmatrix} x_k + r\cos(\theta_k+\varphi) \\ y_k + r\sin(\theta_k+\varphi) \end{bmatrix}. \]

Problem 2 (Jacobian for Range–Bearing): For \( r = \sqrt{(m^x-x)^2+(m^y-y)^2} \) and \( \varphi = \operatorname{atan2}(m^y-y, m^x-x) - \theta \), compute \( \mathbf{H}=\frac{\partial (r,\varphi)}{\partial (m^x,m^y)} \).

Solution: Let \( dx=m^x-x \), \( dy=m^y-y \), \( q=dx^2+dy^2 \), \( r=\sqrt{q} \). Then

\[ \mathbf{H} = \begin{bmatrix} \frac{dx}{r} & \frac{dy}{r} \\ -\frac{dy}{q} & \frac{dx}{q} \end{bmatrix}. \]

Problem 3 (Gating as a Hypothesis Test): Show that if the innovation \( \mathbf{y} \) is Gaussian with covariance \( \mathbf{S} \), then \( d^2 = \mathbf{y}^\top \mathbf{S}^{-1} \mathbf{y} \) has a chi-square distribution.

Solution: If \( \mathbf{y}\sim\mathcal{N}(\mathbf{0},\mathbf{S}) \), define \( \mathbf{u}=\mathbf{S}^{-1/2}\mathbf{y} \). Then \( \mathbf{u}\sim\mathcal{N}(\mathbf{0},\mathbf{I}) \) and \( d^2 = \|\mathbf{u}\|^2 = \sum_{j=1}^m u_j^2 \). The sum of squares of \( m \) i.i.d. standard normals is \( \chi^2_m \). For range–bearing, \( m=2 \).

Problem 4 (One-Step Gaussian Update as MAP): Consider the linear model \( \mathbf{z}=\mathbf{H}\mathbf{m}+\mathbf{v} \), \( \mathbf{v}\sim\mathcal{N}(\mathbf{0},\mathbf{R}) \), with prior \( \mathbf{m}\sim\mathcal{N}(\boldsymbol{\mu},\mathbf{\Sigma}) \). Show that the posterior mean equals the minimizer of a quadratic objective and matches the Kalman update.

Solution: The MAP estimator minimizes the negative log posterior:

\[ J(\mathbf{m}) = \tfrac{1}{2}(\mathbf{m}-\boldsymbol{\mu})^\top\mathbf{\Sigma}^{-1}(\mathbf{m}-\boldsymbol{\mu}) + \tfrac{1}{2}(\mathbf{z}-\mathbf{H}\mathbf{m})^\top\mathbf{R}^{-1}(\mathbf{z}-\mathbf{H}\mathbf{m}). \]

Taking gradients and setting to zero yields: \( (\mathbf{\Sigma}^{-1}+\mathbf{H}^\top\mathbf{R}^{-1}\mathbf{H})\hat{\mathbf{m} } = \mathbf{\Sigma}^{-1}\boldsymbol{\mu} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{z} \). Solving and rewriting with the matrix inversion lemma produces the Kalman gain form: \( \hat{\mathbf{m} } = \boldsymbol{\mu} + \mathbf{K}(\mathbf{z}-\mathbf{H}\boldsymbol{\mu}) \), \( \mathbf{K} = \mathbf{\Sigma}\mathbf{H}^\top(\mathbf{H}\mathbf{\Sigma}\mathbf{H}^\top+\mathbf{R})^{-1} \).

Problem 5 (Complexity Comparison): Compare the memory complexity of occupancy grid mapping versus point-landmark mapping for a map covering an area of size \( A \) with grid resolution \( \Delta \) and a landmark density of \( \rho \) landmarks per unit area.

Solution: A 2D grid has about \( A/\Delta^2 \) cells, so memory is \( \mathcal{O}(A/\Delta^2) \). A landmark map has \( N \approx \rho A \) landmarks, each storing mean and covariance (constant size), so memory is \( \mathcal{O}(\rho A) \). The grid grows quadratically with finer resolution (smaller \( \Delta \)), while landmark memory grows linearly with area and landmark density.

We formulated landmark maps as sparse probabilistic representations, derived the Bayesian recursion under known poses, proved per-landmark factorization under conditional independence, and developed Gaussian (EKF-style) landmark updates. We then addressed the key practical bottleneck—data association—via Mahalanobis distance and chi-square gating, and implemented the full loop in Python, C++, Java, MATLAB/Simulink, and Mathematica. These ideas will directly support SLAM in later chapters where pose uncertainty couples landmarks.