In EKF-SLAM, the state mean stacks robot pose and landmark states: \( \boldsymbol{\mu}_k = [x_k, y_k, \theta_k, m_{1x}, m_{1y}, \dots, m_{Nx}, m_{Ny}]^\top \), with covariance \( \mathbf{P}_k \). A sensor produces a set of measurements at time \( k \), for example range–bearing returns \( \mathbf{z}_k^i \in \mathbb{R}^2 \).

The filter update needs a correspondence variable \( c_k^i \) that identifies which mapped landmark generated measurement \( i \). If \( c_k^i \) is wrong, the update injects a systematic error that can rapidly make both the robot and map estimates inconsistent.

flowchart TD
  A["Predicted belief: mu_{k|k-1}, P_{k|k-1}"] --> B["Incoming measurements: z_k^1..z_k^M"]
  B --> C["For each z_k^i: predict zhat^j for all landmarks j"]
  C --> D["Compute innovation nu_ij and S_ij"]
  D --> E["Gate: accept pairs with d2_ij = nu_ij^T * inv(S_ij) * nu_ij <= gamma"]
  E --> F["Decide association: NN / global assignment / JCBB"]
  F --> G["Update filter for \nassociated measurements"]
  F --> H["If no match: new landmark init \nor reject as outlier"]
  G --> I["mu_k, P_k (posterior)"]
  H --> I
        

The rest of this lesson answers three questions: (i) how to derive the gate \( d^2 \) and its threshold statistically, (ii) why independent “nearest-neighbor” decisions fail for multiple measurements, and (iii) what structural properties of SLAM make association harder than in pure localization.

Assume a (possibly nonlinear) landmark sensor model with additive Gaussian noise:

\[ \mathbf{z}_k^i = h(\mathbf{x}_k, \mathbf{m}_{c_k^i}) + \mathbf{v}_k^i, \quad \mathbf{v}_k^i \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \]

Given a candidate landmark \( j \), define the predicted measurement \( \hat{\mathbf{z} }_k^j = h(\hat{\mathbf{x} }_{k|k-1}, \hat{\mathbf{m} }_j) \) and innovation (residual) \( \boldsymbol{\nu}_k^{i,j} = \mathbf{z}_k^i - \hat{\mathbf{z} }_k^j \). Under first-order linearization, the innovation covariance is

\[ \mathbf{S}_k^{i,j} = \mathbf{H}_k^{i,j}\,\mathbf{P}_{k|k-1}\,(\mathbf{H}_k^{i,j})^\top + \mathbf{R}, \quad \mathbf{H}_k^{i,j} = \left.\frac{\partial h}{\partial \boldsymbol{\mu} }\right|_{\boldsymbol{\mu}_{k|k-1},\,j} \]

The Normalized Innovation Squared (NIS) / Mahalanobis distance is \( d_{i,j}^2 \):

\[ d_{i,j}^2 = (\boldsymbol{\nu}_k^{i,j})^\top (\mathbf{S}_k^{i,j})^{-1}\,\boldsymbol{\nu}_k^{i,j}. \]

Proposition (Chi-square gating). If the candidate association is correct, linearization is accurate, and the filter covariance is consistent, then \( d_{i,j}^2 \sim \chi^2_m \) where \( m = \dim(\mathbf{z}) \) (e.g., \( m=2 \) for range–bearing).

Proof sketch. Under the above assumptions, the innovation is Gaussian \( \boldsymbol{\nu} \sim \mathcal{N}(\mathbf{0}, \mathbf{S}) \). Let \( \mathbf{w} = \mathbf{S}^{-1/2}\boldsymbol{\nu} \). Then \( \mathbf{w} \sim \mathcal{N}(\mathbf{0}, \mathbf{I}_m) \) and \( d^2 = \boldsymbol{\nu}^\top \mathbf{S}^{-1} \boldsymbol{\nu} = \mathbf{w}^\top \mathbf{w} = \sum_{\ell=1}^m w_\ell^2 \), which is chi-square with \( m \) degrees of freedom.

Therefore, a statistical gate at confidence level \( \alpha \) is \( d_{i,j}^2 \le \gamma_\alpha \) with \( \gamma_\alpha = F_{\chi^2_m}^{-1}(\alpha) \). For range–bearing, the common choice \( \alpha=0.99 \) yields a relatively permissive gate.

After gating, one must still choose among remaining candidates. The (approximate) likelihood that landmark \( j \) generated measurement \( i \) is

\[ \mathcal{L}(j \mid \mathbf{z}_k^i) \propto \frac{\exp\!\left(-\tfrac{1}{2} d_{i,j}^2\right)}{\sqrt{\det(\mathbf{S}_k^{i,j})} }. \]

If we attach a prior probability \( \pi_j \) for observing landmark \( j \) (visibility, field-of-view, persistence), a MAP rule selects \( j^\star = \arg\max_j \; \pi_j\,\mathcal{L}(j \mid \mathbf{z}_k^i) \).

SLAM-specific coupling. Unlike localization on a fixed map, the covariance \( \mathbf{P} \) in SLAM couples robot pose and landmarks, so innovations for different candidates are not independent. Intuitively, if the pose estimate shifts, the predicted bearings/ranges to all landmarks shift together. This coupling is a key reason why greedy, per-measurement choices can be wrong even when each individual match looks plausible.

With \( M \) measurements in a scan, deciding correspondences independently can yield conflicts (two measurements assigned to one landmark) and can ignore the fact that the set of assignments must be jointly consistent. For an association set \( \mathcal{A} = \{(i_1,j_1),\dots,(i_K,j_K)\} \), define a stacked innovation:

\[ \boldsymbol{\nu}_\mathcal{A} = \begin{bmatrix} \boldsymbol{\nu}_k^{i_1,j_1} \\ \vdots \\ \boldsymbol{\nu}_k^{i_K,j_K} \end{bmatrix}, \quad \mathbf{S}_\mathcal{A} = \mathbf{H}_\mathcal{A}\mathbf{P}\mathbf{H}_\mathcal{A}^\top + \mathbf{R}_\mathcal{A} \]

The joint NIS test is:

\[ d_\mathcal{A}^2 = \boldsymbol{\nu}_\mathcal{A}^\top \mathbf{S}_\mathcal{A}^{-1} \boldsymbol{\nu}_\mathcal{A} \;\le\; F_{\chi^2_{mK} }^{-1}(\alpha), \quad m=\dim(\mathbf{z}). \]

flowchart TD
  S["Start with i=1"] --> B["Branch: choose landmark j from gated candidates"]
  B --> T["Check joint test for current pairs"]
  T -->|"pass"| NXT["i = i + 1 (extend partial assignment)"]
  T -->|"fail"| PR["Prune branch"]
  NXT --> B
  B --> U["Alternative branch: mark measurement i unassigned"]
  U --> T2["Joint test (unchanged)"]
  T2 -->|"pass"| NXT
  T2 -->|"fail"| PR
  NXT -->|"i > M"| OUT["Return best set (max K)"]
        

JCBB searches over assignments and prunes branches that fail joint compatibility early. It is exponential in the worst case, but becomes practical when individual gates remove most candidates.

• Perceptual aliasing: repeated structures (doors, pillars, shelves) create ambiguous matches.
• Clutter and spurious returns: dynamic obstacles produce measurements with no corresponding landmark.
• Missed detections / occlusions: correct landmarks disappear temporarily; greedy rules can “steal” observations.
• Linearization error: EKF linearization can underestimate uncertainty, shrinking gates and causing false rejections.
• Correlation growth: pose–map correlations create joint constraints not captured by independent matching.

Mitigations: conservative gating early, delayed initialization (confirm landmarks over multiple frames), explicit “null” hypothesis, and adding discriminative descriptors (if available) to reduce aliasing.

# Chapter11_Lesson4.py
"""
AMR Course — Chapter 11 (SLAM I — Filter-Based SLAM)
Lesson 4: Data Association Challenges

Implements innovation-based data association for EKF-SLAM:
  - Chi-square gating with Normalized Innovation Squared (NIS)
  - Nearest-Neighbor (NN) association
  - Joint Compatibility Branch-and-Bound (JCBB) for small measurement sets

State mean:
  mu = [x, y, theta, m1x, m1y, ..., mNx, mNy]^T
Measurement:
  z = [range, bearing]^T (bearing relative to robot heading)

Dependencies: numpy (required). SciPy is NOT required.
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import List, Tuple, Optional, Dict

import numpy as np


# ---------------------------
# Numerics
# ---------------------------
def wrap_to_pi(a: float) -> float:
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def erfinv_winitzki(x: float) -> float:
    x = min(0.999999, max(-0.999999, x))
    a = 0.147
    ln = math.log(1.0 - x * x)
    t = 2.0 / (math.pi * a) + ln / 2.0
    return math.copysign(math.sqrt(math.sqrt(t * t - ln / a) - t), x)


def chi2inv_approx(p: float, dof: int) -> float:
    """
    Chi-square inverse CDF via Wilson–Hilferty approximation.
    Good enough for gating thresholds in demos.
    """
    z = math.sqrt(2.0) * erfinv_winitzki(2.0 * p - 1.0)  # Normal quantile approx
    k = float(dof)
    return k * (1.0 - 2.0 / (9.0 * k) + z * math.sqrt(2.0 / (9.0 * k))) ** 3


# ---------------------------
# EKF-SLAM container (mean + covariance)
# ---------------------------
@dataclass
class EKFSLAMState:
    mu: np.ndarray  # (3+2N,)
    P: np.ndarray   # (3+2N, 3+2N)
    N: int

    @staticmethod
    def demo(pose: np.ndarray, landmarks: np.ndarray, seed: int = 7) -> "EKFSLAMState":
        N = landmarks.shape[0]
        mu = np.concatenate([pose, landmarks.reshape(-1)], axis=0)

        dim = 3 + 2 * N
        rng = np.random.RandomState(seed)
        A = rng.randn(dim, dim)
        P = A @ A.T

        S = np.diag([0.2, 0.2, 0.05] + [0.5] * (2 * N))
        P = S @ (P / np.max(np.diag(P))) @ S
        return EKFSLAMState(mu=mu, P=P, N=N)


# ---------------------------
# Range-bearing model and Jacobian
# ---------------------------
def predict_z(pose: np.ndarray, lm: np.ndarray) -> np.ndarray:
    dx = lm[0] - pose[0]
    dy = lm[1] - pose[1]
    r = math.hypot(dx, dy)
    b = wrap_to_pi(math.atan2(dy, dx) - pose[2])
    return np.array([r, b], dtype=float)


def jacobian_H(pose: np.ndarray, lm: np.ndarray, j: int, N: int) -> np.ndarray:
    dx = lm[0] - pose[0]
    dy = lm[1] - pose[1]
    q = dx * dx + dy * dy
    r = math.sqrt(q)

    H = np.zeros((2, 3 + 2 * N), dtype=float)

    # wrt pose (x,y,theta)
    H[0, 0] = -dx / r
    H[0, 1] = -dy / r
    H[1, 0] = dy / q
    H[1, 1] = -dx / q
    H[1, 2] = -1.0

    # wrt landmark j (mx,my)
    idx = 3 + 2 * j
    H[0, idx + 0] = dx / r
    H[0, idx + 1] = dy / r
    H[1, idx + 0] = -dy / q
    H[1, idx + 1] = dx / q
    return H


def innovation_and_S(st: EKFSLAMState, z: np.ndarray, j: int, R: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    pose = st.mu[0:3]
    lm = st.mu[3 + 2 * j: 3 + 2 * j + 2]
    zhat = predict_z(pose, lm)

    nu = z - zhat
    nu[1] = wrap_to_pi(float(nu[1]))

    H = jacobian_H(pose, lm, j, st.N)
    S = H @ st.P @ H.T + R
    return nu, S


def nis(nu: np.ndarray, S: np.ndarray) -> float:
    return float(nu.T @ np.linalg.solve(S, nu))


# ---------------------------
# NN association (single measurement)
# ---------------------------
def associate_nn(st: EKFSLAMState, z: np.ndarray, R: np.ndarray, gate_prob: float = 0.99) -> Optional[int]:
    gamma = chi2inv_approx(gate_prob, 2)
    best_j, best_d2 = None, float("inf")

    for j in range(st.N):
        nu, S = innovation_and_S(st, z, j, R)
        d2 = nis(nu, S)
        if d2 <= gamma and d2 < best_d2:
            best_j, best_d2 = j, d2
    return best_j


# ---------------------------
# JCBB (multiple measurements)
# ---------------------------
def joint_compatibility(st: EKFSLAMState,
                        pairs: List[Tuple[int, int]],
                        Z: List[np.ndarray],
                        R: np.ndarray,
                        gate_prob: float = 0.99) -> bool:
    k = len(pairs)
    if k == 0:
        return True

    nus, Hs = [], []
    for (i, j) in pairs:
        pose = st.mu[0:3]
        lm = st.mu[3 + 2 * j: 3 + 2 * j + 2]
        zhat = predict_z(pose, lm)
        nu = Z[i] - zhat
        nu[1] = wrap_to_pi(float(nu[1]))
        H = jacobian_H(pose, lm, j, st.N)
        nus.append(nu)
        Hs.append(H)

    nu_stack = np.concatenate(nus, axis=0)
    H_stack = np.vstack(Hs)
    R_stack = np.kron(np.eye(k), R)
    S = H_stack @ st.P @ H_stack.T + R_stack

    d2 = float(nu_stack.T @ np.linalg.solve(S, nu_stack))
    gamma = chi2inv_approx(gate_prob, 2 * k)
    return d2 <= gamma


def jcbb(st: EKFSLAMState, Z: List[np.ndarray], R: np.ndarray, gate_prob: float = 0.99) -> Dict[int, int]:
    """
    Returns meas_index -> landmark_index for the *largest* jointly compatible set.
    Complexity is exponential in worst case: keep M small (e.g., M<=6).
    """
    M, N = len(Z), st.N
    gamma2 = chi2inv_approx(gate_prob, 2)

    candidates: List[List[int]] = []
    for i in range(M):
        js = []
        for j in range(N):
            nu, S = innovation_and_S(st, Z[i], j, R)
            if nis(nu, S) <= gamma2:
                js.append(j)
        candidates.append(js)

    best: List[Tuple[int, int]] = []

    def rec(i: int, used: set, pairs: List[Tuple[int, int]]) -> None:
        nonlocal best
        if len(pairs) + (M - i) < len(best):
            return
        if i == M:
            if len(pairs) > len(best):
                best = pairs.copy()
            return

        for j in candidates[i]:
            if j in used:
                continue
            new_pairs = pairs + [(i, j)]
            if joint_compatibility(st, new_pairs, Z, R, gate_prob):
                used.add(j)
                rec(i + 1, used, new_pairs)
                used.remove(j)

        rec(i + 1, used, pairs)  # leave measurement i unassigned

    rec(0, set(), [])
    return {i: j for (i, j) in best}


# ---------------------------
# Demo
# ---------------------------
def main() -> None:
    np.set_printoptions(precision=3, suppress=True)

    pose_true = np.array([2.0, 1.0, 0.4])
    lms_true = np.array([[5.0, 2.0],
                         [4.0, -1.5],
                         [1.0, 4.0],
                         [7.0, 5.0],
                         [6.0, -2.0]], dtype=float)

    pose_est = pose_true + np.array([0.1, -0.05, 0.03])
    lms_est = lms_true + np.array([0.2, -0.1])

    st = EKFSLAMState.demo(pose_est, lms_est)

    sigma_r = 0.15
    sigma_b = math.radians(2.0)
    R = np.diag([sigma_r**2, sigma_b**2])

    # Three noisy measurements from landmarks 0,2,4
    rng = np.random.RandomState(1)
    ids = [0, 2, 4]
    Z = []
    for idx in ids:
        z = predict_z(pose_true, lms_true[idx]) + rng.multivariate_normal(np.zeros(2), R)
        z[1] = wrap_to_pi(float(z[1]))
        Z.append(z)

    print("Measurements:")
    for i, z in enumerate(Z):
        print(f"  z[{i}] = {z}")

    print("\nNN associations:")
    for i, z in enumerate(Z):
        print(f"  meas {i} -> {associate_nn(st, z, R, 0.99)}")

    print("\nJCBB associations:")
    print(" ", jcbb(st, Z, R, 0.99))


if __name__ == "__main__":
    main()
      

// Chapter11_Lesson4.cpp
// Chapter 11 (SLAM I) — Lesson 4: Data Association Challenges
// Minimal C++ demo: chi-square gating + nearest-neighbor association (range-bearing).
//
// Dependency: Eigen (header-only)

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

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

// Wilson–Hilferty chi2 inverse (approx) for gating.
static double erfinvApprox(double x) {
    x = std::max(-0.999999, std::min(0.999999, x));
    const double a = 0.147;
    const double ln = std::log(1.0 - x * x);
    const double t  = 2.0 / (M_PI * a) + ln / 2.0;
    const double s  = std::sqrt(std::sqrt(t * t - ln / a) - t);
    return (x >= 0 ? s : -s);
}
static double chi2invApprox(double p, int dof) {
    const double z = std::sqrt(2.0) * erfinvApprox(2.0 * p - 1.0);
    const double k = static_cast<double>(dof);
    return k * std::pow(1.0 - 2.0 / (9.0 * k) + z * std::sqrt(2.0 / (9.0 * k)), 3.0);
}

static Eigen::Vector2d predictZ(const Eigen::Vector3d& pose, const Eigen::Vector2d& lm) {
    const double dx = lm(0) - pose(0);
    const double dy = lm(1) - pose(1);
    const double r  = std::hypot(dx, dy);
    const double b  = wrapToPi(std::atan2(dy, dx) - pose(2));
    return {r, b};
}

static Eigen::MatrixXd jacobianH(const Eigen::Vector3d& pose, const Eigen::Vector2d& lm, int j, int N) {
    const double dx = lm(0) - pose(0);
    const double dy = lm(1) - pose(1);
    const double q  = dx*dx + dy*dy;
    const double r  = std::sqrt(q);

    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, 3 + 2*N);

    H(0,0) = -dx/r;  H(0,1) = -dy/r;
    H(1,0) =  dy/q;  H(1,1) = -dx/q;  H(1,2) = -1.0;

    const int idx = 3 + 2*j;
    H(0,idx+0) = dx/r;   H(0,idx+1) = dy/r;
    H(1,idx+0) = -dy/q;  H(1,idx+1) = dx/q;
    return H;
}

static double nis(const Eigen::Vector2d& nu, const Eigen::Matrix2d& S) {
    return nu.transpose() * S.ldlt().solve(nu);
}

static int associateNN(const Eigen::VectorXd& mu,
                       const Eigen::MatrixXd& P,
                       int N,
                       const Eigen::Vector2d& z,
                       const Eigen::Matrix2d& R,
                       double gateProb = 0.99) {
    const double gamma = chi2invApprox(gateProb, 2);
    int bestJ = -1;
    double bestD2 = std::numeric_limits<double>::infinity();

    Eigen::Vector3d pose = mu.segment<3>(0);

    for (int j = 0; j < N; ++j) {
        Eigen::Vector2d lm = mu.segment<2>(3 + 2*j);
        Eigen::Vector2d zhat = predictZ(pose, lm);

        Eigen::Vector2d nu = z - zhat;
        nu(1) = wrapToPi(nu(1));

        Eigen::MatrixXd H = jacobianH(pose, lm, j, N);
        Eigen::Matrix2d S = (H * P * H.transpose()).eval() + R;

        const double d2 = nis(nu, S);
        if (d2 <= gamma && d2 < bestD2) { bestD2 = d2; bestJ = j; }
    }
    return bestJ; // -1: no association
}

int main() {
    Eigen::Vector3d poseTrue(2.0, 1.0, 0.4);
    std::vector<Eigen::Vector2d> lms = { {5,2},{4,-1.5},{1,4},{7,5},{6,-2} };

    Eigen::Vector3d poseEst = poseTrue + Eigen::Vector3d(0.1,-0.05,0.03);
    std::vector<Eigen::Vector2d> lmsEst;
    for (auto& p : lms) lmsEst.push_back(p + Eigen::Vector2d(0.2,-0.1));

    const int N = (int)lmsEst.size();
    Eigen::VectorXd mu(3 + 2*N);
    mu.segment<3>(0) = poseEst;
    for (int j=0;j<N;++j) mu.segment<2>(3+2*j) = lmsEst[j];

    // demo SPD covariance
    int dim = 3 + 2*N;
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(dim, dim);
    Eigen::MatrixXd P = A*A.transpose();
    P /= P.diagonal().maxCoeff();
    Eigen::VectorXd s(dim); s << 0.2,0.2,0.05, Eigen::VectorXd::Constant(2*N,0.5);
    P = s.asDiagonal()*P*s.asDiagonal();

    double sigmaR = 0.15, sigmaB = 2.0*M_PI/180.0;
    Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
    R(0,0) = sigmaR*sigmaR; R(1,1) = sigmaB*sigmaB;

    std::vector<Eigen::Vector2d> Z = { predictZ(poseTrue, lms[0]),
                                       predictZ(poseTrue, lms[2]),
                                       predictZ(poseTrue, lms[4]) };

    for (int i=0;i<(int)Z.size();++i) {
        int j = associateNN(mu, P, N, Z[i], R, 0.99);
        std::cout << "meas " << i << " -> landmark " << j << "\n";
    }
    return 0;
}
      

// Chapter11_Lesson4.java
// Chapter 11 (SLAM I) — Lesson 4: Data Association Challenges
// Minimal Java demo: chi-square gating + nearest-neighbor association (range-bearing).
//
// Dependency: EJML (ejml-simple) https://ejml.org/

import org.ejml.simple.SimpleMatrix;
import java.util.*;

public class Chapter11_Lesson4 {

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

    // Winitzki erfinv + Wilson–Hilferty chi2 inverse (approx).
    static double erfinvApprox(double x) {
        x = Math.max(-0.999999, Math.min(0.999999, x));
        double a = 0.147;
        double ln = Math.log(1.0 - x * x);
        double t  = 2.0 / (Math.PI * a) + ln / 2.0;
        double s  = Math.sqrt(Math.sqrt(t * t - ln / a) - t);
        return (x >= 0 ? s : -s);
    }
    static double chi2invApprox(double p, int dof) {
        double z = Math.sqrt(2.0) * erfinvApprox(2.0 * p - 1.0);
        double k = (double)dof;
        return k * Math.pow(1.0 - 2.0/(9.0*k) + z*Math.sqrt(2.0/(9.0*k)), 3.0);
    }

    static double[] predictZ(double[] pose, double[] lm) {
        double dx = lm[0] - pose[0];
        double dy = lm[1] - pose[1];
        double r  = Math.hypot(dx, dy);
        double b  = wrapToPi(Math.atan2(dy, dx) - pose[2]);
        return new double[]{r, b};
    }

    static SimpleMatrix jacobianH(double[] pose, double[] lm, int j, int N) {
        double dx = lm[0] - pose[0];
        double dy = lm[1] - pose[1];
        double q  = dx*dx + dy*dy;
        double r  = Math.sqrt(q);

        int dim = 3 + 2*N;
        SimpleMatrix H = new SimpleMatrix(2, dim);

        H.set(0,0, -dx/r);  H.set(0,1, -dy/r);
        H.set(1,0,  dy/q);  H.set(1,1, -dx/q);  H.set(1,2, -1.0);

        int idx = 3 + 2*j;
        H.set(0, idx+0, dx/r);   H.set(0, idx+1, dy/r);
        H.set(1, idx+0, -dy/q);  H.set(1, idx+1, dx/q);
        return H;
    }

    static double nis(SimpleMatrix nu, SimpleMatrix S) {
        return nu.transpose().mult(S.invert()).mult(nu).get(0,0);
    }

    static int associateNN(SimpleMatrix mu, SimpleMatrix P, int N, double[] z, SimpleMatrix R, double gateProb) {
        double gamma = chi2invApprox(gateProb, 2);
        int bestJ = -1;
        double bestD2 = Double.POSITIVE_INFINITY;

        double[] pose = new double[]{mu.get(0,0), mu.get(1,0), mu.get(2,0)};

        for (int j=0;j<N;j++) {
            double[] lm = new double[]{mu.get(3+2*j,0), mu.get(3+2*j+1,0)};
            double[] zhat = predictZ(pose, lm);

            SimpleMatrix nu = new SimpleMatrix(2,1);
            nu.set(0,0, z[0] - zhat[0]);
            nu.set(1,0, wrapToPi(z[1] - zhat[1]));

            SimpleMatrix H = jacobianH(pose, lm, j, N);
            SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(R);

            double d2 = nis(nu, S);
            if (d2 <= gamma && d2 < bestD2) { bestD2 = d2; bestJ = j; }
        }
        return bestJ;
    }

    public static void main(String[] args) {
        double[] poseTrue = {2.0, 1.0, 0.4};
        double[][] lms = { {5,2},{4,-1.5},{1,4},{7,5},{6,-2} };

        double[] poseEst = {poseTrue[0]+0.1, poseTrue[1]-0.05, poseTrue[2]+0.03};

        int N = lms.length;
        SimpleMatrix mu = new SimpleMatrix(3 + 2*N, 1);
        mu.set(0,0, poseEst[0]); mu.set(1,0, poseEst[1]); mu.set(2,0, poseEst[2]);
        for (int j=0;j<N;j++) {
            mu.set(3+2*j,0, lms[j][0]+0.2);
            mu.set(3+2*j+1,0, lms[j][1]-0.1);
        }

        // demo SPD covariance
        int dim = 3 + 2*N;
        SimpleMatrix A = SimpleMatrix.random_DDRM(dim, dim, -1, 1, new Random(7));
        SimpleMatrix P = A.mult(A.transpose());
        double maxDiag = 0;
        for (int i=0;i<dim;i++) maxDiag = Math.max(maxDiag, P.get(i,i));
        P = P.divide(maxDiag);
        double[] s = new double[dim]; s[0]=0.2; s[1]=0.2; s[2]=0.05; for(int i=3;i<dim;i++) s[i]=0.5;
        SimpleMatrix Sscale = SimpleMatrix.diag(s);
        P = Sscale.mult(P).mult(Sscale);

        double sigmaR = 0.15, sigmaB = Math.toRadians(2.0);
        SimpleMatrix R = new SimpleMatrix(2,2);
        R.set(0,0, sigmaR*sigmaR); R.set(1,1, sigmaB*sigmaB);

        double[][] Z = { predictZ(poseTrue, lms[0]),
                         predictZ(poseTrue, lms[2]),
                         predictZ(poseTrue, lms[4]) };

        for (int i=0;i<Z.length;i++) {
            int j = associateNN(mu, P, N, Z[i], R, 0.99);
            System.out.println("meas " + i + " -> landmark " + j);
        }
    }
}
      

% Chapter11_Lesson4.m
% Chapter 11 (SLAM I) — Lesson 4: Data Association Challenges
% MATLAB demo: chi-square gating (NIS) + NN association + programmatic Simulink model.

function Chapter11_Lesson4()
clc;

poseTrue = [2.0; 1.0; 0.4];
lmsTrue  = [5 2; 4 -1.5; 1 4; 7 5; 6 -2];

poseEst  = poseTrue + [0.1; -0.05; 0.03];
lmsEst   = lmsTrue + repmat([0.2 -0.1], size(lmsTrue,1), 1);
N = size(lmsEst,1);

% demo SPD covariance
dim = 3 + 2*N;
rng(7); A = randn(dim, dim);
P = A*A'; P = P ./ max(diag(P));
S = diag([0.2 0.2 0.05 repmat(0.5, 1, 2*N)]);
P = S*P*S;

sigma_r = 0.15; sigma_b = deg2rad(2.0);
R = diag([sigma_r^2, sigma_b^2]);

% generate 3 noisy measurements from landmarks 1,3,5
ids = [1 3 5]; rng(1);
Z = zeros(2, numel(ids));
for i=1:numel(ids)
    z = predictZ(poseTrue, lmsTrue(ids(i),:)');
    z = z + mvnrnd([0 0], R)'; %#ok<MVNRND>
    z(2) = wrapToPi(z(2));
    Z(:,i) = z;
end

disp('Measurements [range, bearing]:'), disp(Z')

disp('NN association with gating:')
for i=1:size(Z,2)
    j = associateNN(poseEst, lmsEst, P, Z(:,i), R, 0.99);
    fprintf('  meas %d -> landmark %d\n', i, j);
end

buildSimulinkNN('Chapter11_Lesson4_Simulink', N);
disp('Created Simulink model: Chapter11_Lesson4_Simulink.slx')
end


% ---------- model ----------
function z = predictZ(pose, lm)
dx = lm(1) - pose(1); dy = lm(2) - pose(2);
z = [hypot(dx,dy); wrapToPi(atan2(dy,dx) - pose(3))];
end

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

function H = jacobianH(pose, lm, j, N)
dx = lm(1) - pose(1); dy = lm(2) - pose(2);
q = dx^2 + dy^2; r = sqrt(q);
H = zeros(2, 3 + 2*N);
H(1,1)=-dx/r; H(1,2)=-dy/r;
H(2,1)= dy/q; H(2,2)=-dx/q; H(2,3)=-1;
idx = 3 + 2*(j-1) + 1;
H(1,idx)=dx/r; H(1,idx+1)=dy/r;
H(2,idx)=-dy/q; H(2,idx+1)=dx/q;
end

function [nu,S] = innovAndS(pose, lms, P, z, j, R)
N = size(lms,1);
lm = lms(j,:)';
zhat = predictZ(pose, lm);
nu = z - zhat; nu(2)=wrapToPi(nu(2));
H = jacobianH(pose, lm, j, N);
S = H*P*H' + R;
end

function gamma = chi2invApprox(p, dof)
% toolbox-free chi2 inverse (Wilson–Hilferty)
z = sqrt(2)*erfinv(2*p - 1);
k = dof;
gamma = k*(1 - 2/(9*k) + z*sqrt(2/(9*k)))^3;
end

function bestJ = associateNN(pose, lms, P, z, R, gateProb)
N = size(lms,1);
gamma = chi2invApprox(gateProb, 2);
bestJ = 0; bestD2 = inf;
for j=1:N
    [nu,S] = innovAndS(pose, lms, P, z, j, R);
    d2 = nu'*(S\nu);
    if d2 <= gamma && d2 < bestD2
        bestD2 = d2; bestJ = j;
    end
end
end


% ---------- Simulink ----------
function buildSimulinkNN(modelName, N)
if bdIsLoaded(modelName), close_system(modelName, 0); end
if exist([modelName '.slx'], 'file') == 2, delete([modelName '.slx']); end

new_system(modelName); open_system(modelName);

add_block('simulink/Sources/Constant', [modelName '/z'], 'Position',[30 50 120 90], 'Value','[1;0]');
add_block('simulink/Sources/Constant', [modelName '/zhat'], 'Position',[30 120 120 160], 'Value',sprintf('zeros(%d,1)',2*N));
add_block('simulink/Sources/Constant', [modelName '/Sdiag'], 'Position',[30 190 120 230], 'Value',sprintf('ones(%d,1)',2*N));

add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/NN_Gate'], 'Position',[220 80 380 200]);
add_block('simulink/Sinks/Display', [modelName '/Display'], 'Position',[430 120 500 160]);

code = sprintf([ ...
'function idx = f(z, zhat, Sdiag)\n' ...
'N = length(zhat)/2;\n' ...
'gamma = chi2invApprox(0.99, 2);\n' ...
'best = 1e9; idx = 0;\n' ...
'for j=1:N\n' ...
'  nu = z - zhat(2*j-1:2*j);\n' ...
'  nu(2) = wrapToPi(nu(2));\n' ...
'  S = diag(Sdiag(2*j-1:2*j));\n' ...
'  d2 = nu''*(S\\nu);\n' ...
'  if d2 <= gamma && d2 < best\n' ...
'    best = d2; idx = j;\n' ...
'  end\n' ...
'end\n' ...
'end\n\n' ...
'function a = wrapToPi(a)\n' ...
'a = mod(a + pi, 2*pi) - pi;\n' ...
'end\n\n' ...
'function g = chi2invApprox(p, dof)\n' ...
'z = sqrt(2)*erfinv(2*p - 1);\n' ...
'k = dof;\n' ...
'g = k*(1 - 2/(9*k) + z*sqrt(2/(9*k)))^3;\n' ...
'end\n' ...
]);

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

add_line(modelName, 'z/1', 'NN_Gate/1');
add_line(modelName, 'zhat/1', 'NN_Gate/2');
add_line(modelName, 'Sdiag/1', 'NN_Gate/3');
add_line(modelName, 'NN_Gate/1', 'Display/1');

save_system(modelName);
end
      

(* Chapter11_Lesson4.nb *)
Notebook[{
  Cell["Autonomous Mobile Robots — Chapter 11 (SLAM I — Filter-Based SLAM)\nLesson 4: Data Association Challenges", "Title"],
  Cell["This notebook demonstrates innovation-based gating (NIS) and nearest-neighbor association for a range-bearing sensor.", "Text"],

  Cell["1) Utilities", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      wrapToPi[a_] := Mod[a + Pi, 2 Pi] - Pi;
      predictZ[pose_, lm_] := Module[{dx, dy, r, b},
        dx = lm[[1]] - pose[[1]];
        dy = lm[[2]] - pose[[2]];
        r = Sqrt[dx^2 + dy^2];
        b = wrapToPi[ArcTan[dx, dy] - pose[[3]]]; (* ArcTan[x,y] gives atan2(y,x) in Mathematica *)
        {r, b}
      ];
    ], "Input"],

  Cell["2) Gating: NIS and chi-square threshold", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      nis[nu_, S_] := nu . LinearSolve[S, nu];
      gatePassQ[nu_, S_, p_:0.99] := Module[{gamma},
        gamma = Quantile[ChiSquareDistribution[Length[nu]], p];
        nis[nu, S] <= gamma
      ];
    ], "Input"],

  Cell["3) Nearest-Neighbor association", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      associateNN[pose_, landmarks_, z_, R_, p_:0.99] := Module[
        {N = Length[landmarks], best = Infinity, bestJ = None, j, zhat, nu, S, d2, gamma},
        gamma = Quantile[ChiSquareDistribution[2], p];
        For[j = 1, j <= N, j++,
          zhat = predictZ[pose, landmarks[[j]]];
          nu = z - zhat;
          nu[[2]] = wrapToPi[nu[[2]]];
          (* Simple covariance for demo: S = R (in practice use H P H^T + R) *)
          S = R;
          d2 = nis[nu, S];
          If[d2 <= gamma && d2 < best,
            best = d2; bestJ = j;
          ];
        ];
        bestJ
      ];
    ], "Input"],

  Cell["4) Demo", "Section"],
  Cell[BoxData@ToBoxes@
    HoldForm[
      poseTrue = {2.0, 1.0, 0.4};
      landmarks = { {5.0, 2.0}, {4.0, -1.5}, {1.0, 4.0}, {7.0, 5.0}, {6.0, -2.0} };
      poseEst = poseTrue + {0.1, -0.05, 0.03};

      sigmaR = 0.15; sigmaB = 2 Degree;
      R = DiagonalMatrix[{sigmaR^2, sigmaB^2}];

      (* Generate one noisy measurement of landmark 1 *)
      z = predictZ[poseTrue, landmarks[[1]]] + RandomVariate[MultinormalDistribution[{0,0}, R]];
      z[[2]] = wrapToPi[z[[2]]];

      Print["z = ", z];
      jHat = associateNN[poseEst, landmarks, z, R, 0.99];
      Print["NN association -> landmark index: ", jHat];
    ], "Input"]
}]
      

Problem 1 (Innovation whitening and chi-square). Let \( \boldsymbol{\nu} \sim \mathcal{N}(\mathbf{0}, \mathbf{S}) \) with SPD \( \mathbf{S}\in\mathbb{R}^{m\times m} \). Prove \( d^2=\boldsymbol{\nu}^\top\mathbf{S}^{-1}\boldsymbol{\nu}\sim\chi^2_m \).

Solution. Let \( \mathbf{S}=\mathbf{L}\mathbf{L}^\top \) (Cholesky) and define \( \mathbf{w}=\mathbf{L}^{-1}\boldsymbol{\nu} \). Then \( \mathbf{w}\sim\mathcal{N}(\mathbf{0},\mathbf{I}) \) and \( d^2=\mathbf{w}^\top\mathbf{w}=\sum_{\ell=1}^m w_\ell^2 \), hence chi-square.

Problem 2 (Gate threshold for range–bearing). For \( m=2 \) and \( \alpha=0.99 \), what is \( \gamma_\alpha \) approximately?

Solution. \( F^{-1}_{\chi^2_2}(0.99)\approx 9.21 \), so accept if \( d^2\le 9.21 \).

Problem 3 (Independent NN vs joint compatibility). Explain why joint compatibility can reject an assignment even when each pair passes individual gating.

Solution. Individual gates test \( d_{i,j}^2 \) separately. Joint tests use the stacked statistic \( d_\mathcal{A}^2=\boldsymbol{\nu}_\mathcal{A}^\top\mathbf{S}_\mathcal{A}^{-1}\boldsymbol{\nu}_\mathcal{A} \). In SLAM, \( \mathbf{S}_\mathcal{A} \) contains cross-covariances, so correlated residuals can make the joint statistic large.

Problem 4 (Association vs clutter, likelihood ratio). With clutter likelihood \( p(\mathbf{z}\mid H_0)=1/V \), derive the likelihood ratio comparing a landmark hypothesis.

Solution. Under \( H_j \), \( p(\mathbf{z}\mid H_j)\approx \mathcal{N}(\boldsymbol{\nu};\mathbf{0},\mathbf{S}_j) \). The likelihood ratio is

\[ \Lambda_j(\mathbf{z}) = V \cdot \frac{\exp\!\left(-\tfrac{1}{2} d_j^2\right)}{\sqrt{(2\pi)^m\det(\mathbf{S}_j)} }. \]

Problem 5 (JCBB pruning bound). Justify pruning when \( |\mathcal{A}| + (M-i) < |\mathcal{A}_\text{best}| \).

Solution. Even if all remaining \( (M-i) \) measurements match, the branch can reach at most \( |\mathcal{A}| + (M-i) \). If this is smaller than the best-so-far cardinality, the branch cannot win.

We treated data association as a statistical decision problem, derived NIS chi-square gating, and explained why SLAM correlations motivate joint compatibility (JCBB). These tools are essential for the EKF-SLAM lab in the next lesson, where incorrect correspondences can destabilize the entire filter.