In Lesson 1 we localized a planar robot pose \( \mathbf{x}_k = [x_k,\; y_k,\; \theta_k]^\top \) using an EKF: nonlinear models were linearized via Jacobians. The UKF uses the same Gaussian-belief assumption, but replaces Jacobians with a carefully chosen set of deterministic samples (sigma points) that capture the first two moments.

We will work with the standard discrete-time state-space model (additive noise form):

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k,\quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k), \qquad \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k,\quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k). \]

The key question is: given a Gaussian belief \( \mathbf{x} \sim \mathcal{N}(\boldsymbol{\mu}, \mathbf{P}) \) and a nonlinear map \( \mathbf{y} = g(\mathbf{x}) \), how do we approximate \( \mathbb{E}[\mathbf{y}] \) and \( \operatorname{Cov}(\mathbf{y}) \)? EKF uses a first-order Taylor approximation; UKF uses sigma points.

flowchart TD
  A["Prior belief: x_hat(k), P(k)"] --> B["Choose sigma points (2L+1) around x_hat(k)"]
  B --> C["Propagate sigma points through motion model: x_i- = f(x_i, u_k)"]
  C --> D["Recombine: x_hat-(k+1), P-(k+1)"]
  D --> E["Propagate through measurement model: z_i = h(x_i-)"]
  E --> F["Recombine: z_hat, S, cross-cov Pxz"]
  F --> G["Gain: K = Pxz * inv(S)"]
  G --> H["Update: x_hat = x_hat- + K*(z - z_hat)"]
  H --> I["Update covariance: P = P- - K*S*K^T"]
  I --> A
        

When the UKF helps (high-level): (i) when posterior uncertainty is not small, so a local linearization is unreliable; (ii) when measurement functions are strongly curved (e.g., range-bearing); (iii) when motion includes strong heading uncertainty. The UKF does not “fix” wrong models; it approximates uncertainty propagation more accurately than EKF in many nonlinear regimes. :contentReference[oaicite:0]{index=0}

Let \( \mathbf{x} \in \mathbb{R}^L \) have mean \( \boldsymbol{\mu} \) and covariance \( \mathbf{P} \). The UT constructs sigma points \( \chi_i \) and weights \( W_i^m, W_i^c \) such that the weighted sample mean and covariance match \( \boldsymbol{\mu}, \mathbf{P} \).

Define scaling parameters \( \alpha \), \( \beta \), \( \kappa \), and \( \lambda = \alpha^2 (L + \kappa) - L \). Let \( \mathbf{S} \) satisfy \( \mathbf{S}\mathbf{S}^\top = (L+\lambda)\mathbf{P} \).

\[ \chi_0 = \boldsymbol{\mu},\qquad \chi_i = \boldsymbol{\mu} + \mathbf{S}_i,\qquad \chi_{i+L} = \boldsymbol{\mu} - \mathbf{S}_i,\quad i=1,\dots,L. \]

\[ W_0^m = \frac{\lambda}{L+\lambda},\quad W_0^c = \frac{\lambda}{L+\lambda} + (1-\alpha^2+\beta),\qquad W_i^m = W_i^c = \frac{1}{2(L+\lambda)}\;\; (i=1,\dots,2L). \]

For \( \mathbf{y} = g(\mathbf{x}) \), transform sigma points \( \gamma_i = g(\chi_i) \) and recombine:

\[ \hat{\boldsymbol{\mu} }_y = \sum_{i=0}^{2L} W_i^m\, \gamma_i,\qquad \hat{\mathbf{P} }_y = \sum_{i=0}^{2L} W_i^c\, (\gamma_i-\hat{\boldsymbol{\mu} }_y)(\gamma_i-\hat{\boldsymbol{\mu} }_y)^\top. \]

flowchart TD
  X0["Gaussian belief (mu, P)"] --> SP["Sigma points: chi_i"]
  SP --> MAP["Nonlinear map: y = g(x)"]
  MAP --> YSP["Transformed points: gamma_i"]
  YSP --> MU["Weighted mean: mu_y"]
  YSP --> COV["Weighted covariance: P_y"]
        

The UT captures nonlinear curvature by sampling away from the mean. Standard practice uses \( \beta=2 \) for Gaussian priors. :contentReference[oaicite:1]{index=1}

3.1 Theorem (UT is exact for affine transformations)

If \( g(\mathbf{x})=\mathbf{A}\mathbf{x}+\mathbf{b} \) and \( \mathbf{x} \sim \mathcal{N}(\boldsymbol{\mu},\mathbf{P}) \), then UT yields \( \hat{\boldsymbol{\mu} }_y = \mathbf{A}\boldsymbol{\mu}+\mathbf{b} \), \( \hat{\mathbf{P} }_y = \mathbf{A}\mathbf{P}\mathbf{A}^\top \).

Proof.

\[ \hat{\boldsymbol{\mu} }_y = \sum_i W_i^m(\mathbf{A}\chi_i+\mathbf{b}) = \mathbf{A}\Big(\sum_i W_i^m\chi_i\Big)+\mathbf{b} = \mathbf{A}\boldsymbol{\mu}+\mathbf{b}. \]

\[ \hat{\mathbf{P} }_y = \sum_i W_i^c\mathbf{A}(\chi_i-\boldsymbol{\mu})(\chi_i-\boldsymbol{\mu})^\top\mathbf{A}^\top = \mathbf{A}\mathbf{P}\mathbf{A}^\top. \]

□

3.2 Second-order accuracy (Taylor expansion argument)

Expand a smooth scalar \( g \) around \( \boldsymbol{\mu} \):

\[ g(\boldsymbol{\mu}+\delta) = g(\boldsymbol{\mu}) + \nabla g(\boldsymbol{\mu})^\top\delta + \tfrac{1}{2}\delta^\top \mathbf{H}(\boldsymbol{\mu})\delta + \mathcal{O}(\|\delta\|^3). \]

\[ \mathbb{E}[g(\mathbf{x})] = g(\boldsymbol{\mu}) + \tfrac{1}{2}\operatorname{tr}\big(\mathbf{H}(\boldsymbol{\mu})\mathbf{P}\big) + \mathcal{O}(\mathbb{E}[\|\delta\|^3]). \]

UT’s symmetric sigma points cancel odd-order terms and match second-order central moments, producing second-order-accurate moment estimates without Jacobians. :contentReference[oaicite:2]{index=2}

We use pose state \( \mathbf{x}=[x, y, \theta]^\top \) with differential-drive kinematic motion:

\[ f(\mathbf{x}_k, \mathbf{u}_k) = \begin{bmatrix} x_k + \Delta t\, v_k\cos\theta_k \\ y_k + \Delta t\, v_k\sin\theta_k \\ \operatorname{wrap}(\theta_k + \Delta t\,\omega_k) \end{bmatrix}. \]

Landmark range-bearing:

\[ h(\mathbf{x}) = \begin{bmatrix} \sqrt{(m_x-x)^2 + (m_y-y)^2} \\ \operatorname{wrap}\big(\operatorname{atan2}(m_y-y,\,m_x-x) - \theta\big) \end{bmatrix}. \]

Circular mean for angles:

\[ \bar{\phi} = \operatorname{atan2}\Big(\sum_i W_i^m\sin\phi_i,\; \sum_i W_i^m\cos\phi_i\Big). \]

Predict:

\[ \chi_i^- = f(\chi_i, \mathbf{u}_k),\qquad \hat{\mathbf{x} }^- = \sum_i W_i^m\chi_i^-,\qquad \mathbf{P}^- = \sum_i W_i^c(\chi_i^- - \hat{\mathbf{x} }^-)(\chi_i^- - \hat{\mathbf{x} }^-)^\top + \mathbf{Q}_k. \]

Update:

\[ \zeta_i = h(\chi_i^-),\qquad \hat{\mathbf{z} } = \sum_i W_i^m\zeta_i,\qquad \mathbf{S} = \sum_i W_i^c(\zeta_i-\hat{\mathbf{z} })(\zeta_i-\hat{\mathbf{z} })^\top + \mathbf{R}_k. \]

\[ \mathbf{P}_{xz} = \sum_i W_i^c(\chi_i^- - \hat{\mathbf{x} }^-)(\zeta_i - \hat{\mathbf{z} })^\top,\qquad \mathbf{K} = \mathbf{P}_{xz}\mathbf{S}^{-1}. \]

\[ \hat{\mathbf{x} } = \hat{\mathbf{x} }^- + \mathbf{K}(\mathbf{z}-\hat{\mathbf{z} }),\qquad \mathbf{P} = \mathbf{P}^- - \mathbf{K}\mathbf{S}\mathbf{K}^\top. \]

5.1 Second-order bias heuristic

\[ b_j \approx \tfrac{1}{2}\operatorname{tr}\big(\mathbf{H}_j\,\mathbf{P}\big), \]

If \( \|\mathbf{b}\| \) is comparable to measurement noise standard deviations, EKF innovation statistics become biased; UKF tends to be more consistent because it captures curvature in the predicted moments. :contentReference[oaicite:3]{index=3}

5.2 When UKF often improves AMR localization

• Large heading uncertainty + turning: nonlinearity of \( \cos\theta,\sin\theta \) under wide distributions.
• Range-bearing curvature: \( \sqrt{\cdot} \) and \( \operatorname{atan2} \) create strong local curvature.

5.3 When UKF may not help

• Near-linear / small-uncertainty regimes: EKF and UKF perform similarly.
• Severely non-Gaussian posteriors: later handled by Particle Filters (Chapter 8).
• High-dimensional states: sigma points are \(2L+1\); for SLAM-scale states, computational and consistency issues arise. :contentReference[oaicite:4]{index=4}

6.1 Angle wrapping

Always wrap angle differences: \( \operatorname{wrap}(a-b)=\operatorname{atan2}(\sin(a-b),\cos(a-b)) \).

6.2 Positive definiteness

• \( \mathbf{P} \leftarrow \tfrac{1}{2}(\mathbf{P}+\mathbf{P}^\top) \) (symmetrize)
• \( \mathbf{P} \leftarrow \mathbf{P} + \epsilon\mathbf{I} \) (jitter)
• Square-root UKF improves numerical stability. :contentReference[oaicite:5]{index=5}

6.3 Choosing \( \alpha,\beta,\kappa \)

Typical defaults: \( 10^{-3} \le \alpha \le 1 \), \( \beta=2 \), \( \kappa=0 \).

Practical ecosystem note: in robotics stacks you may encounter UKF/EKF implementations in ROS tooling and Python libraries. This lab is intentionally from-scratch to expose the exact sigma-point mechanics.

File: Chapter7_Lesson2.py

"""
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2.py

This script implements a from-scratch UKF for planar pose localization
(x, y, theta) with:
  - motion model: differential-drive kinematics with control (v, omega)
  - measurement model: range-bearing to a known landmark (mx, my)

It also includes a baseline EKF implementation for comparison (Jacobian-based),
consistent with Chapter 7 Lesson 1.

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

from __future__ import annotations

import math
import numpy as np


def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return math.atan2(math.sin(a), math.cos(a))


def circular_mean(angles: np.ndarray, w: np.ndarray) -> float:
    """Weighted circular mean."""
    s = float(np.sum(w * np.sin(angles)))
    c = float(np.sum(w * np.cos(angles)))
    return math.atan2(s, c)


def ensure_spd(P: np.ndarray, eps: float = 1e-10) -> np.ndarray:
    """Symmetrize and jitter covariance to keep it SPD."""
    P = 0.5 * (P + P.T)
    # Jitter increases until Cholesky works.
    jitter = eps
    for _ in range(8):
        try:
            np.linalg.cholesky(P + jitter * np.eye(P.shape[0]))
            return P + jitter * np.eye(P.shape[0])
        except np.linalg.LinAlgError:
            jitter *= 10.0
    # Fallback (not ideal): add large jitter.
    return P + 1e-3 * np.eye(P.shape[0])


def sigma_points(mu: np.ndarray, P: np.ndarray, alpha: float, beta: float, kappa: float):
    """
    Scaled sigma points for Unscented Transform.

    Returns:
        X: (2L+1, L) sigma points
        Wm: (2L+1,) mean weights
        Wc: (2L+1,) covariance weights
        lam: lambda parameter
    """
    mu = mu.reshape(-1)
    L = mu.size
    lam = alpha**2 * (L + kappa) - L
    c = L + lam

    P_spd = ensure_spd(P)
    S = np.linalg.cholesky(c * P_spd)

    X = np.zeros((2 * L + 1, L), dtype=float)
    X[0] = mu
    for i in range(L):
        X[1 + i] = mu + S[:, i]
        X[1 + L + i] = mu - S[:, i]

    Wm = np.full(2 * L + 1, 1.0 / (2.0 * c), dtype=float)
    Wc = np.full(2 * L + 1, 1.0 / (2.0 * c), dtype=float)
    Wm[0] = lam / c
    Wc[0] = lam / c + (1.0 - alpha**2 + beta)
    return X, Wm, Wc, lam


def motion_model(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """x=[x,y,theta], u=[v,omega]."""
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    v, w = float(u[0]), float(u[1])
    px2 = px + dt * v * math.cos(th)
    py2 = py + dt * v * math.sin(th)
    th2 = wrap_angle(th + dt * w)
    return np.array([px2, py2, th2], dtype=float)


def measurement_model(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Range-bearing measurement to landmark [mx,my]."""
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    mx, my = float(landmark[0]), float(landmark[1])
    dx = mx - px
    dy = my - py
    rng = math.sqrt(dx * dx + dy * dy)
    brg = wrap_angle(math.atan2(dy, dx) - th)
    return np.array([rng, brg], dtype=float)


def unscented_mean(X: np.ndarray, Wm: np.ndarray, angle_idx: list[int] | None = None) -> np.ndarray:
    """
    Weighted mean with optional circular mean on specified indices.
    """
    mu = np.sum(Wm[:, None] * X, axis=0)
    if angle_idx:
        for j in angle_idx:
            mu[j] = circular_mean(X[:, j], Wm)
    return mu


def unscented_cov(
    X: np.ndarray,
    mu: np.ndarray,
    Wc: np.ndarray,
    angle_idx: list[int] | None = None,
) -> np.ndarray:
    """
    Weighted covariance with wrapped differences for angle indices.
    """
    L = mu.size
    P = np.zeros((L, L), dtype=float)
    for i in range(X.shape[0]):
        d = X[i] - mu
        if angle_idx:
            for j in angle_idx:
                d[j] = wrap_angle(float(d[j]))
        P += Wc[i] * np.outer(d, d)
    return 0.5 * (P + P.T)


class UKF:
    def __init__(
        self,
        x0: np.ndarray,
        P0: np.ndarray,
        Q: np.ndarray,
        R: np.ndarray,
        dt: float,
        alpha: float = 0.3,
        beta: float = 2.0,
        kappa: float = 0.0,
    ):
        self.x = x0.astype(float).reshape(-1)
        self.P = P0.astype(float)
        self.Q = Q.astype(float)
        self.R = R.astype(float)
        self.dt = float(dt)
        self.alpha = float(alpha)
        self.beta = float(beta)
        self.kappa = float(kappa)

        # state angle index
        self.state_angle = [2]

        # caches
        self._Xsig = None
        self._Wm = None
        self._Wc = None

    def predict(self, u: np.ndarray):
        X, Wm, Wc, _ = sigma_points(self.x, self.P, self.alpha, self.beta, self.kappa)
        Xp = np.zeros_like(X)
        for i in range(X.shape[0]):
            Xp[i] = motion_model(X[i], u, self.dt)

        x_pred = unscented_mean(Xp, Wm, angle_idx=self.state_angle)
        P_pred = unscented_cov(Xp, x_pred, Wc, angle_idx=self.state_angle) + self.Q

        self.x = x_pred
        self.P = ensure_spd(P_pred)

        self._Xsig = Xp
        self._Wm = Wm
        self._Wc = Wc

    def update(self, z: np.ndarray, landmark: np.ndarray):
        assert self._Xsig is not None, "Call predict() before update()."

        Xp = self._Xsig
        Wm = self._Wm
        Wc = self._Wc

        # transform to measurement space
        Zsig = np.zeros((Xp.shape[0], 2), dtype=float)
        for i in range(Xp.shape[0]):
            Zsig[i] = measurement_model(Xp[i], landmark)

        # bearing is an angle (index 1)
        z_pred = unscented_mean(Zsig, Wm, angle_idx=[1])
        S = unscented_cov(Zsig, z_pred, Wc, angle_idx=[1]) + self.R

        # cross covariance
        Pxz = np.zeros((self.x.size, 2), dtype=float)
        for i in range(Xp.shape[0]):
            dx = Xp[i] - self.x
            dx[2] = wrap_angle(float(dx[2]))
            dz = Zsig[i] - z_pred
            dz[1] = wrap_angle(float(dz[1]))
            Pxz += Wc[i] * np.outer(dx, dz)

        # gain and update
        K = Pxz @ np.linalg.inv(S)
        innov = z - z_pred
        innov[1] = wrap_angle(float(innov[1]))

        self.x = self.x + K @ innov
        self.x[2] = wrap_angle(float(self.x[2]))
        self.P = ensure_spd(self.P - K @ S @ K.T)


# ---------- Baseline EKF (Jacobian-based) ----------

def jacobian_F(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Jacobian of motion model w.r.t state."""
    th = float(x[2])
    v = float(u[0])
    F = np.eye(3, dtype=float)
    F[0, 2] = -dt * v * math.sin(th)
    F[1, 2] = dt * v * math.cos(th)
    return F


def jacobian_H(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Jacobian of range-bearing measurement w.r.t state."""
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    mx, my = float(landmark[0]), float(landmark[1])
    dx = mx - px
    dy = my - py
    q = dx * dx + dy * dy
    r = math.sqrt(q)

    H = np.zeros((2, 3), dtype=float)
    # dr/dx, dr/dy
    H[0, 0] = -dx / r
    H[0, 1] = -dy / r
    # db/dx, db/dy, db/dtheta
    H[1, 0] = dy / q
    H[1, 1] = -dx / q
    H[1, 2] = -1.0
    return H


class EKF:
    def __init__(self, x0: np.ndarray, P0: np.ndarray, Q: np.ndarray, R: np.ndarray, dt: float):
        self.x = x0.astype(float).reshape(-1)
        self.P = P0.astype(float)
        self.Q = Q.astype(float)
        self.R = R.astype(float)
        self.dt = float(dt)

    def predict(self, u: np.ndarray):
        self.x = motion_model(self.x, u, self.dt)
        F = jacobian_F(self.x, u, self.dt)
        self.P = ensure_spd(F @ self.P @ F.T + self.Q)

    def update(self, z: np.ndarray, landmark: np.ndarray):
        zhat = measurement_model(self.x, landmark)
        H = jacobian_H(self.x, landmark)
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        innov = z - zhat
        innov[1] = wrap_angle(float(innov[1]))
        self.x = self.x + K @ innov
        self.x[2] = wrap_angle(float(self.x[2]))
        self.P = ensure_spd(self.P - K @ S @ K.T)


# ---------- Simulation / Demonstration ----------

def run_demo(seed: int = 7, do_plot: bool = True):
    np.random.seed(seed)

    dt = 0.1
    T = 250
    landmark = np.array([5.0, 5.0], dtype=float)

    # Ground-truth initial state
    x_true = np.array([0.0, 0.0, 0.0], dtype=float)

    # Filter initial belief
    x0 = np.array([0.5, -0.3, 0.2], dtype=float)
    P0 = np.diag([0.5**2, 0.5**2, (15.0 * math.pi / 180.0) ** 2])

    # Process noise (model mismatch / unmodeled disturbances)
    Q = np.diag([0.02**2, 0.02**2, (1.0 * math.pi / 180.0) ** 2])

    # Measurement noise
    R = np.diag([0.10**2, (3.0 * math.pi / 180.0) ** 2])

    ukf = UKF(x0, P0, Q, R, dt, alpha=0.3, beta=2.0, kappa=0.0)
    ekf = EKF(x0, P0, Q, R, dt)

    # Control nominal
    v_nom = 1.0
    w_nom = 0.25

    # Control noise (acts through true dynamics)
    sig_v = 0.05
    sig_w = 0.02

    # Logs
    Xtrue = np.zeros((T, 3))
    Xukf = np.zeros((T, 3))
    Xekf = np.zeros((T, 3))

    for k in range(T):
        # Noisy control applied to the true system
        v = v_nom + np.random.normal(0.0, sig_v)
        w = w_nom + np.random.normal(0.0, sig_w)
        u = np.array([v_nom, w_nom], dtype=float)  # filter uses nominal command
        u_true = np.array([v, w], dtype=float)

        # propagate truth
        x_true = motion_model(x_true, u_true, dt)

        # generate measurement
        z = measurement_model(x_true, landmark)
        z = z + np.array([np.random.normal(0.0, math.sqrt(R[0, 0])),
                          np.random.normal(0.0, math.sqrt(R[1, 1]))], dtype=float)
        z[1] = wrap_angle(float(z[1]))

        # filters
        ukf.predict(u)
        ukf.update(z, landmark)

        ekf.predict(u)
        ekf.update(z, landmark)

        # log
        Xtrue[k] = x_true
        Xukf[k] = ukf.x
        Xekf[k] = ekf.x

    # RMSE (position)
    e_ukf = Xukf[:, :2] - Xtrue[:, :2]
    e_ekf = Xekf[:, :2] - Xtrue[:, :2]
    rmse_ukf = math.sqrt(float(np.mean(np.sum(e_ukf**2, axis=1))))
    rmse_ekf = math.sqrt(float(np.mean(np.sum(e_ekf**2, axis=1))))

    print("RMSE position (UKF):", rmse_ukf)
    print("RMSE position (EKF):", rmse_ekf)

    if do_plot:
        try:
            import matplotlib.pyplot as plt

            plt.figure()
            plt.plot(Xtrue[:, 0], Xtrue[:, 1], label="true")
            plt.plot(Xekf[:, 0], Xekf[:, 1], label="EKF")
            plt.plot(Xukf[:, 0], Xukf[:, 1], label="UKF")
            plt.scatter([landmark[0]], [landmark[1]], marker="x", label="landmark")
            plt.axis("equal")
            plt.legend()
            plt.title("Planar localization: EKF vs UKF")
            plt.show()
        except Exception as ex:
            print("Plot skipped:", ex)


if __name__ == "__main__":
    run_demo(seed=7, do_plot=True)
      

Exercise file: Chapter7_Lesson2_Ex1.py

"""
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2_Ex1.py

Exercise:
1) Run the demo and record RMSE(EKF) and RMSE(UKF).
2) Increase heading uncertainty in P0 (e.g., 45 deg, 90 deg). Compare EKF vs UKF.
3) Increase turning rate w_nom and observe when EKF deteriorates.
4) (Optional) add a second landmark and fuse two measurements per step.

This file reuses the core UKF/EKF code from Chapter7_Lesson2.py
to focus on experimental exploration.
"""

import math
import numpy as np

from Chapter7_Lesson2 import UKF, EKF, motion_model, measurement_model, wrap_angle


def run_experiment(P0_theta_deg: float = 45.0, w_nom: float = 0.40, seed: int = 1):
    np.random.seed(seed)

    dt = 0.1
    T = 250
    landmark = np.array([5.0, 5.0], dtype=float)

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

    x0 = np.array([0.5, -0.3, 0.2], dtype=float)
    P0 = np.diag([0.5**2, 0.5**2, (P0_theta_deg * math.pi / 180.0) ** 2])

    Q = np.diag([0.02**2, 0.02**2, (1.0 * math.pi / 180.0) ** 2])
    R = np.diag([0.10**2, (3.0 * math.pi / 180.0) ** 2])

    ukf = UKF(x0, P0, Q, R, dt, alpha=0.3, beta=2.0, kappa=0.0)
    ekf = EKF(x0, P0, Q, R, dt)

    v_nom = 1.0

    sig_v = 0.05
    sig_w = 0.02

    Xtrue = np.zeros((T, 3))
    Xukf = np.zeros((T, 3))
    Xekf = np.zeros((T, 3))

    for k in range(T):
        v = v_nom + np.random.normal(0.0, sig_v)
        w = w_nom + np.random.normal(0.0, sig_w)
        u = np.array([v_nom, w_nom], dtype=float)
        u_true = np.array([v, w], dtype=float)

        x_true = motion_model(x_true, u_true, dt)

        z = measurement_model(x_true, landmark)
        z = z + np.array([np.random.normal(0.0, math.sqrt(R[0, 0])),
                          np.random.normal(0.0, math.sqrt(R[1, 1]))], dtype=float)
        z[1] = wrap_angle(float(z[1]))

        ukf.predict(u)
        ukf.update(z, landmark)

        ekf.predict(u)
        ekf.update(z, landmark)

        Xtrue[k] = x_true
        Xukf[k] = ukf.x
        Xekf[k] = ekf.x

    e_ukf = Xukf[:, :2] - Xtrue[:, :2]
    e_ekf = Xekf[:, :2] - Xtrue[:, :2]
    rmse_ukf = math.sqrt(float(np.mean(np.sum(e_ukf**2, axis=1))))
    rmse_ekf = math.sqrt(float(np.mean(np.sum(e_ekf**2, axis=1))))

    return rmse_ukf, rmse_ekf


if __name__ == "__main__":
    for th in [15.0, 45.0, 90.0]:
        rmse_u, rmse_e = run_experiment(P0_theta_deg=th, w_nom=0.40, seed=1)
        print(f"P0_theta_deg={th:5.1f} -> RMSE(UKF)={rmse_u:.3f}, RMSE(EKF)={rmse_e:.3f}")
      

Typical robotics C++ stacks rely on high-performance linear algebra (Eigen) and middleware (often ROS2). This file uses Eigen only.

File: Chapter7_Lesson2.cpp

/*
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2.cpp

From-scratch UKF for planar pose localization (x, y, theta) with:
  - motion model: differential-drive kinematics (v, omega)
  - measurement model: range-bearing to a known landmark

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

Dependency: Eigen (matrix algebra)
*/

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

static inline double wrapAngle(double a) {
  return std::atan2(std::sin(a), std::cos(a));
}

static inline double circularMean(const Eigen::VectorXd& angles, const Eigen::VectorXd& w) {
  double s = 0.0, c = 0.0;
  for (int i = 0; i < angles.size(); ++i) {
    s += w(i) * std::sin(angles(i));
    c += w(i) * std::cos(angles(i));
  }
  return std::atan2(s, c);
}

static inline Eigen::Vector3d motionModel(const Eigen::Vector3d& x, const Eigen::Vector2d& u, double dt) {
  double px = x(0), py = x(1), th = x(2);
  double v = u(0), w = u(1);
  Eigen::Vector3d xp;
  xp(0) = px + dt * v * std::cos(th);
  xp(1) = py + dt * v * std::sin(th);
  xp(2) = wrapAngle(th + dt * w);
  return xp;
}

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

struct UKF {
  // state dimension L=3, measurement dimension M=2
  Eigen::Vector3d x;
  Eigen::Matrix3d P;
  Eigen::Matrix3d Q;
  Eigen::Matrix2d R;
  double dt;
  double alpha, beta, kappa;
  double lambda;

  // sigma points caches
  Eigen::Matrix<double, 3, 7> Xsig_pred; // L x (2L+1)
  Eigen::VectorXd Wm; // (2L+1)
  Eigen::VectorXd Wc; // (2L+1)

  UKF(const Eigen::Vector3d& x0,
      const Eigen::Matrix3d& P0,
      const Eigen::Matrix3d& Q_,
      const Eigen::Matrix2d& R_,
      double dt_,
      double alpha_=0.3, double beta_=2.0, double kappa_=0.0)
      : x(x0), P(P0), Q(Q_), R(R_), dt(dt_), alpha(alpha_), beta(beta_), kappa(kappa_) {
    int L = 3;
    lambda = alpha*alpha*(L + kappa) - L;

    Wm = Eigen::VectorXd::Constant(2*L + 1, 1.0 / (2.0*(L + lambda)));
    Wc = Wm;
    Wm(0) = lambda / (L + lambda);
    Wc(0) = lambda / (L + lambda) + (1.0 - alpha*alpha + beta);
  }

  void sigmaPoints(Eigen::Matrix<double,3,7>& Xsig) {
    int L = 3;
    double c = L + lambda;

    // Ensure symmetry
    P = 0.5*(P + P.transpose());

    // Cholesky of c*P; add jitter if needed
    Eigen::Matrix3d Pj = P;
    double jitter = 1e-10;
    bool ok = false;
    for (int t = 0; t < 8; ++t) {
      Eigen::LLT<Eigen::Matrix3d> llt(c*(Pj + jitter*Eigen::Matrix3d::Identity()));
      if (llt.info() == Eigen::Success) {
        Eigen::Matrix3d S = llt.matrixL();
        Xsig.col(0) = x;
        for (int i = 0; i < L; ++i) {
          Xsig.col(1 + i)     = x + S.col(i);
          Xsig.col(1 + L + i) = x - S.col(i);
        }
        ok = true;
        break;
      }
      jitter *= 10.0;
    }
    if (!ok) {
      // fallback
      Eigen::LLT<Eigen::Matrix3d> llt(c*(P + 1e-3*Eigen::Matrix3d::Identity()));
      Eigen::Matrix3d S = llt.matrixL();
      Xsig.col(0) = x;
      for (int i = 0; i < L; ++i) {
        Xsig.col(1 + i)     = x + S.col(i);
        Xsig.col(1 + L + i) = x - S.col(i);
      }
    }
  }

  void predict(const Eigen::Vector2d& u) {
    Eigen::Matrix<double,3,7> Xsig;
    sigmaPoints(Xsig);

    // propagate
    for (int i = 0; i < 7; ++i) {
      Xsig_pred.col(i) = motionModel(Xsig.col(i), u, dt);
    }

    // mean (theta uses circular mean)
    Eigen::Vector3d xpred = Eigen::Vector3d::Zero();
    for (int i = 0; i < 7; ++i) {
      xpred(0) += Wm(i) * Xsig_pred(0, i);
      xpred(1) += Wm(i) * Xsig_pred(1, i);
    }
    // theta
    Eigen::VectorXd th(7);
    for (int i = 0; i < 7; ++i) th(i) = Xsig_pred(2, i);
    xpred(2) = circularMean(th, Wm);

    // covariance
    Eigen::Matrix3d Ppred = Eigen::Matrix3d::Zero();
    for (int i = 0; i < 7; ++i) {
      Eigen::Vector3d d = Xsig_pred.col(i) - xpred;
      d(2) = wrapAngle(d(2));
      Ppred += Wc(i) * (d * d.transpose());
    }
    P = 0.5*(Ppred + Ppred.transpose()) + Q;
    x = xpred;
  }

  void update(const Eigen::Vector2d& z, const Eigen::Vector2d& lm) {
    // transform sigma points to measurement
    Eigen::Matrix<double,2,7> Zsig;
    for (int i = 0; i < 7; ++i) {
      Zsig.col(i) = measModel(Xsig_pred.col(i), lm);
    }

    // predicted measurement mean (bearing circular)
    Eigen::Vector2d zpred = Eigen::Vector2d::Zero();
    for (int i = 0; i < 7; ++i) {
      zpred(0) += Wm(i) * Zsig(0, i);
    }
    Eigen::VectorXd b(7);
    for (int i = 0; i < 7; ++i) b(i) = Zsig(1, i);
    zpred(1) = circularMean(b, Wm);

    // innovation covariance S
    Eigen::Matrix2d S = Eigen::Matrix2d::Zero();
    for (int i = 0; i < 7; ++i) {
      Eigen::Vector2d dz = Zsig.col(i) - zpred;
      dz(1) = wrapAngle(dz(1));
      S += Wc(i) * (dz * dz.transpose());
    }
    S += R;

    // cross-covariance Pxz
    Eigen::Matrix<double,3,2> Pxz = Eigen::Matrix<double,3,2>::Zero();
    for (int i = 0; i < 7; ++i) {
      Eigen::Vector3d dx = Xsig_pred.col(i) - x;
      dx(2) = wrapAngle(dx(2));
      Eigen::Vector2d dz = Zsig.col(i) - zpred;
      dz(1) = wrapAngle(dz(1));
      Pxz += Wc(i) * (dx * dz.transpose());
    }

    Eigen::Matrix<double,3,2> K = Pxz * S.inverse();
    Eigen::Vector2d innov = z - zpred;
    innov(1) = wrapAngle(innov(1));

    x = x + K * innov;
    x(2) = wrapAngle(x(2));
    P = 0.5*(P + P.transpose()) - K * S * K.transpose();
    P = 0.5*(P + P.transpose());
  }
};

int main() {
  const double dt = 0.1;
  const int T = 250;
  Eigen::Vector2d lm(5.0, 5.0);

  // truth and filter initialization
  Eigen::Vector3d x_true(0.0, 0.0, 0.0);
  Eigen::Vector3d x0(0.5, -0.3, 0.2);

  Eigen::Matrix3d P0 = Eigen::Matrix3d::Zero();
  P0(0,0) = 0.5*0.5;
  P0(1,1) = 0.5*0.5;
  P0(2,2) = std::pow(15.0*M_PI/180.0, 2);

  Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
  Q(0,0) = 0.02*0.02;
  Q(1,1) = 0.02*0.02;
  Q(2,2) = std::pow(1.0*M_PI/180.0, 2);

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

  UKF ukf(x0, P0, Q, R, dt);

  // random generators for truth noise
  std::mt19937 rng(7);
  std::normal_distribution<double> nv(0.0, 0.05);
  std::normal_distribution<double> nw(0.0, 0.02);
  std::normal_distribution<double> nr(0.0, std::sqrt(R(0,0)));
  std::normal_distribution<double> nb(0.0, std::sqrt(R(1,1)));

  double v_nom = 1.0;
  double w_nom = 0.25;

  for (int k = 0; k < T; ++k) {
    // true control with noise; filter uses nominal control
    Eigen::Vector2d u_nom(v_nom, w_nom);
    Eigen::Vector2d u_true(v_nom + nv(rng), w_nom + nw(rng));

    x_true = motionModel(x_true, u_true, dt);

    Eigen::Vector2d z = measModel(x_true, lm);
    z(0) += nr(rng);
    z(1) = wrapAngle(z(1) + nb(rng));

    ukf.predict(u_nom);
    ukf.update(z, lm);
  }

  std::cout << "Final UKF state estimate:\n" << ukf.x.transpose() << std::endl;
  std::cout << "Final true state:\n" << x_true.transpose() << std::endl;
  std::cout << "Final covariance diag:\n" << ukf.P.diagonal().transpose() << std::endl;
  return 0;
}
      

In Java robotics contexts, EJML is a common matrix library; some robotics stacks also use ROSJava for messaging. This lab focuses strictly on UKF math using EJML.

File: Chapter7_Lesson2.java

/*
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2.java

From-scratch UKF for planar pose localization (x, y, theta) with:
  - motion model: differential-drive kinematics (v, omega)
  - measurement model: range-bearing to a known landmark

Dependency (matrix algebra): EJML (org.ejml:ejml-simple)
Example (Maven coordinates):
  <dependency>
    <groupId>org.ejml</groupId>
    <artifactId>ejml-simple</artifactId>
    <version>0.43.1</version>
  </dependency>

This code is written to be readable for course use; performance is not the focus.
*/

import org.ejml.simple.SimpleMatrix;

import java.util.Random;

public class Chapter7_Lesson2 {

    static double wrapAngle(double a) {
        return Math.atan2(Math.sin(a), Math.cos(a));
    }

    static double circularMean(double[] angles, double[] w) {
        double s = 0.0, c = 0.0;
        for (int i = 0; i < angles.length; i++) {
            s += w[i] * Math.sin(angles[i]);
            c += w[i] * Math.cos(angles[i]);
        }
        return Math.atan2(s, c);
    }

    static SimpleMatrix motionModel(SimpleMatrix x, SimpleMatrix u, double dt) {
        double px = x.get(0), py = x.get(1), th = x.get(2);
        double v = u.get(0), w = u.get(1);
        double px2 = px + dt * v * Math.cos(th);
        double py2 = py + dt * v * Math.sin(th);
        double th2 = wrapAngle(th + dt * w);
        return new SimpleMatrix(new double[][]{ {px2},{py2},{th2} });
    }

    static SimpleMatrix measurementModel(SimpleMatrix x, SimpleMatrix lm) {
        double px = x.get(0), py = x.get(1), th = x.get(2);
        double mx = lm.get(0), my = lm.get(1);
        double dx = mx - px;
        double dy = my - py;
        double r = Math.sqrt(dx*dx + dy*dy);
        double b = wrapAngle(Math.atan2(dy, dx) - th);
        return new SimpleMatrix(new double[][]{ {r},{b} });
    }

    static class UKF {
        final int L = 3;
        final int Nsig = 2*L + 1;

        SimpleMatrix x;       // 3x1
        SimpleMatrix P;       // 3x3
        SimpleMatrix Q;       // 3x3
        SimpleMatrix R;       // 2x2
        double dt;

        double alpha, beta, kappa;
        double lambda;
        double[] Wm;
        double[] Wc;

        SimpleMatrix XsigPred; // 3 x 7

        UKF(SimpleMatrix x0, SimpleMatrix P0, SimpleMatrix Q, SimpleMatrix R, double dt,
            double alpha, double beta, double kappa) {
            this.x = x0.copy();
            this.P = P0.copy();
            this.Q = Q.copy();
            this.R = R.copy();
            this.dt = dt;

            this.alpha = alpha;
            this.beta = beta;
            this.kappa = kappa;

            this.lambda = alpha*alpha*(L + kappa) - L;
            double c = L + lambda;

            this.Wm = new double[Nsig];
            this.Wc = new double[Nsig];
            for (int i = 0; i < Nsig; i++) {
                Wm[i] = 1.0 / (2.0*c);
                Wc[i] = 1.0 / (2.0*c);
            }
            Wm[0] = lambda / c;
            Wc[0] = lambda / c + (1.0 - alpha*alpha + beta);

            this.XsigPred = new SimpleMatrix(L, Nsig);
        }

        SimpleMatrix sigmaPoints(SimpleMatrix mu, SimpleMatrix P) {
            int L = this.L;
            double c = L + lambda;

            // Ensure symmetry and add small jitter
            SimpleMatrix Ps = P.plus(P.transpose()).scale(0.5).plus(SimpleMatrix.identity(L).scale(1e-10));

            // Cholesky via EJML decomposition (simple interface uses built-in)
            SimpleMatrix S = Ps.scale(c).chol().getT(null).transpose(); // lower-triangular approx

            SimpleMatrix Xsig = new SimpleMatrix(L, Nsig);
            Xsig.setColumn(0, 0, mu.get(0), mu.get(1), mu.get(2));
            for (int i = 0; i < L; i++) {
                SimpleMatrix col = S.extractVector(false, i);
                SimpleMatrix xp = mu.plus(col);
                SimpleMatrix xm = mu.minus(col);
                Xsig.setColumn(1+i, 0, xp.get(0), xp.get(1), xp.get(2));
                Xsig.setColumn(1+L+i, 0, xm.get(0), xm.get(1), xm.get(2));
            }
            return Xsig;
        }

        void predict(SimpleMatrix u) {
            SimpleMatrix Xsig = sigmaPoints(x, P);

            for (int i = 0; i < Nsig; i++) {
                SimpleMatrix xi = Xsig.extractVector(false, i);
                SimpleMatrix xp = motionModel(xi, u, dt);
                XsigPred.setColumn(i, 0, xp.get(0), xp.get(1), xp.get(2));
            }

            // mean (theta circular)
            double mx = 0.0, my = 0.0;
            double[] th = new double[Nsig];
            for (int i = 0; i < Nsig; i++) {
                mx += Wm[i] * XsigPred.get(0, i);
                my += Wm[i] * XsigPred.get(1, i);
                th[i] = XsigPred.get(2, i);
            }
            double mth = circularMean(th, Wm);
            x = new SimpleMatrix(new double[][]{ {mx},{my},{mth} });

            // covariance
            SimpleMatrix Pp = new SimpleMatrix(L, L);
            for (int i = 0; i < Nsig; i++) {
                double dx0 = XsigPred.get(0,i) - mx;
                double dx1 = XsigPred.get(1,i) - my;
                double dx2 = wrapAngle(XsigPred.get(2,i) - mth);
                SimpleMatrix d = new SimpleMatrix(new double[][]{ {dx0},{dx1},{dx2} });
                Pp = Pp.plus(d.mult(d.transpose()).scale(Wc[i]));
            }
            P = Pp.plus(Q);
        }

        void update(SimpleMatrix z, SimpleMatrix lm) {
            int M = 2;

            // transform into measurement space
            SimpleMatrix Zsig = new SimpleMatrix(M, Nsig);
            for (int i = 0; i < Nsig; i++) {
                SimpleMatrix xi = XsigPred.extractVector(false, i);
                SimpleMatrix zi = measurementModel(xi, lm);
                Zsig.setColumn(i, 0, zi.get(0), zi.get(1));
            }

            // predicted measurement mean (bearing circular)
            double mr = 0.0;
            double[] b = new double[Nsig];
            for (int i = 0; i < Nsig; i++) {
                mr += Wm[i] * Zsig.get(0, i);
                b[i] = Zsig.get(1, i);
            }
            double mb = circularMean(b, Wm);
            SimpleMatrix zhat = new SimpleMatrix(new double[][]{ {mr},{mb} });

            // innovation covariance S
            SimpleMatrix S = new SimpleMatrix(M, M);
            for (int i = 0; i < Nsig; i++) {
                double dz0 = Zsig.get(0,i) - mr;
                double dz1 = wrapAngle(Zsig.get(1,i) - mb);
                SimpleMatrix dz = new SimpleMatrix(new double[][]{ {dz0},{dz1} });
                S = S.plus(dz.mult(dz.transpose()).scale(Wc[i]));
            }
            S = S.plus(R);

            // cross-covariance Pxz
            SimpleMatrix Pxz = new SimpleMatrix(L, M);
            for (int i = 0; i < Nsig; i++) {
                double dx0 = XsigPred.get(0,i) - x.get(0);
                double dx1 = XsigPred.get(1,i) - x.get(1);
                double dx2 = wrapAngle(XsigPred.get(2,i) - x.get(2));
                SimpleMatrix dx = new SimpleMatrix(new double[][]{ {dx0},{dx1},{dx2} });

                double dz0 = Zsig.get(0,i) - mr;
                double dz1 = wrapAngle(Zsig.get(1,i) - mb);
                SimpleMatrix dz = new SimpleMatrix(new double[][]{ {dz0},{dz1} });

                Pxz = Pxz.plus(dx.mult(dz.transpose()).scale(Wc[i]));
            }

            SimpleMatrix K = Pxz.mult(S.invert());
            double innov0 = z.get(0) - mr;
            double innov1 = wrapAngle(z.get(1) - mb);
            SimpleMatrix innov = new SimpleMatrix(new double[][]{ {innov0},{innov1} });

            x = x.plus(K.mult(innov));
            x.set(2, wrapAngle(x.get(2)));
            P = P.minus(K.mult(S).mult(K.transpose()));
        }
    }

    public static void main(String[] args) {
        double dt = 0.1;
        int T = 250;

        SimpleMatrix lm = new SimpleMatrix(new double[][]{ {5.0},{5.0} });
        SimpleMatrix xTrue = new SimpleMatrix(new double[][]{ {0.0},{0.0},{0.0} });

        SimpleMatrix x0 = new SimpleMatrix(new double[][]{ {0.5},{-0.3},{0.2} });
        SimpleMatrix P0 = SimpleMatrix.diag(0.25, 0.25, Math.pow(15.0*Math.PI/180.0, 2));
        SimpleMatrix Q = SimpleMatrix.diag(0.02*0.02, 0.02*0.02, Math.pow(1.0*Math.PI/180.0, 2));
        SimpleMatrix R = SimpleMatrix.diag(0.10*0.10, Math.pow(3.0*Math.PI/180.0, 2));

        UKF ukf = new UKF(x0, P0, Q, R, dt, 0.3, 2.0, 0.0);

        double vNom = 1.0;
        double wNom = 0.25;

        Random rng = new Random(7);
        double sigV = 0.05;
        double sigW = 0.02;

        for (int k = 0; k < T; k++) {
            // noisy true control
            double vTrue = vNom + sigV * rng.nextGaussian();
            double wTrue = wNom + sigW * rng.nextGaussian();

            SimpleMatrix uNom = new SimpleMatrix(new double[][]{ {vNom},{wNom} });
            SimpleMatrix uTrue = new SimpleMatrix(new double[][]{ {vTrue},{wTrue} });

            xTrue = motionModel(xTrue, uTrue, dt);

            SimpleMatrix z = measurementModel(xTrue, lm);
            z.set(0, z.get(0) + Math.sqrt(R.get(0,0)) * rng.nextGaussian());
            z.set(1, wrapAngle(z.get(1) + Math.sqrt(R.get(1,1)) * rng.nextGaussian()));

            ukf.predict(uNom);
            ukf.update(z, lm);
        }

        System.out.println("Final UKF estimate: " + ukf.x.transpose());
        System.out.println("Final true state:  " + xTrue.transpose());
        System.out.println("Final covariance diag: " + ukf.P.diag().transpose());
    }
}
      

MATLAB provides UKF-related estimation workflows (toolbox-dependent). For example, MathWorks documents the trackingUKF object for UKF-based estimation in tracking contexts. :contentReference[oaicite:6]{index=6}

File: Chapter7_Lesson2.m

%{
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2.m

From-scratch UKF for planar pose localization (x, y, theta) with:
  - motion model: differential-drive kinematics (v, omega)
  - measurement model: range-bearing to a known landmark

This script is self-contained and does not require toolboxes.
(If you have toolboxes, you can compare against built-in UKF objects/blocks;
see Chapter7_Lesson2_Simulink.m for Simulink workflow references.)

%}

clear; clc;

dt = 0.1;
T = 250;
lm = [5.0; 5.0];

wrap = @(a) atan2(sin(a), cos(a));

motion = @(x,u) [x(1) + dt*u(1)*cos(x(3));
                 x(2) + dt*u(1)*sin(x(3));
                 wrap(x(3) + dt*u(2))];

meas = @(x) [sqrt((lm(1)-x(1))^2 + (lm(2)-x(2))^2);
             wrap(atan2(lm(2)-x(2), lm(1)-x(1)) - x(3))];

% UKF parameters
alpha = 0.3; beta = 2.0; kappa = 0.0;

% Initial truth and belief
x_true = [0;0;0];
x = [0.5; -0.3; 0.2];
P = diag([0.5^2, 0.5^2, (15*pi/180)^2]);

Q = diag([0.02^2, 0.02^2, (1*pi/180)^2]);
R = diag([0.10^2, (3*pi/180)^2]);

% Controls
v_nom = 1.0; w_nom = 0.25;
sig_v = 0.05; sig_w = 0.02;

Xtrue = zeros(3,T);
Xest  = zeros(3,T);

for k = 1:T
    % true control with noise
    u_true = [v_nom + sig_v*randn();
              w_nom + sig_w*randn()];
    u_nom = [v_nom; w_nom];

    % truth propagation
    x_true = motion(x_true, u_true);

    % measurement
    z = meas(x_true);
    z = z + [sqrt(R(1,1))*randn(); sqrt(R(2,2))*randn()];
    z(2) = wrap(z(2));

    % UKF predict + update
    [x, P, Xsig_pred, Wm, Wc] = ukf_predict(x, P, u_nom, Q, alpha, beta, kappa, motion, wrap);
    [x, P] = ukf_update(x, P, Xsig_pred, Wm, Wc, z, R, meas, wrap);

    Xtrue(:,k) = x_true;
    Xest(:,k)  = x;
end

rmse = sqrt(mean(sum((Xest(1:2,:)-Xtrue(1:2,:)).^2, 1)));
disp(['RMSE position (UKF): ', num2str(rmse)]);

figure;
plot(Xtrue(1,:), Xtrue(2,:), 'LineWidth', 1.5); hold on;
plot(Xest(1,:), Xest(2,:), 'LineWidth', 1.5);
plot(lm(1), lm(2), 'x', 'LineWidth', 2);
axis equal; grid on;
legend('true','UKF','landmark');
title('Planar localization (UKF)');


% ---------- Local functions ----------

function [Xsig, Wm, Wc, lambda] = sigma_points(mu, P, alpha, beta, kappa)
    L = numel(mu);
    lambda = alpha^2*(L + kappa) - L;
    c = L + lambda;

    P = 0.5*(P + P'); % symmetrize
    jitter = 1e-10;
    for t = 1:8
        [S,p] = chol(c*(P + jitter*eye(L)), 'lower');
        if p == 0
            break;
        end
        jitter = 10*jitter;
    end
    if p ~= 0
        S = chol(c*(P + 1e-3*eye(L)), 'lower');
    end

    Xsig = zeros(L, 2*L+1);
    Xsig(:,1) = mu;
    for i = 1:L
        Xsig(:,1+i)   = mu + S(:,i);
        Xsig(:,1+L+i) = mu - S(:,i);
    end

    Wm = ones(2*L+1,1) * (1/(2*c));
    Wc = Wm;
    Wm(1) = lambda/c;
    Wc(1) = lambda/c + (1 - alpha^2 + beta);
end

function mu = weighted_mean(X, Wm, angle_idx, wrap)
    mu = X * Wm;
    for j = angle_idx
        ang = X(j,:)';
        s = sum(Wm .* sin(ang));
        c = sum(Wm .* cos(ang));
        mu(j) = atan2(s,c);
    end
end

function P = weighted_cov(X, mu, Wc, angle_idx, wrap)
    L = numel(mu);
    P = zeros(L,L);
    for i = 1:size(X,2)
        d = X(:,i) - mu;
        for j = angle_idx
            d(j) = wrap(d(j));
        end
        P = P + Wc(i) * (d*d');
    end
    P = 0.5*(P + P');
end

function [x, P, Xsig_pred, Wm, Wc] = ukf_predict(x, P, u, Q, alpha, beta, kappa, motion, wrap)
    [Xsig, Wm, Wc, ~] = sigma_points(x, P, alpha, beta, kappa);
    L = numel(x);
    Xsig_pred = zeros(L, size(Xsig,2));
    for i = 1:size(Xsig,2)
        Xsig_pred(:,i) = motion(Xsig(:,i), u);
    end
    x = weighted_mean(Xsig_pred, Wm, 3, wrap);
    P = weighted_cov(Xsig_pred, x, Wc, 3, wrap) + Q;
end

function [x, P] = ukf_update(x, P, Xsig_pred, Wm, Wc, z, R, meas, wrap)
    M = numel(z);
    Nsig = size(Xsig_pred,2);
    Zsig = zeros(M, Nsig);
    for i = 1:Nsig
        Zsig(:,i) = meas(Xsig_pred(:,i));
    end

    zhat = weighted_mean(Zsig, Wm, 2, wrap);
    S = weighted_cov(Zsig, zhat, Wc, 2, wrap) + R;

    Pxz = zeros(numel(x), M);
    for i = 1:Nsig
        dx = Xsig_pred(:,i) - x;
        dx(3) = wrap(dx(3));
        dz = Zsig(:,i) - zhat;
        dz(2) = wrap(dz(2));
        Pxz = Pxz + Wc(i) * (dx*dz');
    end

    K = Pxz / S;
    innov = z - zhat;
    innov(2) = wrap(innov(2));

    x = x + K*innov;
    x(3) = wrap(x(3));
    P = P - K*S*K';
    P = 0.5*(P + P');
end
      

Simulink also supports UKF-based state estimation, including an "Unscented Kalman Filter" block for discrete-time nonlinear systems (availability depends on installed products), and MathWorks provides documentation on online state estimation using EKF/UKF. :contentReference[oaicite:7]{index=7}

File: Chapter7_Lesson2_Simulink.m

%{
Chapter 7 - Kalman-Filter Localization for AMR
Lesson 2 - UKF Localization and When It Helps

File: Chapter7_Lesson2_Simulink.m

This file provides a Simulink-oriented workflow to run UKF state estimation.
Exact block paths depend on installed products.

Two common approaches:
  (A) Use an Unscented Kalman Filter block (if available) and provide f(x,u) and h(x)
  (B) Implement the predict/update in a MATLAB Function block (from-scratch UKF)

This script focuses on (A) as a template and provides (B) as a fallback.

References:
  - Unscented Kalman Filter block documentation (MathWorks)
  - Online state estimation with EKF/UKF in Simulink (MathWorks)

%}

%% A) Block-based UKF (if available)
% You may have a dedicated "Unscented Kalman Filter" block in your installation.
% For example, MathWorks documents an "Unscented Kalman Filter" block for discrete-time nonlinear systems.
% If present, you typically:
%   1) Define state transition function f(x,u)
%   2) Define measurement function h(x)
%   3) Set initial state/covariance and noise covariances Q,R
%   4) Provide input u and measurement z signals

% Create a new model
model = 'Chapter7_Lesson2_UKF_Model';
new_system(model);
open_system(model);

% Add Inport blocks for u (2x1) and z (2x1)
add_block('simulink/Sources/In1', [model '/u']);
set_param([model '/u'], 'PortDimensions', '2');

add_block('simulink/Sources/In1', [model '/z']);
set_param([model '/z'], 'PortDimensions', '2');

% Add Outport block for x_hat (3x1)
add_block('simulink/Sinks/Out1', [model '/xhat']);
set_param([model '/xhat'], 'PortDimensions', '3');

% Add a MATLAB Function block that implements f and h (for feeding the UKF block)
% If you have a dedicated UKF block, you may not need this; some blocks accept function handles.

add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Models_f_h']);

% Set the MATLAB Function code (simple template)
code = [
"function [xnext, zhat] = Models_f_h(x,u,mx,my,dt)" newline ...
"%#codegen" newline ...
"wrap = @(a) atan2(sin(a),cos(a));" newline ...
"% motion model" newline ...
"xnext = zeros(3,1);" newline ...
"xnext(1) = x(1) + dt*u(1)*cos(x(3));" newline ...
"xnext(2) = x(2) + dt*u(1)*sin(x(3));" newline ...
"xnext(3) = wrap(x(3) + dt*u(2));" newline ...
"% measurement model (range-bearing)" newline ...
"dx = mx - x(1);" newline ...
"dy = my - x(2);" newline ...
"r = sqrt(dx*dx + dy*dy);" newline ...
"b = wrap(atan2(dy,dx) - x(3));" newline ...
"zhat = [r; b];" newline ...
"end"
];
set_param([model '/Models_f_h'], 'Script', code);

% NOTE:
% To complete approach (A), insert your installation's UKF block here, connect u/z,
% set its parameters (Q,R,x0,P0), and route its output to xhat.

%% B) Fallback: from-scratch UKF in MATLAB Function block
% If you do not have a UKF block, you can place the entire UKF predict/update
% inside a MATLAB Function block, using persistent variables for x and P.
%
% This approach is conceptually straightforward but requires careful codegen compliance if generating C/C++.
%
% Suggested structure inside MATLAB Function block:
%   persistent x P initialized
%   [x,P] = ukf_step(x,P,u,z,params...)
%   y = x

save_system(model);
disp(['Created Simulink template model: ' model]);
      

File: Chapter7_Lesson2.nb

(* ::Package:: *)

(* Chapter 7 - Kalman-Filter Localization for AMR
   Lesson 2 - UKF Localization and When It Helps

   File: Chapter7_Lesson2.nb

   This notebook (in plain-text Notebook expression form) demonstrates:
     - Scaled sigma points
     - Unscented transform mean/cov
     - A single predict/update step for planar pose localization

   You can open it in Mathematica by importing this .nb file.
*)

Notebook[{
  Cell["Chapter 7 - Lesson 2: UKF Localization (UT + UKF Step)", "Title"],

  Cell["1. Unscented Transform: sigma points and weights", "Section"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      λ = α^2 (L + κ) - L;
      W0m = λ/(L + λ);
      W0c = λ/(L + λ) + (1 - α^2 + β);
      Wi = 1/(2 (L + λ));
    ]
  ], "Input"],

  Cell["2. Implement UT numerically", "Section"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      wrap[a_] := ArcTan[Sin[a], Cos[a]];
      sigmaPoints[μ_, P_, α_, β_, κ_] := Module[{L, λ, c, S, X, Wm, Wc},
        L = Length[μ];
        λ = α^2 (L + κ) - L;
        c = L + λ;
        S = CholeskyDecomposition[c P];
        X = Join[{μ}, Table[μ + S[[All, i]], {i, 1, L}], Table[μ - S[[All, i]], {i, 1, L}]];
        Wm = Join[{λ/c}, ConstantArray[1/(2 c), 2 L]];
        Wc = Join[{λ/c + (1 - α^2 + β)}, ConstantArray[1/(2 c), 2 L]];
        {X, Wm, Wc}
      ];
    ]
  ], "Input"],

  Cell["3. Example: propagate through a nonlinear map", "Section"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      μ = {0.5, -0.3, 0.2};
      P = DiagonalMatrix[{0.5^2, 0.5^2, (15 Degree)^2}];
      {X, Wm, Wc} = sigmaPoints[μ, P, 0.3, 2.0, 0.0];

      (* Nonlinear transform: y = {x + cos(theta), y + sin(theta)} *)
      g[x_] := {x[[1]] + Cos[x[[3]]], x[[2]] + Sin[x[[3]]]};
      Y = g /@ X;
      μy = Total[MapThread[#1 #2 &, {Wm, Y}]];
      Py = Total[
        MapThread[
          #1 Outer[Times, #2 - μy, #2 - μy] &,
          {Wc, Y}
        ]
      ];
      {μy, Py}
    ]
  ], "Input"]
}]
      

Problem 1 (UT weights from moment constraints): Derive UT weights by enforcing \( \sum_i W_i^m=1 \), \( \sum_i W_i^m(\chi_i-\boldsymbol{\mu})=\mathbf{0} \), and \( \sum_i W_i^c(\chi_i-\boldsymbol{\mu})(\chi_i-\boldsymbol{\mu})(\chi_i-\boldsymbol{\mu})^\top = \mathbf{P} \).

Solution:

\[ w_0 + 2Lw = 1,\qquad 2w(L+\lambda)\mathbf{P} = \mathbf{P} \;\Rightarrow\; w = \frac{1}{2(L+\lambda)},\quad w_0 = \frac{\lambda}{L+\lambda}. \]

Problem 2 (Linear-system equivalence): Prove UKF equals KF for linear motion and measurement.

Solution: UT is exact for affine transforms, so mean/cov propagation and cross-covariances match KF exactly; hence \(K\) and update match KF.

Problem 3 (Scalar UT check on quadratic): Let \(X \sim \mathcal{N}(\mu,\sigma^2)\) and \(Y=X^2\). Show UT mean is exact if \(1+\lambda > 0\).

Solution:

\[ \hat{\mu}_Y = W_0^m\mu^2 + W_1^m(\mu+\gamma)^2 + W_2^m(\mu-\gamma)^2 = \mu^2 + \sigma^2 = \mathbb{E}[X^2]. \]

We derived UKF localization by replacing EKF Jacobian linearization with sigma-point moment propagation (UT). We proved UT exactness for affine maps, motivated its second-order accuracy, specialized the UKF equations to planar AMR pose with range-bearing sensing, and addressed angle statistics and numerical robustness. Multi-language implementations (Python/C++/Java/MATLAB/Simulink/Mathematica) are provided as downloadable code.