In Chapter 11, Lesson 1, SLAM was posed as the posterior \( p(\mathbf{x}_k, \mathbf{m} \mid \mathbf{z}_{1:k}, \mathbf{u}_{1:k}) \), where \( \mathbf{x}_k \) is robot pose and \( \mathbf{m} \) is the map. EKF-SLAM approximates this posterior as a single Gaussian over an augmented state. This is attractive because the EKF recursion is closed-form under linear-Gaussian assumptions and remains practical when the motion and sensor models are mildly nonlinear.

The key conceptual point is that SLAM is not “localization + mapping” as two independent problems: the uncertainty of the robot pose and the uncertainty of the landmarks become correlated. EKF-SLAM explicitly maintains these correlations in the covariance matrix, which is both its strength and its scaling bottleneck.

For a planar mobile robot with pose \( (x, y, \theta) \) and \( N \) point landmarks \( \mathbf{m}_j = (m_{j x}, m_{j y}) \), define the augmented state \( \mathbf{s}_k \in \mathbb{R}^{3+2N} \):

\[ \mathbf{s}_k = \begin{bmatrix} \mathbf{x}_k \\ \mathbf{m}_1 \\ \vdots \\ \mathbf{m}_N \end{bmatrix} = \begin{bmatrix} x_k \\ y_k \\ \theta_k \\ m_{1 x} \\ m_{1 y} \\ \cdots \\ m_{N x} \\ m_{N y} \end{bmatrix}. \]

The motion model (unicycle / differential-drive kinematics) can be written as:

\[ \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k), \qquad \mathbf{m}_j(k) = \mathbf{m}_j(k-1) \]

A standard range–bearing observation to landmark \( j \) is:

\[ \mathbf{z}_k^j = h^j(\mathbf{x}_k, \mathbf{m}_j) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k), \quad h^j= \begin{bmatrix} r \\ \phi \end{bmatrix} = \begin{bmatrix} \sqrt{(m_{j x}-x)^2 + (m_{j y}-y)^2} \\ \operatorname{atan2}(m_{j y}-y,\; m_{j x}-x) - \theta \end{bmatrix}. \]

The EKF maintains Gaussian belief \( \mathbf{s}_k \sim \mathcal{N}(\boldsymbol{\mu}_k, \mathbf{P}_k) \), where \( \mathbf{P}_k \) is a dense covariance containing robot–landmark and landmark–landmark cross-covariances.

Let \( \boldsymbol{\mu}_{k-1} \) and \( \mathbf{P}_{k-1} \) be the prior mean and covariance of the augmented state. EKF-SLAM proceeds by (i) predicting robot motion, (ii) updating using any landmark observations at time \( k \).

3.1 Prediction

Define the Jacobian of the augmented transition \( \mathbf{F}_k = \frac{\partial F}{\partial \mathbf{s}}\big|_{\boldsymbol{\mu}_{k-1}} \). Because landmarks are static, \( \mathbf{F}_k \) is identity everywhere except the robot pose block. With control Jacobian \( \mathbf{G}_k = \frac{\partial f}{\partial \mathbf{u}} \) embedded into the full state, the prediction is:

\[ \boldsymbol{\mu}_k^- = \begin{bmatrix} f(\boldsymbol{\mu}^{\mathbf{x}}_{k-1}, \mathbf{u}_k) \\ \boldsymbol{\mu}^{\mathbf{m}}_{k-1} \end{bmatrix}, \qquad \mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^\top + \mathbf{G}_k \mathbf{Q}_k \mathbf{G}_k^\top. \]

3.2 Update for a Known Landmark

For a measurement \( \mathbf{z}_k^j \) associated with landmark \( j \), define predicted measurement \( \hat{\mathbf{z}}_k^j = h^j(\boldsymbol{\mu}_k^-) \) and the Jacobian \( \mathbf{H}_k^j = \frac{\partial h^j}{\partial \mathbf{s}}\big|_{\boldsymbol{\mu}_k^-} \). The innovation and innovation covariance are:

\[ \mathbf{y}_k^j = \mathbf{z}_k^j - \hat{\mathbf{z}}_k^j, \qquad \mathbf{S}_k^j = \mathbf{H}_k^j \mathbf{P}_k^- (\mathbf{H}_k^j)^\top + \mathbf{R}_k. \]

The Kalman gain and EKF update are:

\[ \mathbf{K}_k^j = \mathbf{P}_k^- (\mathbf{H}_k^j)^\top (\mathbf{S}_k^j)^{-1}, \qquad \boldsymbol{\mu}_k = \boldsymbol{\mu}_k^- + \mathbf{K}_k^j \mathbf{y}_k^j. \]

For numerical stability and to maintain symmetry/PSD, the Joseph form is recommended:

\[ \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k^j \mathbf{H}_k^j)\, \mathbf{P}_k^-\, (\mathbf{I} - \mathbf{K}_k^j \mathbf{H}_k^j)^\top + \mathbf{K}_k^j \mathbf{R}_k (\mathbf{K}_k^j)^\top. \]

flowchart TD
A["Inputs: u(k) and z(k)"] --> B["Predict: mu minus and P minus"]
B --> C{"Landmark id available?"}
C -->|new| D["Init landmark; augment state and P"]
C -->|seen| E["Compute zhat, H, y, S"]
E --> F["Compute K; update mu and P (Joseph)"]
D --> G["Next observation"]
F --> G
G --> H["Output: pose and map estimate"]

EKF-SLAM requires the Jacobians of \( h^j \). Let \( \Delta x = m_{j x} - x \), \( \Delta y = m_{j y} - y \), \( q = (\Delta x)^2 + (\Delta y)^2 \), and \( r = \sqrt{q} \). Then:

\[ \frac{\partial r}{\partial x} = -\frac{\Delta x}{r}, \quad \frac{\partial r}{\partial y} = -\frac{\Delta y}{r}, \quad \frac{\partial r}{\partial m_{j x}} = \frac{\Delta x}{r}, \quad \frac{\partial r}{\partial m_{j y}} = \frac{\Delta y}{r}. \]

For bearing \( \phi = \operatorname{atan2}(\Delta y,\Delta x) - \theta \):

\[ \frac{\partial \phi}{\partial x} = \frac{\Delta y}{q}, \quad \frac{\partial \phi}{\partial y} = -\frac{\Delta x}{q}, \quad \frac{\partial \phi}{\partial \theta} = -1, \quad \frac{\partial \phi}{\partial m_{j x}} = -\frac{\Delta y}{q}, \quad \frac{\partial \phi}{\partial m_{j y}} = \frac{\Delta x}{q}. \]

The full measurement Jacobian \( \mathbf{H}_k^j \) is sparse: it has nonzero columns only for the robot pose and the two coordinates of landmark \( j \). This sparsity makes the innovation computation efficient, but the covariance update still becomes expensive once \( \mathbf{P} \) is dense.

When landmark \( j \) is first observed, EKF-SLAM must convert its measurement into an initial landmark estimate using the inverse sensor model. For range–bearing:

\[ \mathbf{m}_j = g(\mathbf{x}, \mathbf{z}) = \begin{bmatrix} x + r\cos(\theta + \phi) \\ y + r\sin(\theta + \phi) \end{bmatrix}. \]

Let \( \mathbf{G}_x = \frac{\partial g}{\partial \mathbf{x}} \) and \( \mathbf{G}_z = \frac{\partial g}{\partial \mathbf{z}} \). With the robot covariance block \( \mathbf{P}_{xx} \), measurement covariance \( \mathbf{R} \), and cross-covariance between the existing state and robot pose \( \mathbf{P}_{sx} \), the augmented covariance blocks become:

\[ \mathbf{P}_{mm} = \mathbf{G}_x \mathbf{P}_{xx} \mathbf{G}_x^\top + \mathbf{G}_z \mathbf{R} \mathbf{G}_z^\top, \qquad \mathbf{P}_{sm} = \mathbf{P}_{sx} \mathbf{G}_x^\top. \]

This step already introduces nontrivial correlations: the new landmark inherits uncertainty from the robot pose. If the initial pose uncertainty is large, landmark initialization produces a large and highly correlated landmark covariance—this matters for later linearization and consistency.

Partition the covariance into robot and map blocks: \( \mathbf{P} = \begin{bmatrix} \mathbf{P}_{xx} & \mathbf{P}_{xm} \\ \mathbf{P}_{mx} & \mathbf{P}_{mm} \end{bmatrix} \), where \( \mathbf{P}_{mm} \in \mathbb{R}^{2N \times 2N} \). Even if \( \mathbf{P}_{mm} \) starts sparse (e.g., diagonal when landmarks are added independently), EKF updates tend to make it dense.

Lemma (density propagation). Consider one EKF update using a measurement that depends on the robot and one landmark \( j \). If the robot is correlated with many landmarks (i.e., columns of \( \mathbf{P}_{xm} \) are nonzero), then after the update the landmark–landmark block \( \mathbf{P}_{mm} \) generally gains new nonzero off-diagonal terms (landmark–landmark correlations), making it denser.

Proof sketch (algebraic). EKF covariance update can be written as

\[ \mathbf{P}^+ = \mathbf{P}^- - \mathbf{P}^- \mathbf{H}^\top \mathbf{S}^{-1} \mathbf{H} \mathbf{P}^-, \qquad \mathbf{S} = \mathbf{H} \mathbf{P}^- \mathbf{H}^\top + \mathbf{R}. \]

Because \( \mathbf{H} \) has nonzeros in the robot pose columns and the landmark-\( j \) columns, the product \( \mathbf{H} \mathbf{P}^- \) extracts the robot and landmark-\( j \) rows of \( \mathbf{P}^- \). If \( \mathbf{P}^- \) contains nonzero robot–other-landmark cross terms, the rank-2 correction term \( \mathbf{P}^- \mathbf{H}^\top \mathbf{S}^{-1} \mathbf{H} \mathbf{P}^- \) spreads this coupling to the map block, creating new off-diagonal entries in \( \mathbf{P}_{mm}^+ \). Repeating over time yields a dense \( \mathbf{P} \).

Once \( \mathbf{P} \) is dense, memory and time scale poorly: storing \( \mathbf{P} \) costs \( O(N^2) \) and updating it costs roughly \( O(N^2) \) per measurement (even though \( \mathbf{S} \) is small). For large-scale mapping, this quickly becomes infeasible when \( N \) becomes, e.g., \( N > 10^4 \).

flowchart TD
  N0["State size: 3 + 2N"] --> N1["Covariance P is (3+2N)x(3+2N)"]
  N1 --> N2["Updates create cross-correlations"]
  N2 --> N3["P becomes dense over time"]
  N3 --> N4["Memory ~ O(N^2)"]
  N3 --> N5["Time per update ~ O(N^2)"]
  N2 --> N6["Linearization + gauge freedoms → inconsistency risk"]
  N5 --> N7["Scalability breaks for large N"]
        

EKF-SLAM is also limited by linearization and by the fact that the global reference frame is not observable from relative motion + relative landmark measurements. Intuitively, if we translate and rotate the entire world together (robot trajectory and map), the sensor predictions do not change.

Let a planar rigid transform \( \mathbf{T} \in SE(2) \) act on all poses and landmarks: \( \mathbf{x} \mapsto \mathbf{T}\mathbf{x} \), \( \mathbf{m}_j \mapsto \mathbf{T}\mathbf{m}_j \). For relative range–bearing measurements, the likelihood is invariant:

\[ p\big(\mathbf{z}_k^j \mid \mathbf{x}_k, \mathbf{m}_j\big) = p\big(\mathbf{z}_k^j \mid \mathbf{T}\mathbf{x}_k, \mathbf{T}\mathbf{m}_j\big). \]

This implies a gauge freedom of (at least) 3 degrees of freedom (global x/y translation and yaw rotation). A correct posterior should reflect this by not artificially collapsing uncertainty along these directions.

However, EKF linearization around the current estimate can break invariance and produce overconfident covariances (inconsistency). A standard diagnostic is the normalized estimation error squared (NEES):

\[ \text{NEES}_k = (\mathbf{e}_k)^\top \mathbf{P}_k^{-1} \mathbf{e}_k, \qquad \mathbf{e}_k = \mathbf{s}_k - \boldsymbol{\mu}_k. \]

If the filter is consistent, \( \text{NEES}_k \) should statistically match a chi-square distribution with degrees of freedom equal to the state dimension. In practice, EKF-SLAM often yields NEES values that are too large (covariance too small), especially with poor initialization, high nonlinearity, or incorrect association.

Important limitation boundary for this course: detailed remedies (e.g., alternative linearization points, observability-constrained EKF variants, or invariant filtering) are advanced topics. Here we focus on recognizing the structural reasons for inconsistency; subsequent lessons will introduce scalable alternatives (FastSLAM) and later chapters introduce graph-based SLAM.

• Quadratic scaling: storing and updating \( \mathbf{P} \) is typically \( O(N^2) \), so EKF-SLAM is best for small maps.
• Linearization sensitivity: strong nonlinearity in range–bearing (especially at long range) can cause biased innovations and covariance underestimation.
• Gauge freedoms: global translation/rotation are unobservable; EKF may falsely reduce uncertainty along them.
• Association brittleness: wrong landmark association injects large, systematic errors that EKF cannot “average out”. (Association algorithms are treated in Lesson 4; here it is a limitation statement.)
• Numerical issues: loss of symmetry/PSD if the covariance update is not performed carefully; Joseph form helps.

The implementations below follow the equations in Sections 3–5 using a unicycle motion model and range–bearing measurements. For didactic clarity, they assume known data association (the measurement includes the landmark id).

9.1 Python (NumPy) — Full Minimal EKF-SLAM Demo

File: Chapter11_Lesson2.py

# Chapter11_Lesson2.py
# EKF-SLAM (structure and limitations) — minimal didactic implementation (known data association)
# NOTE: This is a teaching implementation for small N (quadratic scaling in P).

import numpy as np

def wrap_angle(a):
    return (a + np.pi) % (2*np.pi) - np.pi

class EKFSLAM2D:
    """
    State s = [x, y, theta, m1x, m1y, ..., mNx, mNy]^T
    Covariance P is (3+2N)x(3+2N)

    This implementation assumes:
      - Known landmark IDs in measurements
      - Landmarks are 2D points
      - Range-bearing sensor model
      - Unicycle motion model with control u=(v, w, dt)
    """
    def __init__(self, N, Q, R, init_pose=np.zeros(3), init_P_pose=None):
        self.N = N
        self.dim = 3 + 2*N
        self.mu = np.zeros(self.dim)
        self.mu[0:3] = init_pose.copy()

        if init_P_pose is None:
            init_P_pose = np.diag([0.05**2, 0.05**2, (np.deg2rad(2.0))**2])

        self.P = np.zeros((self.dim, self.dim))
        self.P[0:3, 0:3] = init_P_pose

        # Mark which landmarks have been initialized
        self.seen = np.zeros(N, dtype=bool)

        # Motion/process noise in control space (v,w) or pose space? We'll treat as pose-space additive for simplicity.
        self.Q = Q  # 3x3
        self.R = R  # 2x2

    # ---------------------------
    # Motion model (unicycle)
    # ---------------------------
    def f(self, x, u):
        v, w, dt = u
        px, py, th = x
        if abs(w) < 1e-9:
            px_new = px + v*dt*np.cos(th)
            py_new = py + v*dt*np.sin(th)
            th_new = th
        else:
            px_new = px + (v/w)*(np.sin(th + w*dt) - np.sin(th))
            py_new = py - (v/w)*(np.cos(th + w*dt) - np.cos(th))
            th_new = th + w*dt
        return np.array([px_new, py_new, wrap_angle(th_new)])

    def F_jacobian(self, x, u):
        v, w, dt = u
        _, _, th = x

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

        # Embed into full state
        F = np.eye(self.dim)
        F[0:3, 0:3] = Fx
        return F

    # ---------------------------
    # Measurement model (range-bearing)
    # ---------------------------
    def h(self, x, m):
        px, py, th = x
        mx, my = m
        dx = mx - px
        dy = my - py
        q = dx*dx + dy*dy
        r = np.sqrt(q)
        phi = wrap_angle(np.arctan2(dy, dx) - th)
        return np.array([r, phi])

    def H_jacobian(self, x, m, j):
        """
        Jacobian w.r.t full state s (dim=3+2N) for landmark j.
        Nonzeros only in robot pose block and landmark j block.
        """
        px, py, th = x
        mx, my = m

        dx = mx - px
        dy = my - py
        q  = dx*dx + dy*dy
        r  = np.sqrt(q)
        if q < 1e-12:
            raise ValueError("Landmark too close or coincident for Jacobian stability.")

        # dr/d[px,py,th,mx,my]
        dr_dpx = -dx/r
        dr_dpy = -dy/r
        dr_dth = 0.0
        dr_dmx =  dx/r
        dr_dmy =  dy/r

        # dphi/d[px,py,th,mx,my]
        dphi_dpx =  dy/q
        dphi_dpy = -dx/q
        dphi_dth = -1.0
        dphi_dmx = -dy/q
        dphi_dmy =  dx/q

        H = np.zeros((2, self.dim))
        # robot part
        H[0,0] = dr_dpx
        H[0,1] = dr_dpy
        H[0,2] = dr_dth
        H[1,0] = dphi_dpx
        H[1,1] = dphi_dpy
        H[1,2] = dphi_dth

        # landmark j part
        idx = 3 + 2*j
        H[0, idx+0] = dr_dmx
        H[0, idx+1] = dr_dmy
        H[1, idx+0] = dphi_dmx
        H[1, idx+1] = dphi_dmy
        return H

    # ---------------------------
    # Landmark initialization (inverse sensor model)
    # ---------------------------
    def initialize_landmark(self, z, j):
        """
        z = [r, phi] in robot frame. Initialize landmark in global frame.
        Also augment covariance blocks using linearized propagation.
        """
        r, phi = z
        x = self.mu[0:3]
        px, py, th = x
        ang = th + phi
        mx = px + r*np.cos(ang)
        my = py + r*np.sin(ang)

        idx = 3 + 2*j
        self.mu[idx:idx+2] = np.array([mx, my])
        self.seen[j] = True

        # Compute Gx and Gz Jacobians for g(x,z):
        # m = [px + r cos(th+phi), py + r sin(th+phi)]
        c = np.cos(ang)
        s = np.sin(ang)

        Gx = np.array([
            [1.0, 0.0, -r*s],
            [0.0, 1.0,  r*c]
        ])

        Gz = np.array([
            [ c, -r*s],
            [ s,  r*c]
        ])

        # Extract robot covariance and cross-covariances
        Pxx = self.P[0:3, 0:3]
        Psx = self.P[:, 0:3]  # (dim x 3)

        # New landmark covariance and cross-covariance
        Pmm = Gx @ Pxx @ Gx.T + Gz @ self.R @ Gz.T
        Psm = Psx @ Gx.T

        # Set blocks (symmetric)
        self.P[idx:idx+2, idx:idx+2] = Pmm
        self.P[:, idx:idx+2] = Psm
        self.P[idx:idx+2, :] = Psm.T

    # ---------------------------
    # EKF predict and update
    # ---------------------------
    def predict(self, u):
        x = self.mu[0:3]
        x_new = self.f(x, u)
        F = self.F_jacobian(x, u)

        self.mu[0:3] = x_new
        self.P = F @ self.P @ F.T
        # Add process noise on pose block (embedded)
        self.P[0:3, 0:3] += self.Q

    def update_one(self, z, j):
        """
        Update for one measurement z=[r,phi] with landmark id j.
        If landmark unseen, initialize it first.
        """
        if not self.seen[j]:
            self.initialize_landmark(z, j)
            return

        x = self.mu[0:3]
        idx = 3 + 2*j
        m = self.mu[idx:idx+2]

        zhat = self.h(x, m)
        y = z - zhat
        y[1] = wrap_angle(y[1])

        H = self.H_jacobian(x, m, j)
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)

        # mean update
        self.mu = self.mu + K @ y
        self.mu[2] = wrap_angle(self.mu[2])

        # Joseph-form covariance update for stability
        I = np.eye(self.dim)
        KH = K @ H
        self.P = (I - KH) @ self.P @ (I - KH).T + K @ self.R @ K.T

    def step(self, u, measurements):
        """
        u = (v,w,dt)
        measurements: list of tuples (j, z), where z=[r,phi]
        """
        self.predict(u)
        for (j, z) in measurements:
            self.update_one(z, j)

def simulate_small_world():
    """
    Small simulation to illustrate structure:
    - 3 landmarks, robot moves and observes them.
    - Known data association
    """
    np.random.seed(0)

    N = 3
    Q = np.diag([0.02**2, 0.02**2, (np.deg2rad(1.0))**2])
    R = np.diag([0.10**2, (np.deg2rad(2.0))**2])

    ekf = EKFSLAM2D(N=N, Q=Q, R=R, init_pose=np.array([0.0, 0.0, 0.0]))

    # True landmarks (fixed)
    M_true = np.array([[5.0, 0.0],
                       [5.0, 5.0],
                       [0.0, 5.0]])

    # True robot state
    x_true = np.array([0.0, 0.0, 0.0])

    def observe(x, M):
        px, py, th = x
        meas = []
        for j in range(N):
            mx, my = M[j]
            dx = mx - px
            dy = my - py
            r = np.sqrt(dx*dx + dy*dy)
            phi = wrap_angle(np.arctan2(dy, dx) - th)
            # add noise
            r_n = r + np.random.normal(0, np.sqrt(R[0,0]))
            p_n = phi + np.random.normal(0, np.sqrt(R[1,1]))
            meas.append((j, np.array([r_n, wrap_angle(p_n)])))
        return meas

    # Run a few steps
    for k in range(15):
        u = (0.5, np.deg2rad(5.0), 0.2)  # v, w, dt
        # propagate true
        x_true = ekf.f(x_true, u)
        # get measurements
        meas = observe(x_true, M_true)
        # EKF step
        ekf.step(u, meas)

        # Print scaling-related info
        nnz = np.count_nonzero(np.abs(ekf.P) > 1e-10)
        density = nnz / (ekf.P.size)
        print(f"k={k:02d} pose_est={ekf.mu[0:3]}  P_density={density:.3f}")

    print("\nFinal mean (pose + landmarks):")
    print(ekf.mu)
    print("\nFinal covariance top-left block (pose):")
    print(ekf.P[0:3,0:3])

if __name__ == "__main__":
    simulate_small_world()
      

9.2 C++ (Eigen) — EKF-SLAM Skeleton

Libraries commonly used in C++ robotics stacks include Eigen (linear algebra), Sophus (Lie groups), and ROS message ecosystems. This lesson uses Eigen only.

File: Chapter11_Lesson2.cpp

/* Chapter11_Lesson2.cpp
   EKF-SLAM (structure and limitations) — minimal didactic skeleton (known data association)
   Build idea (example):
     g++ -O2 -std=c++17 Chapter11_Lesson2.cpp -I /usr/include/eigen3 -o ekf_slam

   NOTE: This is a teaching skeleton for small N. It keeps a dense covariance P,
   so memory/time scale ~ O(N^2).
*/
#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

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

struct MeasRB {
  int id;        // landmark id in [0..N-1]
  double r;      // range
  double phi;    // bearing (rad)
};

class EKFSLAM2D {
public:
  EKFSLAM2D(int N, const Eigen::Matrix3d& Q, const Eigen::Matrix2d& R)
    : N_(N), dim_(3 + 2*N), Q_(Q), R_(R) {
    mu_.setZero(dim_);
    P_.setZero(dim_, dim_);
    // small initial pose uncertainty
    P_.block<3,3>(0,0) = (Eigen::Vector3d(0.05*0.05, 0.05*0.05, (M_PI/180.0*2.0)*(M_PI/180.0*2.0))).asDiagonal();
    seen_.assign(N_, false);
  }

  void predict(double v, double w, double dt) {
    Eigen::Vector3d x = mu_.segment<3>(0);
    Eigen::Vector3d xNew = motion(x, v, w, dt);

    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(dim_, dim_);
    F.block<3,3>(0,0) = motionJacobian(x, v, w, dt);

    mu_.segment<3>(0) = xNew;
    P_ = F * P_ * F.transpose();
    P_.block<3,3>(0,0) += Q_;
  }

  void updateOne(const MeasRB& z) {
    int j = z.id;
    if (!seen_[j]) {
      initializeLandmark(z);
      return;
    }
    const int idx = 3 + 2*j;

    Eigen::Vector3d x = mu_.segment<3>(0);
    Eigen::Vector2d m = mu_.segment<2>(idx);

    Eigen::Vector2d zhat = h(x, m);
    Eigen::Vector2d y;
    y << (z.r - zhat(0)), wrapAngle(z.phi - zhat(1));

    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, dim_);
    HJacobian(x, m, j, H);

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

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

    // Joseph form
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_, dim_);
    Eigen::MatrixXd KH = K * H;
    P_ = (I - KH) * P_ * (I - KH).transpose() + K * R_ * K.transpose();
  }

  void step(double v, double w, double dt, const std::vector<MeasRB>& meas) {
    predict(v, w, dt);
    for (const auto& z : meas) updateOne(z);
  }

  const Eigen::VectorXd& mu() const { return mu_; }
  const Eigen::MatrixXd& P()  const { return P_; }

private:
  int N_;
  int dim_;
  Eigen::VectorXd mu_;
  Eigen::MatrixXd P_;
  Eigen::Matrix3d Q_;
  Eigen::Matrix2d R_;
  std::vector<bool> seen_;

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

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

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

  void HJacobian(const Eigen::Vector3d& x, const Eigen::Vector2d& m, int j, Eigen::MatrixXd& H) {
    double px = x(0), py = x(1), th = x(2);
    (void)th;
    double mx = m(0), my = m(1);
    double dx = mx - px;
    double dy = my - py;
    double q = dx*dx + dy*dy;
    double r = std::sqrt(q);
    if (q < 1e-12) throw std::runtime_error("Jacobian unstable: q too small.");

    // range
    H(0,0) = -dx/r;
    H(0,1) = -dy/r;
    H(0,2) = 0.0;

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

    const int idx = 3 + 2*j;
    // range w.r.t landmark
    H(0, idx+0) =  dx/r;
    H(0, idx+1) =  dy/r;
    // bearing w.r.t landmark
    H(1, idx+0) = -dy/q;
    H(1, idx+1) =  dx/q;
  }

  void initializeLandmark(const MeasRB& z) {
    const int j = z.id;
    const int idx = 3 + 2*j;

    Eigen::Vector3d x = mu_.segment<3>(0);
    double px = x(0), py = x(1), th = x(2);
    double ang = th + z.phi;
    double mx = px + z.r * std::cos(ang);
    double my = py + z.r * std::sin(ang);

    mu_.segment<2>(idx) << mx, my;
    seen_[j] = true;

    // Jacobians of inverse sensor model m=g(x,z)
    double c = std::cos(ang);
    double s = std::sin(ang);

    Eigen::Matrix<double,2,3> Gx;
    Gx << 1.0, 0.0, -z.r*s,
          0.0, 1.0,  z.r*c;

    Eigen::Matrix2d Gz;
    Gz << c, -z.r*s,
          s,  z.r*c;

    Eigen::Matrix3d Pxx = P_.block<3,3>(0,0);
    Eigen::MatrixXd Psx = P_.block(0,0,dim_,3);

    Eigen::Matrix2d Pmm = Gx * Pxx * Gx.transpose() + Gz * R_ * Gz.transpose();
    Eigen::MatrixXd Psm = Psx * Gx.transpose(); // (dim x 2)

    P_.block<2,2>(idx, idx) = Pmm;
    P_.block(0, idx, dim_, 2) = Psm;
    P_.block(idx, 0, 2, dim_) = Psm.transpose();
  }
};

int main() {
  const int N = 3;
  Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
  Q.diagonal() << 0.02*0.02, 0.02*0.02, (M_PI/180.0)*(M_PI/180.0);
  Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
  R.diagonal() << 0.10*0.10, (2.0*M_PI/180.0)*(2.0*M_PI/180.0);

  EKFSLAM2D ekf(N, Q, R);

  // One fake step (for compilation sanity)
  std::vector<MeasRB> meas;
  meas.push_back({0, 5.0, 0.0});
  meas.push_back({1, 7.0, 0.7});
  meas.push_back({2, 6.0, 1.2});

  ekf.step(0.5, 0.1, 0.2, meas);

  std::cout << "mu (pose): " << ekf.mu().segment<3>(0).transpose() << "\n";
  std::cout << "P(0:2,0:2):\n" << ekf.P().block<3,3>(0,0) << "\n";
  return 0;
}
      

9.3 Java (EJML) — EKF-SLAM Skeleton

In Java, EJML provides matrix operations suitable for EKF-style filters. For robotics middleware, Java bindings exist for ROS2 (rcljava) in some ecosystems.

File: Chapter11_Lesson2.java

// Chapter11_Lesson2.java
// EKF-SLAM (structure and limitations) — minimal didactic skeleton (known data association)
// Dependencies: EJML (org.ejml:ejml-all)
// NOTE: Dense covariance => ~ O(N^2) scaling.
//
// This is intentionally compact and not a full robotics stack integration.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import java.util.Arrays;
import java.util.List;

public class Chapter11_Lesson2 {

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

    static class MeasRB {
        int id;
        double r;
        double phi;
        MeasRB(int id, double r, double phi) { this.id=id; this.r=r; this.phi=phi; }
    }

    static class EKFSLAM2D {
        final int N;
        final int dim;
        final DMatrixRMaj mu;     // dim x 1
        final DMatrixRMaj P;      // dim x dim
        final DMatrixRMaj Q;      // 3 x 3
        final DMatrixRMaj R;      // 2 x 2
        final boolean[] seen;

        EKFSLAM2D(int N, DMatrixRMaj Q, DMatrixRMaj R) {
            this.N = N;
            this.dim = 3 + 2*N;
            this.mu = new DMatrixRMaj(dim,1);
            this.P  = new DMatrixRMaj(dim,dim);
            this.Q  = Q;
            this.R  = R;
            this.seen = new boolean[N];

            // init pose covariance
            P.set(0,0, 0.05*0.05);
            P.set(1,1, 0.05*0.05);
            double thStd = Math.toRadians(2.0);
            P.set(2,2, thStd*thStd);
        }

        // motion model (unicycle) for pose only
        double[] motion(double[] x, double v, double w, double dt) {
            double px=x[0], py=x[1], th=x[2];
            double[] out = new double[3];
            if (Math.abs(w) < 1e-9) {
                out[0] = px + v*dt*Math.cos(th);
                out[1] = py + v*dt*Math.sin(th);
                out[2] = th;
            } else {
                out[0] = px + (v/w)*(Math.sin(th + w*dt) - Math.sin(th));
                out[1] = py - (v/w)*(Math.cos(th + w*dt) - Math.cos(th));
                out[2] = th + w*dt;
            }
            out[2] = wrapAngle(out[2]);
            return out;
        }

        DMatrixRMaj motionJacobian(double[] x, double v, double w, double dt) {
            double th = x[2];
            DMatrixRMaj Fx = CommonOps_DDRM.identity(3);
            if (Math.abs(w) < 1e-9) {
                Fx.set(0,2, -v*dt*Math.sin(th));
                Fx.set(1,2,  v*dt*Math.cos(th));
            } else {
                Fx.set(0,2, (v/w)*(Math.cos(th + w*dt) - Math.cos(th)));
                Fx.set(1,2, (v/w)*(Math.sin(th + w*dt) - Math.sin(th)));
            }
            return Fx;
        }

        // measurement model h(x,m) = [range; bearing]
        double[] h(double[] x, double[] m) {
            double px=x[0], py=x[1], th=x[2];
            double dx = m[0]-px;
            double dy = m[1]-py;
            double q = dx*dx + dy*dy;
            double r = Math.sqrt(q);
            double phi = wrapAngle(Math.atan2(dy,dx) - th);
            return new double[]{r, phi};
        }

        DMatrixRMaj HJacobian(double[] x, double[] m, int j) {
            double px=x[0], py=x[1];
            double mx=m[0], my=m[1];
            double dx = mx-px;
            double dy = my-py;
            double q = dx*dx + dy*dy;
            double r = Math.sqrt(q);
            if (q < 1e-12) throw new RuntimeException("Jacobian unstable: q too small");

            DMatrixRMaj H = new DMatrixRMaj(2, dim);

            // robot part
            H.set(0,0, -dx/r);
            H.set(0,1, -dy/r);
            H.set(0,2,  0.0);

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

            int idx = 3 + 2*j;
            // landmark part
            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;
        }

        void initializeLandmark(MeasRB z) {
            int j = z.id;
            int idx = 3 + 2*j;

            double px = mu.get(0,0);
            double py = mu.get(1,0);
            double th = mu.get(2,0);

            double ang = th + z.phi;
            double mx = px + z.r*Math.cos(ang);
            double my = py + z.r*Math.sin(ang);

            mu.set(idx,0, mx);
            mu.set(idx+1,0, my);
            seen[j] = true;

            // Gx (2x3), Gz (2x2)
            double c = Math.cos(ang);
            double s = Math.sin(ang);

            DMatrixRMaj Gx = new DMatrixRMaj(2,3);
            Gx.set(0,0, 1.0); Gx.set(0,1, 0.0); Gx.set(0,2, -z.r*s);
            Gx.set(1,0, 0.0); Gx.set(1,1, 1.0); Gx.set(1,2,  z.r*c);

            DMatrixRMaj Gz = new DMatrixRMaj(2,2);
            Gz.set(0,0, c); Gz.set(0,1, -z.r*s);
            Gz.set(1,0, s); Gz.set(1,1,  z.r*c);

            // Extract Pxx and Psx
            DMatrixRMaj Pxx = new DMatrixRMaj(3,3);
            CommonOps_DDRM.extract(P, 0,3, 0,3, Pxx, 0,0);

            DMatrixRMaj Psx = new DMatrixRMaj(dim,3);
            CommonOps_DDRM.extract(P, 0,dim, 0,3, Psx, 0,0);

            // Pmm = Gx Pxx Gx^T + Gz R Gz^T
            DMatrixRMaj tmp23 = new DMatrixRMaj(2,3);
            DMatrixRMaj Pmm = new DMatrixRMaj(2,2);
            CommonOps_DDRM.mult(Gx, Pxx, tmp23);                // 2x3
            CommonOps_DDRM.multTransB(tmp23, Gx, Pmm);          // 2x2

            DMatrixRMaj tmp22 = new DMatrixRMaj(2,2);
            DMatrixRMaj add22 = new DMatrixRMaj(2,2);
            CommonOps_DDRM.mult(Gz, R, tmp22);
            CommonOps_DDRM.multTransB(tmp22, Gz, add22);
            CommonOps_DDRM.addEquals(Pmm, add22);

            // Psm = Psx Gx^T
            DMatrixRMaj Psm = new DMatrixRMaj(dim,2);
            CommonOps_DDRM.multTransB(Psx, Gx, Psm);

            // Insert blocks
            // P[idx:idx+2, idx:idx+2] = Pmm
            CommonOps_DDRM.insert(Pmm, P, idx, idx);

            // P[:, idx:idx+2] = Psm
            CommonOps_DDRM.insert(Psm, P, 0, idx);

            // P[idx:idx+2, :] = Psm^T
            DMatrixRMaj PsmT = new DMatrixRMaj(2, dim);
            CommonOps_DDRM.transpose(Psm, PsmT);
            CommonOps_DDRM.insert(PsmT, P, idx, 0);
        }

        void predict(double v, double w, double dt) {
            double[] x = new double[]{ mu.get(0,0), mu.get(1,0), mu.get(2,0) };
            double[] xNew = motion(x, v, w, dt);
            mu.set(0,0, xNew[0]);
            mu.set(1,0, xNew[1]);
            mu.set(2,0, xNew[2]);

            // Build F (dim x dim) with Fx in top-left
            DMatrixRMaj F = CommonOps_DDRM.identity(dim);
            DMatrixRMaj Fx = motionJacobian(x, v, w, dt);
            CommonOps_DDRM.insert(Fx, F, 0,0);

            // P = F P F^T
            DMatrixRMaj FP = new DMatrixRMaj(dim,dim);
            DMatrixRMaj FPFt = new DMatrixRMaj(dim,dim);
            CommonOps_DDRM.mult(F, P, FP);
            CommonOps_DDRM.multTransB(FP, F, FPFt);
            P.set(FPFt);

            // add Q into pose block
            for (int i=0;i<3;i++) for (int j=0;j<3;j++) P.add(i,j, Q.get(i,j));
        }

        void updateOne(MeasRB z) {
            int j = z.id;
            if (!seen[j]) {
                initializeLandmark(z);
                return;
            }
            int idx = 3 + 2*j;

            double[] x = new double[]{ mu.get(0,0), mu.get(1,0), mu.get(2,0) };
            double[] m = new double[]{ mu.get(idx,0), mu.get(idx+1,0) };

            double[] zhat = h(x, m);
            double[] y = new double[]{ z.r - zhat[0], wrapAngle(z.phi - zhat[1]) };

            DMatrixRMaj H = HJacobian(x, m, j);

            // S = H P H^T + R
            DMatrixRMaj HP = new DMatrixRMaj(2, dim);
            DMatrixRMaj S = new DMatrixRMaj(2,2);
            CommonOps_DDRM.mult(H, P, HP);
            CommonOps_DDRM.multTransB(HP, H, S);
            CommonOps_DDRM.addEquals(S, R);

            // K = P H^T S^{-1}
            DMatrixRMaj PHt = new DMatrixRMaj(dim,2);
            CommonOps_DDRM.multTransB(P, H, PHt);

            DMatrixRMaj Sinv = new DMatrixRMaj(2,2);
            CommonOps_DDRM.invert(S, Sinv);

            DMatrixRMaj K = new DMatrixRMaj(dim,2);
            CommonOps_DDRM.mult(PHt, Sinv, K);

            // mu = mu + K y
            DMatrixRMaj yVec = new DMatrixRMaj(2,1);
            yVec.set(0,0, y[0]);
            yVec.set(1,0, y[1]);

            DMatrixRMaj Ky = new DMatrixRMaj(dim,1);
            CommonOps_DDRM.mult(K, yVec, Ky);
            CommonOps_DDRM.addEquals(mu, Ky);
            mu.set(2,0, wrapAngle(mu.get(2,0)));

            // Joseph form: P = (I-KH) P (I-KH)^T + K R K^T
            DMatrixRMaj I = CommonOps_DDRM.identity(dim);
            DMatrixRMaj KH = new DMatrixRMaj(dim, dim);
            CommonOps_DDRM.mult(K, H, KH);

            DMatrixRMaj IminusKH = new DMatrixRMaj(dim, dim);
            CommonOps_DDRM.subtract(I, KH, IminusKH);

            DMatrixRMaj A = new DMatrixRMaj(dim, dim);
            DMatrixRMaj Atmp = new DMatrixRMaj(dim, dim);
            CommonOps_DDRM.mult(IminusKH, P, Atmp);
            CommonOps_DDRM.multTransB(Atmp, IminusKH, A);

            DMatrixRMaj KR = new DMatrixRMaj(dim,2);
            DMatrixRMaj add = new DMatrixRMaj(dim,dim);
            CommonOps_DDRM.mult(K, R, KR);
            CommonOps_DDRM.multTransB(KR, K, add);

            CommonOps_DDRM.add(A, add, P);
        }

        void step(double v, double w, double dt, List<MeasRB> meas) {
            predict(v,w,dt);
            for (MeasRB z : meas) updateOne(z);
        }
    }

    public static void main(String[] args) {
        int N = 3;
        DMatrixRMaj Q = new DMatrixRMaj(3,3);
        Q.set(0,0, 0.02*0.02);
        Q.set(1,1, 0.02*0.02);
        Q.set(2,2, Math.toRadians(1.0)*Math.toRadians(1.0));

        DMatrixRMaj R = new DMatrixRMaj(2,2);
        R.set(0,0, 0.10*0.10);
        R.set(1,1, Math.toRadians(2.0)*Math.toRadians(2.0));

        EKFSLAM2D ekf = new EKFSLAM2D(N, Q, R);

        List<MeasRB> meas = Arrays.asList(
                new MeasRB(0, 5.0, 0.0),
                new MeasRB(1, 7.0, 0.7),
                new MeasRB(2, 6.0, 1.2)
        );

        ekf.step(0.5, 0.1, 0.2, meas);

        System.out.printf("pose_est = [%.3f, %.3f, %.3f]\n",
                ekf.mu.get(0,0), ekf.mu.get(1,0), ekf.mu.get(2,0));
    }
}
      

9.4 MATLAB / Simulink — EKF-SLAM Demo + Simulink Skeleton

MATLAB is widely used for rapid prototyping and for Simulink-based estimator pipelines. Because binary .slx models cannot be embedded here, the provided script includes a small programmatic Simulink skeleton as a starting point.

File: Chapter11_Lesson2.m

% Chapter11_Lesson2.m
% EKF-SLAM (structure and limitations) — minimal didactic implementation (known data association)
% NOTE: Dense covariance => O(N^2) scaling. Suitable for small N.

clear; clc; close all;
rng(0);

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

% Problem setup
N = 3;
dim = 3 + 2*N;

Q = diag([0.02^2, 0.02^2, deg2rad(1.0)^2]);          % pose process noise
R = diag([0.10^2, deg2rad(2.0)^2]);                  % range-bearing noise

mu = zeros(dim,1);
P  = zeros(dim,dim);
P(1:3,1:3) = diag([0.05^2, 0.05^2, deg2rad(2.0)^2]);

seen = false(N,1);

M_true = [5,0; 5,5; 0,5];   % true landmarks
x_true = [0;0;0];

motion = @(x,u) unicycle_step(x,u,wrap);
h = @(x,m) rb_model(x,m,wrap);

steps = 15;

for k = 1:steps
    u = [0.5; deg2rad(5.0); 0.2];    % [v; w; dt]

    % propagate true (for synthetic measurements)
    x_true = motion(x_true,u);

    % EKF predict
    [mu,P] = ekf_predict(mu,P,u,Q,N,wrap);

    % generate measurements (known IDs)
    meas = {};
    for j = 1:N
        z = h(x_true, M_true(j,:)');
        z = z + [sqrt(R(1,1))*randn; sqrt(R(2,2))*randn];
        z(2) = wrap(z(2));
        meas{end+1} = struct('id', j, 'z', z);
    end

    % EKF update for each meas
    for t = 1:numel(meas)
        j = meas{t}.id;
        z = meas{t}.z;

        if ~seen(j)
            [mu,P,seen] = init_landmark(mu,P,seen,z,j,R,wrap);
            continue;
        end

        [mu,P] = ekf_update_one(mu,P,z,j,R,wrap);
    end

    % quick info
    density = nnz(abs(P) > 1e-10) / numel(P);
    fprintf("k=%02d pose_est=[%.3f %.3f %.3f]  P_density=%.3f\n", ...
        k, mu(1), mu(2), mu(3), density);
end

disp("Final mean (pose+landmarks):");
disp(mu');

disp("Final pose covariance:");
disp(P(1:3,1:3));

% -------------------------------
% Helper functions
% -------------------------------
function xNew = unicycle_step(x,u,wrap)
v = u(1); w = u(2); dt = u(3);
px = x(1); py = x(2); th = x(3);

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

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

function [mu,P] = ekf_predict(mu,P,u,Q,N,wrap)
% Predict pose in augmented state
x = mu(1:3);
xNew = unicycle_step(x,u,wrap);
mu(1:3) = xNew;

% Jacobian Fx
v=u(1); w=u(2); dt=u(3); th=x(3);
Fx = eye(3);
if abs(w) < 1e-9
    Fx(1,3) = -v*dt*sin(th);
    Fx(2,3) =  v*dt*cos(th);
else
    Fx(1,3) = (v/w)*(cos(th + w*dt) - cos(th));
    Fx(2,3) = (v/w)*(sin(th + w*dt) - sin(th));
end

dim = 3 + 2*N;
F = eye(dim);
F(1:3,1:3) = Fx;

P = F*P*F';
P(1:3,1:3) = P(1:3,1:3) + Q;
end

function [mu,P,seen] = init_landmark(mu,P,seen,z,j,R,wrap)
% inverse sensor model: m = [x + r cos(th+phi), y + r sin(th+phi)]
r = z(1); phi = z(2);
x = mu(1:3);
px=x(1); py=x(2); th=x(3);

ang = th + phi;
mx = px + r*cos(ang);
my = py + r*sin(ang);

idx = 3 + 2*(j-1) + 1;
mu(idx:idx+1) = [mx; my];
seen(j) = true;

% Gx (2x3), Gz (2x2)
c = cos(ang); s = sin(ang);
Gx = [1 0 -r*s;
      0 1  r*c];
Gz = [ c -r*s;
       s  r*c];

Pxx = P(1:3,1:3);
Psx = P(:,1:3);

Pmm = Gx*Pxx*Gx' + Gz*R*Gz';
Psm = Psx*Gx';

P(idx:idx+1, idx:idx+1) = Pmm;
P(:, idx:idx+1) = Psm;
P(idx:idx+1,:) = Psm';
end

function [mu,P] = ekf_update_one(mu,P,z,j,R,wrap)
N = (numel(mu)-3)/2;
idx = 3 + 2*(j-1) + 1;

x = mu(1:3);
m = mu(idx:idx+1);

% predicted measurement
zhat = rb_model(x,m,wrap);
y = z - zhat;
y(2) = wrap(y(2));

% H Jacobian (2 x dim)
px=x(1); py=x(2); th=x(3);
mx=m(1); my=m(2);
dx = mx - px; dy = my - py;
q = dx^2 + dy^2;
r = sqrt(q);

H = zeros(2, 3+2*N);

% robot part
H(1,1) = -dx/r;
H(1,2) = -dy/r;
H(1,3) = 0;
H(2,1) =  dy/q;
H(2,2) = -dx/q;
H(2,3) = -1;

% landmark part
H(1,idx)   =  dx/r;
H(1,idx+1) =  dy/r;
H(2,idx)   = -dy/q;
H(2,idx+1) =  dx/q;

S = H*P*H' + R;
K = P*H'/S;

mu = mu + K*y;
mu(3) = wrap(mu(3));

I = eye(size(P,1));
P = (I - K*H)*P*(I - K*H)' + K*R*K'; % Joseph
end

% -------------------------------
% Simulink skeleton (optional)
% -------------------------------
% To create a minimal Simulink scaffold programmatically (no .slx included):
% model = 'Chapter11_Lesson2_EKFSLAM';
% new_system(model); open_system(model);
% add_block('simulink/Sources/In1',[model '/u']);
% add_block('simulink/Sources/In1',[model '/z']);
% add_block('simulink/User-Defined Functions/MATLAB Function',[model '/EKF_SLAM_Step']);
% add_block('simulink/Sinks/Out1',[model '/mu']);
% add_line(model,'u/1','EKF_SLAM_Step/1');
% add_line(model,'z/1','EKF_SLAM_Step/2');
% add_line(model,'EKF_SLAM_Step/1','mu/1');
% save_system(model);
      

9.5 Wolfram Mathematica — Symbolic Jacobian + One EKF Update

Mathematica is useful for deriving Jacobians symbolically and validating EKF algebra.

File: Chapter11_Lesson2.nb

(* Chapter11_Lesson2.nb
   EKF-SLAM (structure and limitations) — Mathematica snippet:
   - Symbolic Jacobians for range-bearing
   - One EKF update step (small dimensional example)
*)

ClearAll["Global`*"];

wrap[a_] := Mod[a + Pi, 2 Pi] - Pi;

(* Symbols *)
px =.; py=.; th=.; mx=.; my=.;
dx = mx - px; dy = my - py;
q = dx^2 + dy^2;
r = Sqrt[q];
phi = ArcTan[dx, dy] - th; (* Note: ArcTan[x,y] in Mathematica is atan2(y,x) analog *)

z = {r, phi};

vars = {px, py, th, mx, my};

J = D[z, {vars}];
Print["Jacobian [dr; dphi] w.r.t [px,py,th,mx,my]:"];
MatrixForm[J]

(* Example numeric substitution *)
sub = {px -> 1.0, py -> 2.0, th -> 0.3, mx -> 5.0, my -> 6.0};
Jnum = N[J /. sub];
Print["Numeric Jacobian:"];
MatrixForm[Jnum]

(* One EKF update in a toy setting: state [px,py,th,mx,my] *)
mu = {1.0, 2.0, 0.3, 5.2, 5.8};
P = DiagonalMatrix[{0.05^2, 0.05^2, (2 Degree)^2, 0.2^2, 0.2^2}];
R = DiagonalMatrix[{0.10^2, (2 Degree)^2}];

(* Predicted measurement and Jacobian at mu *)
zhat = N[z /. Thread[vars -> mu]];
H = N[J /. Thread[vars -> mu]];

(* Suppose measured z is zhat plus some offset *)
zmeas = zhat + {0.05, -0.01};

y = zmeas - zhat;
y[[2]] = wrap[y[[2]]];

S = H.P.Transpose[H] + R;
K = P.Transpose[H].Inverse[S];

muNew = mu + K.y;
muNew[[3]] = wrap[muNew[[3]]];

I5 = IdentityMatrix[Length[mu]];
PNew = (I5 - K.H).P.Transpose[I5 - K.H] + K.R.Transpose[K]; (* Joseph form *)

Print["Updated mu:"];
muNew
Print["Updated P (top-left pose block):"];
MatrixForm[PNew[[1 ;; 3, 1 ;; 3]]]
      

Problem 1 (Range–Bearing Jacobian): For the measurement model in Section 2, \( r = \sqrt{(m_x-x)^2 + (m_y-y)^2} \), \( \phi = \operatorname{atan2}(m_y-y, m_x-x) - \theta \), derive the partial derivatives of \( r \) and \( \phi \) w.r.t. \( x, y, \theta, m_x, m_y \).

Solution: Let \( \Delta x=m_x-x \), \( \Delta y=m_y-y \), \( q=(\Delta x)^2+(\Delta y)^2 \), and \( r=\sqrt{q} \). Then the derivatives are:

\[ \frac{\partial r}{\partial x} = -\frac{\Delta x}{r},\; \frac{\partial r}{\partial y} = -\frac{\Delta y}{r},\; \frac{\partial r}{\partial \theta} = 0,\; \frac{\partial r}{\partial m_x} = \frac{\Delta x}{r},\; \frac{\partial r}{\partial m_y} = \frac{\Delta y}{r}. \]

\[ \frac{\partial \phi}{\partial x} = \frac{\Delta y}{q},\; \frac{\partial \phi}{\partial y} = -\frac{\Delta x}{q},\; \frac{\partial \phi}{\partial \theta} = -1,\; \frac{\partial \phi}{\partial m_x} = -\frac{\Delta y}{q},\; \frac{\partial \phi}{\partial m_y} = \frac{\Delta x}{q}. \]

Problem 2 (Joseph vs Simple Covariance Update): Show that the Joseph form \( \mathbf{P}^+ = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \) is symmetric and positive semidefinite when \( \mathbf{P} \) and \( \mathbf{R} \) are.

Solution: Symmetry follows because each term is of the form \( \mathbf{A}\mathbf{P}\mathbf{A}^\top \) and \( \mathbf{B}\mathbf{R}\mathbf{B}^\top \), both symmetric if \( \mathbf{P},\mathbf{R} \) are. PSD follows because for any vector \( \mathbf{t} \): \( \mathbf{t}^\top \mathbf{A}\mathbf{P}\mathbf{A}^\top \mathbf{t} = (\mathbf{A}^\top \mathbf{t})^\top \mathbf{P} (\mathbf{A}^\top \mathbf{t}) \ge 0 \) when \( \mathbf{P} \succeq 0 \), and similarly for the \( \mathbf{R} \) term. Therefore \( \mathbf{P}^+ \succeq 0 \).

Problem 3 (Why Landmark Correlations Appear): Consider a state with robot pose \( \mathbf{x} \) and two landmarks \( \mathbf{m}_1, \mathbf{m}_2 \). Suppose after some steps the covariance contains \( \operatorname{Cov}(\mathbf{x},\mathbf{m}_2) \neq 0 \). You now update using a measurement of \( \mathbf{m}_1 \). Explain algebraically why \( \operatorname{Cov}(\mathbf{m}_1,\mathbf{m}_2) \) generally becomes nonzero after the update.

Solution: In the Kalman update, \( \mathbf{P}^+ = \mathbf{P}^- - \mathbf{P}^-\mathbf{H}^\top\mathbf{S}^{-1}\mathbf{H}\mathbf{P}^- \). Here \( \mathbf{H} \) touches the robot and \( \mathbf{m}_1 \) blocks only. The product \( \mathbf{H}\mathbf{P}^- \) contains terms proportional to \( \operatorname{Cov}(\mathbf{x},\mathbf{m}_2) \) because those correlations sit in \( \mathbf{P}^- \). When the rank-2 correction is subtracted, it modifies the \( (\mathbf{m}_1,\mathbf{m}_2) \) block as well, creating \( \operatorname{Cov}(\mathbf{m}_1,\mathbf{m}_2) \neq 0 \) unless special cancellations occur.

Problem 4 (Gauge Freedom): Argue that with only relative measurements (range–bearing to landmarks) and relative motion, the global translation and rotation are not observable. Provide a concise invariance argument.

Solution: Let \( \mathbf{T} \in SE(2) \) be any rigid transform. Replace every pose and landmark by \( \mathbf{T} \)-transformed versions: \( (\mathbf{x}_k,\mathbf{m}_j) \mapsto (\mathbf{T}\mathbf{x}_k, \mathbf{T}\mathbf{m}_j) \). Because range and bearing depend only on the relative vector \( \mathbf{m}_j - \mathbf{x}_k \) expressed in the robot frame, all predicted measurements are unchanged, so the likelihood is invariant: \( p(\mathbf{z} \mid \mathbf{x},\mathbf{m})=p(\mathbf{z} \mid \mathbf{T}\mathbf{x},\mathbf{T}\mathbf{m}) \). Hence these global degrees of freedom cannot be inferred from data alone (unobservable).

Problem 5 (Complexity Estimate): Assume a dense covariance for EKF-SLAM with \( N \) landmarks. Estimate memory to store \( \mathbf{P} \) and the dominant cost to update \( \mathbf{P} \) for one measurement.

Solution: The state dimension is \( D = 3 + 2N \). Storing \( \mathbf{P} \in \mathbb{R}^{D\times D} \) costs \( O(D^2) = O(N^2) \). In the update, \( \mathbf{S} \) is \( 2\times 2 \), but updating \( \mathbf{P}^+ \) (e.g., via Joseph form) involves matrix products between \( (\mathbf{I}-\mathbf{K}\mathbf{H}) \) and dense \( \mathbf{P} \), yielding \( O(D^2) = O(N^2) \) per measurement as the dominant term.

EKF-SLAM represents the joint pose–map posterior as a single Gaussian over an augmented state. We derived the prediction, measurement update, and landmark augmentation steps, emphasizing the role of cross-correlations. The same structure that makes EKF-SLAM statistically coherent for small problems also causes its limitations: the covariance becomes dense, leading to \( O(N^2) \) memory/time scaling; nonlinear linearization can break gauge invariances and produce inconsistency; and incorrect association can catastrophically corrupt the estimate. These limitations motivate the next lesson’s factorization idea (FastSLAM) and the later transition to graph-based SLAM.