In the Chapter 7 EKF/UKF pipeline, we model state evolution and sensing as \( \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{w}_k \) and \( \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k \), where \( \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_k) \) is the (discretized) process uncertainty capturing unmodeled effects (slip, bias drift, terrain coupling), and \( \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \) is sensor uncertainty (GPS multipath, wheel encoder quantization, IMU noise, timestamp jitter).

Tuning is the act of choosing \( \mathbf{Q}_k \) and \( \mathbf{R}_k \) so that the filter is simultaneously: (i) accurate (low estimation error), (ii) consistent (reported covariance matches true errors statistically), and (iii) robust (does not diverge under realistic outliers and model mismatch).

flowchart TD
  A["Collect dataset (u,z) + \nground truth if available"] --> B["Choose initial Q and R \n(specs + heuristics)"]
  B --> C["Run EKF/UKF and log innovations"]
  C --> D["Compute NIS/NEES and \nwhiteness metrics"]
  D --> E{"Stats consistent with model?"}
  E -->|No| F["Adjust Q/R (structure + scale), \nhandle outliers/latency"]
  F --> C
  E -->|Yes| G["Validate on new routes \n(slip, stops, turns)"]
  G --> H["Freeze baseline tuning + \ndefine online adaptation rules"]
        

A key AMR-specific point: \( \mathbf{Q} \) is rarely “pure physics noise.” It is usually a proxy for model inadequacy. For example, a unicycle kinematic model is correct only when wheel–ground interaction follows rolling constraints; on slippery floors, the dominant uncertainty is not encoder noise but constraint violation, which should primarily increase the effective process uncertainty in pose.

We specialize to the planar unicycle motion model used throughout Chapter 7: state \( \mathbf{x}_k = [x_k, y_k, \theta_k]^\top \), input \( \mathbf{u}_k = [v_k,\omega_k]^\top \), step \( \Delta t \).

\[ \mathbf{x}_{k+1} = \begin{bmatrix} x_k + v_k \Delta t \cos\theta_k \\ y_k + v_k \Delta t \sin\theta_k \\ \theta_k + \omega_k \Delta t \end{bmatrix} + \mathbf{w}_k . \]

A common (and physically interpretable) construction is to model uncertainty on the inputs: \( \tilde{\mathbf{u}}_k = \mathbf{u}_k + \boldsymbol{\eta}_k \), with \( \boldsymbol{\eta}_k \sim \mathcal{N}(\mathbf{0},\mathbf{M}) \). Then the induced state noise covariance is obtained by first-order propagation through the input Jacobian.

Define the Jacobians (EKF linearization): \( \mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_k,\mathbf{u}_k} \) and \( \mathbf{L}_k = \frac{\partial f}{\partial \boldsymbol{\eta}}\big|_{\hat{\mathbf{x}}_k,\mathbf{u}_k} \). For the unicycle model:

\[ \mathbf{F}_k = \begin{bmatrix} 1 & 0 & -v_k \Delta t \sin\hat{\theta}_k \\ 0 & 1 & \;\;v_k \Delta t \cos\hat{\theta}_k \\ 0 & 0 & 1 \end{bmatrix}, \qquad \mathbf{L}_k = \begin{bmatrix} \Delta t \cos\hat{\theta}_k & 0 \\ \Delta t \sin\hat{\theta}_k & 0 \\ 0 & \Delta t \end{bmatrix}. \]

With \( \mathbf{M} = \mathrm{diag}(\sigma_v^2,\sigma_\omega^2) \), the discrete process covariance is

\[ \mathbf{Q}_k = \mathbf{L}_k \mathbf{M}\mathbf{L}_k^\top . \]

Proposition 1 (First-order input-noise propagation).

If \( \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k+\boldsymbol{\eta}_k) \) and \( \boldsymbol{\eta}_k \) is small, then \( \mathrm{Cov}(\mathbf{x}_{k+1}\mid \mathbf{x}_k) \approx \mathbf{L}_k \mathbf{M}\mathbf{L}_k^\top \).

Proof. First-order Taylor expansion in \( \boldsymbol{\eta}_k \) gives \( f(\mathbf{x}_k,\mathbf{u}_k+\boldsymbol{\eta}_k) \approx f(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{L}_k \boldsymbol{\eta}_k \). Taking covariance conditioned on \( \mathbf{x}_k \) and using \( \mathrm{Cov}(\mathbf{A}\boldsymbol{\eta}) = \mathbf{A}\mathrm{Cov}(\boldsymbol{\eta})\mathbf{A}^\top \) yields \( \mathbf{Q}_k \approx \mathbf{L}_k\mathbf{M}\mathbf{L}_k^\top \). ∎

AMR tuning implication: the structure of \( \mathbf{Q}_k \) already encodes coupling: uncertainty in \( v \) inflates both \( x \) and \( y \), and depends on heading. If you observe yaw drift during long straight lines, increasing only \( \sigma_\omega \) is more defensible than blindly inflating all of \( \mathbf{Q} \).

Continuous-to-discrete note (when modeling acceleration noise): if you use a continuous-time white-noise model \( \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{G}\mathbf{q}(t) \), \( \mathbf{q}(t) \) white with spectral density \( \mathbf{Q}_c \), then the discrete covariance over step \( \Delta t \) is

\[ \mathbf{Q}_d = \int_{0}^{\Delta t} \boldsymbol{\Phi}(\tau)\,\mathbf{G}\mathbf{Q}_c\mathbf{G}^\top \boldsymbol{\Phi}(\tau)^\top\, d\tau, \quad \boldsymbol{\Phi}(\tau)=e^{\mathbf{A}\tau}. \]

This formula is critical when you extend the Chapter 7 state to include velocity or IMU biases (done in Chapter 7 Lesson 5 lab), but you can already use it here to justify why \( \mathbf{Q} \) scales with \( \Delta t \) and sometimes \( \Delta t^2 \) depending on the state definition.

For a measurement model \( \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k \), linearize at \( \hat{\mathbf{x}}_{k}^{-} \): \( \mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_{k}^{-}} \). EKF update uses:

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

For GPS position measurements in planar localization: \( h(\mathbf{x}) = [x,y]^\top \) and

\[ \mathbf{H} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \end{bmatrix}, \qquad \mathbf{R}=\mathrm{diag}(\sigma_{gps}^2,\sigma_{gps}^2). \]

AMR tuning implication: increasing \( \mathbf{R} \) reduces trust in the measurement, shrinking the gain in the measured subspace; increasing \( \mathbf{Q} \) increases uncertainty growth, typically increasing reliance on measurements (larger gain) when they arrive.

Theorem 1 (Posterior covariance monotonicity in \(\mathbf{R}\)).

In the linear-Gaussian Kalman filter with measurement update \( \mathbf{P}^+ = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}^- \), if \( \mathbf{R}_1 \preceq \mathbf{R}_2 \) (positive semidefinite ordering), then the posterior covariance satisfies \( \mathbf{P}^+(\mathbf{R}_1) \preceq \mathbf{P}^+(\mathbf{R}_2) \).

Proof (via information form). The covariance update can be written as \( (\mathbf{P}^+)^{-1} = (\mathbf{P}^-)^{-1} + \mathbf{H}^\top \mathbf{R}^{-1}\mathbf{H} \). If \( \mathbf{R}_1 \preceq \mathbf{R}_2 \), then (for positive definite matrices) inversion reverses the order: \( \mathbf{R}_2^{-1} \preceq \mathbf{R}_1^{-1} \). Hence \( \mathbf{H}^\top\mathbf{R}_2^{-1}\mathbf{H} \preceq \mathbf{H}^\top\mathbf{R}_1^{-1}\mathbf{H} \), which implies \( (\mathbf{P}^+)^{-1}(\mathbf{R}_2) \preceq (\mathbf{P}^+)^{-1}(\mathbf{R}_1) \). Inverting again reverses the order, giving \( \mathbf{P}^+(\mathbf{R}_1) \preceq \mathbf{P}^+(\mathbf{R}_2) \). ∎

This theorem formalizes why underestimating \( \mathbf{R} \) makes the filter overconfident and can produce inconsistent covariance (small \( \mathbf{P} \) while true error is large), especially when measurements contain unmodeled biases or time-correlated errors (common in low-cost GPS).

Define the innovation (a.k.a. residual) \( \boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_k^-) \) and its covariance \( \mathbf{S}_k = \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^\top + \mathbf{R}_k \). Under correct modeling (linearization effects aside), \( \boldsymbol{\nu}_k \) is approximately Gaussian: \( \boldsymbol{\nu}_k \sim \mathcal{N}(\mathbf{0},\mathbf{S}_k) \).

Normalized Innovation Squared (NIS). For measurement dimension \( m \):

\[ \epsilon_k = \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1}\boldsymbol{\nu}_k. \]

Theorem 2 (Chi-square property of NIS).

If \( \boldsymbol{\nu}_k \sim \mathcal{N}(\mathbf{0},\mathbf{S}_k) \) and \( \mathbf{S}_k \succ 0 \), then \( \epsilon_k \sim \chi^2_m \).

Proof. Let \( \mathbf{S}_k = \mathbf{L}\mathbf{L}^\top \) be a Cholesky factorization. Define \( \mathbf{y}=\mathbf{L}^{-1}\boldsymbol{\nu}_k \). Then \( \mathbf{y}\sim \mathcal{N}(\mathbf{0},\mathbf{I}) \), and

\[ \epsilon_k = \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1}\boldsymbol{\nu}_k = \boldsymbol{\nu}_k^\top (\mathbf{L}^{-\top}\mathbf{L}^{-1}) \boldsymbol{\nu}_k = \|\mathbf{L}^{-1}\boldsymbol{\nu}_k\|^2 = \|\mathbf{y}\|^2. \]

The squared norm of an \( m \)-dimensional standard normal vector is \( \chi^2_m \). ∎

Normalized Estimation Error Squared (NEES). When ground truth \( \mathbf{x}_k^\star \) is available (motion capture, high-grade RTK), define the error \( \mathbf{e}_k = \hat{\mathbf{x}}_k - \mathbf{x}_k^\star \), and

\[ \eta_k = \mathbf{e}_k^\top \mathbf{P}_k^{-1}\mathbf{e}_k, \quad \eta_k \sim \chi^2_n \;\; \text{(approximately),} \]

where \( n \) is the state dimension. NEES directly tests whether the covariance matches actual errors; NIS tests measurement consistency without requiring ground truth.

flowchart TD
  A["Run filter and log nu_k, S_k, P_k"] --> B["Compute NIS = nu^T S^-1 nu"]
  B --> C["Compare to chi-square bounds \n(gate + consistency)"]
  C --> D{"Mean NIS too high?"}
  D -->|Yes| E["R too small OR Q too small \n(model overconfident)"]
  D -->|No| F{"Mean NIS too low?"}
  F -->|Yes| G["R too large OR Q too large \n(model underconfident)"]
  F -->|No| H["Check whiteness \n(time correlation) + outliers"]
  E --> I["Adjust R scale/structure; \nalso revisit Q during slip"]
  G --> I
  H --> I
        

Innovation log-likelihood. A statistically grounded tuning objective is to maximize the likelihood of the innovations:

\[ \ell(\theta) = \sum_{k=1}^{N} \log p(\boldsymbol{\nu}_k;\mathbf{0},\mathbf{S}_k(\theta)) = -\tfrac{1}{2}\sum_{k=1}^{N}\Big( \log|\mathbf{S}_k(\theta)| + \boldsymbol{\nu}_k^\top \mathbf{S}_k(\theta)^{-1}\boldsymbol{\nu}_k \Big) + c, \]

where \( \theta \) parameterizes \( \mathbf{Q} \) and \( \mathbf{R} \) (often via a few scalars such as \( \sigma_v,\sigma_\omega,\sigma_{gps} \) plus optional inflation factors). Minimizing the negative log-likelihood is equivalent to pushing NIS values toward their expected distribution while also penalizing overly large covariances through the \( \log|\mathbf{S}_k| \) term.

AMR caution: If innovations are heavy-tailed (outliers), the Gaussian likelihood is brittle. In AMR practice, you often combine NIS-based tuning with robust gating (reject measurements with extreme NIS) and with context-dependent noise (e.g., inflate \( \mathbf{Q} \) when slip is detected).

A reliable tuning process proceeds in two stages:

(A) Choose structure. Decide which states receive process noise and which sensor channels are correlated. For planar pose-only EKF, a common structured choice is:

\[ \mathbf{Q}_k = \alpha_Q \,\mathbf{L}_k \begin{bmatrix} \sigma_v^2 & 0\\ 0 & \sigma_\omega^2 \end{bmatrix}\mathbf{L}_k^\top, \qquad \mathbf{R}_k = \alpha_R \,\mathrm{diag}(\sigma_{z,1}^2,\dots,\sigma_{z,m}^2). \]

(B) Identify scales. Start with sensor datasheets and empirical static tests (robot not moving), then refine using NIS/NEES on representative trajectories (straight segments, turns, stops).

Heuristics aligned with AMR failure modes.

• Wheel slip / skid: primarily increases effective process noise in translation; inflate \( \sigma_v \) (and sometimes \( \sigma_\omega \)) during slip indicators (high motor current, low IMU–encoder agreement, sudden yaw residuals).
• GPS multipath: inflates measurement noise and can introduce bias. Use NIS gating and increase \( \sigma_{gps} \) in urban canyons or near reflective surfaces.
• Latency / time misalignment: appears as correlated innovations. If innovation whiteness fails, do not “fix” it solely by inflating \( \mathbf{R} \); fix timestamps or introduce delay compensation.
• Linearization stress (Lesson 3): if large heading uncertainty exists, under-modeled nonlinearity behaves like extra process noise. Increasing \( \alpha_Q \) can improve stability but may reduce accuracy.

Consistency targets (rules of thumb). For measurement dimension \( m \), the expected NIS is \( \mathbb{E}[\epsilon_k]=m \). Over a window of \( N \) steps, the sample mean should satisfy (approximately)

\[ \frac{1}{N}\sum_{k=1}^{N}\epsilon_k \approx m, \quad \sum_{k=1}^{N}\epsilon_k \sim \chi^2_{mN}. \]

This provides quantitative “go/no-go” tuning checks rather than subjective trial-and-error.

The code below implements an EKF for a unicycle robot with GPS position updates and performs innovation-based tuning by adjusting an \( r\_scale \) inflation factor to push mean NIS toward its target.

Note on display vs downloads: as required by your HTML rules, the listings escape < and > inside <code> blocks. The downloadable files contain the same code with raw characters, ready to run/compile.

Chapter7_Lesson4.py

"""
Chapter7_Lesson4.py
Kalman-Filter Localization for AMR — Lesson 4
Tuning Process/Measurement Noise via Innovation Statistics (NIS) for an EKF.

Dependencies:
  numpy
  scipy (optional, for chi-square thresholds)
"""

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

import numpy as np


@dataclass
class NoiseParams:
    # Control noise std-dev (process noise on controls)
    sigma_v: float       # [m/s]
    sigma_w: float       # [rad/s]
    # GPS measurement noise std-dev
    sigma_gps: float     # [m]
    # Optional scalar inflations (tuning knobs)
    q_scale: float = 1.0
    r_scale: float = 1.0


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


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


def jacobian_F(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Jacobian of f wrt state x."""
    _, _, th = x
    v, _ = u
    F = np.eye(3)
    F[0, 2] = -v * dt * math.sin(th)
    F[1, 2] =  v * dt * math.cos(th)
    return F


def jacobian_L(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Jacobian of f wrt control noise (v,w)."""
    _, _, th = x
    L = np.zeros((3, 2))
    L[0, 0] = dt * math.cos(th)
    L[1, 0] = dt * math.sin(th)
    L[2, 1] = dt
    return L


def make_Q(x: np.ndarray, u: np.ndarray, dt: float, p: NoiseParams) -> np.ndarray:
    """
    Q = L M L^T, where M is covariance of control noise.
    """
    L = jacobian_L(x, u, dt)
    M = np.diag([p.sigma_v**2, p.sigma_w**2])
    return p.q_scale * (L @ M @ L.T)


def gps_h(x: np.ndarray) -> np.ndarray:
    """GPS measures position only."""
    return x[:2].copy()


def jacobian_H_gps() -> np.ndarray:
    """Jacobian of gps_h wrt state."""
    H = np.zeros((2, 3))
    H[0, 0] = 1.0
    H[1, 1] = 1.0
    return H


def make_R_gps(p: NoiseParams) -> np.ndarray:
    return p.r_scale * np.diag([p.sigma_gps**2, p.sigma_gps**2])


def ekf_predict(x: np.ndarray, P: np.ndarray, u: np.ndarray, dt: float, p: NoiseParams) -> Tuple[np.ndarray, np.ndarray]:
    x_pred = unicycle_f(x, u, dt)
    F = jacobian_F(x, u, dt)
    Q = make_Q(x, u, dt, p)
    P_pred = F @ P @ F.T + Q
    # keep symmetry
    P_pred = 0.5 * (P_pred + P_pred.T)
    return x_pred, P_pred


def ekf_update_gps(x_pred: np.ndarray, P_pred: np.ndarray, z: np.ndarray, p: NoiseParams) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    H = jacobian_H_gps()
    R = make_R_gps(p)
    y = z - gps_h(x_pred)                    # innovation
    S = H @ P_pred @ H.T + R                 # innovation covariance
    K = P_pred @ H.T @ np.linalg.inv(S)      # Kalman gain
    x_upd = x_pred + K @ y
    x_upd[2] = wrap_angle(float(x_upd[2]))
    P_upd = (np.eye(3) - K @ H) @ P_pred
    P_upd = 0.5 * (P_upd + P_upd.T)
    return x_upd, P_upd, y, S


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


def nees(x_est: np.ndarray, x_true: np.ndarray, P: np.ndarray) -> float:
    e = x_est - x_true
    e[2] = wrap_angle(float(e[2]))
    return float(e.T @ np.linalg.inv(P) @ e)


def chi2_interval(dof: int, alpha: float = 0.95) -> Tuple[float, float]:
    """
    Two-sided interval for chi-square(dof).
    Requires scipy. If scipy is not available, returns a conservative placeholder.
    """
    try:
        from scipy.stats import chi2
        lo = chi2.ppf((1.0 - alpha) / 2.0, dof)
        hi = chi2.ppf((1.0 + alpha) / 2.0, dof)
        return float(lo), float(hi)
    except Exception:
        # Fallback: very rough (not recommended)
        # For dof=2, 95% interval ~ [0.051, 7.378]
        if dof == 2 and abs(alpha - 0.95) < 1e-9:
            return 0.051, 7.378
        return 0.0, float("inf")


def simulate(T: float = 60.0, dt: float = 0.1, seed: int = 1) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Generate ground-truth trajectory and noisy controls/GPS.
    Returns:
      x_true[k], u_meas[k], z_gps[k] (gps at each step)
    """
    rng = np.random.default_rng(seed)
    N = int(T / dt)

    x_true = np.zeros((N, 3))
    u_true = np.zeros((N, 2))
    z_gps = np.zeros((N, 2))

    # True controls: piecewise
    for k in range(N):
        t = k * dt
        v = 1.0 + 0.2 * math.sin(0.2 * t)
        w = 0.2 * math.sin(0.1 * t)
        u_true[k] = [v, w]
        if k > 0:
            x_true[k] = unicycle_f(x_true[k - 1], u_true[k - 1], dt)

    # Sensor noise (unknown to filter)
    sigma_v_meas = 0.08
    sigma_w_meas = 0.04
    sigma_gps_meas = 0.6

    u_meas = u_true + rng.normal(0.0, [sigma_v_meas, sigma_w_meas], size=(N, 2))
    z_gps = x_true[:, :2] + rng.normal(0.0, sigma_gps_meas, size=(N, 2))
    return x_true, u_meas, z_gps


def run_filter_and_collect_stats(p: NoiseParams, x_true: np.ndarray, u_meas: np.ndarray, z_gps: np.ndarray, dt: float) -> Tuple[np.ndarray, List[float], List[float]]:
    N = x_true.shape[0]
    x = np.array([0.0, 0.0, 0.0])
    P = np.diag([1.0, 1.0, (10.0 * math.pi / 180.0) ** 2])

    nis_list: List[float] = []
    nees_list: List[float] = []
    x_est_hist = np.zeros_like(x_true)

    for k in range(N):
        x_pred, P_pred = ekf_predict(x, P, u_meas[k], dt, p)
        x, P, innov, S = ekf_update_gps(x_pred, P_pred, z_gps[k], p)
        x_est_hist[k] = x
        nis_list.append(nis(innov, S))
        nees_list.append(nees(x, x_true[k], P))

    return x_est_hist, nis_list, nees_list


def tune_R_scale_by_mean_nis(p: NoiseParams, nis_list: List[float], dof: int) -> NoiseParams:
    """
    Heuristic: if mean(NIS) > dof, R is too small (overconfident measurements) -> increase r_scale.
              if mean(NIS) < dof, R is too large -> decrease r_scale.
    """
    nis_mean = float(np.mean(nis_list))
    # multiplicative update (mild)
    gain = nis_mean / float(dof)
    gain = max(0.2, min(5.0, gain))  # clamp for stability
    p2 = NoiseParams(**{**p.__dict__})
    p2.r_scale *= gain
    return p2


def main() -> None:
    dt = 0.1
    x_true, u_meas, z_gps = simulate(T=60.0, dt=dt, seed=2)

    # Initial guesses (deliberately imperfect)
    p = NoiseParams(sigma_v=0.03, sigma_w=0.01, sigma_gps=0.3, q_scale=1.0, r_scale=1.0)

    # Evaluate and tune
    for it in range(4):
        _, nis_list, nees_list = run_filter_and_collect_stats(p, x_true, u_meas, z_gps, dt)

        dof = 2  # GPS measurement dimension
        lo, hi = chi2_interval(dof=dof, alpha=0.95)
        nis_mean = float(np.mean(nis_list))
        frac_in = float(np.mean((np.array(nis_list) >= lo) & (np.array(nis_list) <= hi)))

        print(f"Iter {it}: r_scale={p.r_scale:.3f}, q_scale={p.q_scale:.3f}")
        print(f"  mean NIS={nis_mean:.3f} (target ~ {dof}), 95% interval [{lo:.3f},{hi:.3f}], fraction-in={frac_in:.3f}")
        print(f"  mean NEES={float(np.mean(nees_list)):.3f} (target ~ state_dim=3)")

        p = tune_R_scale_by_mean_nis(p, nis_list, dof=dof)

    print("Final tuned parameters:", p)


if __name__ == "__main__":
    main()
      

Chapter7_Lesson4_Ex1.py

"""
Chapter7_Lesson4_Ex1.py
Exercise: Joint scaling of Q and R using NIS and NEES targets.

Idea:
  - Use mean NIS to tune R scale (measurement confidence).
  - Use mean NEES to tune Q scale (process/model confidence).
"""

from __future__ import annotations
import numpy as np
from dataclasses import dataclass
from typing import List

# Reuse the EKF components by importing from Chapter7_Lesson4.py
# (Keep both files in the same folder.)
import Chapter7_Lesson4 as base


@dataclass
class Targets:
    nis_dof: int = 2
    nees_dof: int = 3


def tune_scales(p: base.NoiseParams, nis_list: List[float], nees_list: List[float], t: Targets) -> base.NoiseParams:
    nis_mean = float(np.mean(nis_list))
    nees_mean = float(np.mean(nees_list))

    # R scale update from NIS
    r_gain = nis_mean / float(t.nis_dof)
    r_gain = max(0.2, min(5.0, r_gain))

    # Q scale update from NEES:
    # If NEES too high (filter overconfident about model), increase Q.
    q_gain = nees_mean / float(t.nees_dof)
    q_gain = max(0.2, min(5.0, q_gain))

    p2 = base.NoiseParams(**{**p.__dict__})
    p2.r_scale *= r_gain
    p2.q_scale *= q_gain
    return p2


def main() -> None:
    dt = 0.1
    x_true, u_meas, z_gps = base.simulate(T=80.0, dt=dt, seed=7)

    # Start with under-estimated noises
    p = base.NoiseParams(sigma_v=0.02, sigma_w=0.008, sigma_gps=0.25, q_scale=0.5, r_scale=0.5)
    tgt = Targets()

    for it in range(6):
        _, nis_list, nees_list = base.run_filter_and_collect_stats(p, x_true, u_meas, z_gps, dt)
        p = tune_scales(p, nis_list, nees_list, tgt)
        print(f"Iter {it}: q_scale={p.q_scale:.3f}, r_scale={p.r_scale:.3f}, mean NIS={np.mean(nis_list):.3f}, mean NEES={np.mean(nees_list):.3f}")

    print("Tuned:", p)


if __name__ == "__main__":
    main()
      

Chapter7_Lesson4.cpp

/* 
Chapter7_Lesson4.cpp
Kalman-Filter Localization for AMR — Lesson 4
EKF tuning via innovation statistics (NIS) for a unicycle + GPS example.

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

Dependencies:
  Eigen (header-only)
*/

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

using Vec2 = Eigen::Vector2d;
using Vec3 = Eigen::Vector3d;
using Mat2 = Eigen::Matrix2d;
using Mat3 = Eigen::Matrix3d;
using Mat32 = Eigen::Matrix<double,3,2>;
using Mat23 = Eigen::Matrix<double,2,3>;

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

struct NoiseParams {
    double sigma_v;     // process noise on v
    double sigma_w;     // process noise on w
    double sigma_gps;   // measurement noise
    double q_scale = 1.0;
    double r_scale = 1.0;
};

static Vec3 f_unicycle(const Vec3& x, const Vec2& u, double dt) {
    const double px = x(0), py = x(1), th = x(2);
    const double v = u(0), w = u(1);
    Vec3 x2;
    x2(0) = px + v*dt*std::cos(th);
    x2(1) = py + v*dt*std::sin(th);
    x2(2) = wrap_angle(th + w*dt);
    return x2;
}

static Mat3 jacobian_F(const Vec3& x, const Vec2& u, double dt) {
    Mat3 F = Mat3::Identity();
    const double th = x(2);
    const double v = u(0);
    F(0,2) = -v*dt*std::sin(th);
    F(1,2) =  v*dt*std::cos(th);
    return F;
}

static Mat32 jacobian_L(const Vec3& x, double dt) {
    Mat32 L; L.setZero();
    const double th = x(2);
    L(0,0) = dt*std::cos(th);
    L(1,0) = dt*std::sin(th);
    L(2,1) = dt;
    return L;
}

static Mat3 make_Q(const Vec3& x, const Vec2& u, double dt, const NoiseParams& p) {
    Mat32 L = jacobian_L(x, dt);
    Eigen::Matrix2d M = Eigen::Matrix2d::Zero();
    M(0,0) = p.sigma_v*p.sigma_v;
    M(1,1) = p.sigma_w*p.sigma_w;
    Mat3 Q = p.q_scale * (L * M * L.transpose());
    (void)u; // u not used here (kept for symmetry with theory)
    return Q;
}

static Vec2 h_gps(const Vec3& x) {
    return Vec2(x(0), x(1));
}

static Mat23 jacobian_H() {
    Mat23 H; H.setZero();
    H(0,0) = 1.0;
    H(1,1) = 1.0;
    return H;
}

static Mat2 make_R(const NoiseParams& p) {
    Mat2 R = Mat2::Zero();
    R(0,0) = p.sigma_gps*p.sigma_gps;
    R(1,1) = p.sigma_gps*p.sigma_gps;
    return p.r_scale * R;
}

static double nis(const Vec2& innov, const Mat2& S) {
    return innov.transpose() * S.inverse() * innov;
}

struct SimData {
    std::vector<Vec3> x_true;
    std::vector<Vec2> u_meas;
    std::vector<Vec2> z_gps;
};

static SimData simulate(double T, double dt, unsigned seed) {
    const int N = static_cast<int>(T/dt);
    SimData d;
    d.x_true.resize(N);
    d.u_meas.resize(N);
    d.z_gps.resize(N);

    std::mt19937 gen(seed);
    std::normal_distribution<double> n01(0.0, 1.0);

    // True controls
    std::vector<Vec2> u_true(N);
    for (int k=0; k<N; ++k) {
        const double t = k*dt;
        const double v = 1.0 + 0.2*std::sin(0.2*t);
        const double w = 0.2*std::sin(0.1*t);
        u_true[k] = Vec2(v,w);
        if (k>0) d.x_true[k] = f_unicycle(d.x_true[k-1], u_true[k-1], dt);
    }

    const double sigma_v_meas = 0.08;
    const double sigma_w_meas = 0.04;
    const double sigma_gps_meas = 0.6;

    for (int k=0; k<N; ++k) {
        d.u_meas = u_true + sigma_v_meas*n01(gen);
        d.u_meas = u_true + sigma_w_meas*n01(gen);
        d.z_gps = d.x_true + sigma_gps_meas*n01(gen);
        d.z_gps = d.x_true + sigma_gps_meas*n01(gen);
    }
    return d;
}

static void run_once(const NoiseParams& p, const SimData& d, double dt,
                     std::vector<double>& nis_list) {
    const int N = static_cast<int>(d.x_true.size());
    Vec3 x(0.0,0.0,0.0);
    Mat3 P = Mat3::Zero();
    P(0,0)=1.0; P(1,1)=1.0; P(2,2)=std::pow(10.0*M_PI/180.0,2);

    Mat23 H = jacobian_H();

    nis_list.clear();
    nis_list.reserve(N);

    for (int k=0; k<N; ++k) {
        // Predict
        Vec3 x_pred = f_unicycle(x, d.u_meas[k], dt);
        Mat3 F = jacobian_F(x, d.u_meas[k], dt);
        Mat3 Q = make_Q(x, d.u_meas[k], dt, p);
        Mat3 P_pred = F*P*F.transpose() + Q;

        // Update (GPS)
        Vec2 y = d.z_gps[k] - h_gps(x_pred);
        Mat2 S = H*P_pred*H.transpose() + make_R(p);
        Eigen::Matrix<double,3,2> K = P_pred*H.transpose()*S.inverse();
        x = x_pred + K*y;
        x(2) = wrap_angle(x(2));
        P = (Mat3::Identity() - K*H)*P_pred;

        nis_list.push_back(nis(y,S));
    }
}

int main() {
    const double dt = 0.1;
    SimData d = simulate(60.0, dt, 2);

    NoiseParams p;
    p.sigma_v = 0.03;
    p.sigma_w = 0.01;
    p.sigma_gps = 0.3;

    std::vector<double> nis_list;

    for (int it=0; it<4; ++it) {
        run_once(p, d, dt, nis_list);
        double mean = 0.0;
        for (double v: nis_list) mean += v;
        mean /= static_cast<double>(nis_list.size());

        std::cout << "Iter " << it << ": r_scale=" << p.r_scale
                  << ", mean NIS=" << mean << " (target ~ 2)" << std::endl;

        // heuristic update
        const double gain = std::min(5.0, std::max(0.2, mean/2.0));
        p.r_scale *= gain;
    }

    std::cout << "Final r_scale=" << p.r_scale << std::endl;
    return 0;
}
      

Chapter7_Lesson4.java

/*
Chapter7_Lesson4.java
Kalman-Filter Localization for AMR — Lesson 4
EKF tuning via innovation statistics (NIS) for unicycle + GPS example.

Dependencies:
  EJML (Efficient Java Matrix Library)

Suggested Gradle dependency:
  implementation 'org.ejml:ejml-simple:0.43'
*/

import org.ejml.simple.SimpleMatrix;

import java.util.Random;

public class Chapter7_Lesson4 {

    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 NoiseParams {
        double sigmaV;
        double sigmaW;
        double sigmaGps;
        double qScale = 1.0;
        double rScale = 1.0;
    }

    // x=[px,py,th], u=[v,w]
    static SimpleMatrix f(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);
        SimpleMatrix x2 = new SimpleMatrix(3,1);
        x2.set(0, px + v*dt*Math.cos(th));
        x2.set(1, py + v*dt*Math.sin(th));
        x2.set(2, wrapAngle(th + w*dt));
        return x2;
    }

    static SimpleMatrix jacobianF(SimpleMatrix x, SimpleMatrix u, double dt) {
        double th = x.get(2);
        double v = u.get(0);
        SimpleMatrix F = SimpleMatrix.identity(3);
        F.set(0,2, -v*dt*Math.sin(th));
        F.set(1,2,  v*dt*Math.cos(th));
        return F;
    }

    static SimpleMatrix jacobianL(SimpleMatrix x, double dt) {
        double th = x.get(2);
        SimpleMatrix L = new SimpleMatrix(3,2);
        L.set(0,0, dt*Math.cos(th));
        L.set(1,0, dt*Math.sin(th));
        L.set(2,1, dt);
        return L;
    }

    static SimpleMatrix makeQ(SimpleMatrix x, SimpleMatrix u, double dt, NoiseParams p) {
        SimpleMatrix L = jacobianL(x, dt);
        SimpleMatrix M = new SimpleMatrix(2,2);
        M.set(0,0, p.sigmaV*p.sigmaV);
        M.set(1,1, p.sigmaW*p.sigmaW);
        // Q = qScale * L M L^T
        return L.mult(M).mult(L.transpose()).scale(p.qScale);
    }

    static SimpleMatrix hGps(SimpleMatrix x) {
        SimpleMatrix z = new SimpleMatrix(2,1);
        z.set(0, x.get(0));
        z.set(1, x.get(1));
        return z;
    }

    static SimpleMatrix jacobianH() {
        SimpleMatrix H = new SimpleMatrix(2,3);
        H.set(0,0, 1.0);
        H.set(1,1, 1.0);
        return H;
    }

    static SimpleMatrix makeR(NoiseParams p) {
        SimpleMatrix R = new SimpleMatrix(2,2);
        R.set(0,0, p.sigmaGps*p.sigmaGps);
        R.set(1,1, p.sigmaGps*p.sigmaGps);
        return R.scale(p.rScale);
    }

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

    static class SimData {
        SimpleMatrix[] xTrue;
        SimpleMatrix[] uMeas;
        SimpleMatrix[] zGps;
    }

    static SimData simulate(double T, double dt, long seed) {
        int N = (int)Math.floor(T/dt);
        SimData d = new SimData();
        d.xTrue = new SimpleMatrix[N];
        d.uMeas = new SimpleMatrix[N];
        d.zGps  = new SimpleMatrix[N];

        for (int k=0; k<N; ++k) {
            d.xTrue[k] = new SimpleMatrix(3,1);
            d.uMeas[k] = new SimpleMatrix(2,1);
            d.zGps[k]  = new SimpleMatrix(2,1);
        }

        SimpleMatrix[] uTrue = new SimpleMatrix[N];
        for (int k=0; k<N; ++k) uTrue[k] = new SimpleMatrix(2,1);

        for (int k=0; k<N; ++k) {
            double t = k*dt;
            double v = 1.0 + 0.2*Math.sin(0.2*t);
            double w = 0.2*Math.sin(0.1*t);
            uTrue[k].set(0,v);
            uTrue[k].set(1,w);
            if (k > 0) d.xTrue[k] = f(d.xTrue[k-1], uTrue[k-1], dt);
        }

        Random rng = new Random(seed);
        double sigmaVMeas = 0.08;
        double sigmaWMeas = 0.04;
        double sigmaGpsMeas = 0.6;

        for (int k=0; k<N; ++k) {
            d.uMeas[k].set(0, uTrue[k].get(0) + sigmaVMeas*rng.nextGaussian());
            d.uMeas[k].set(1, uTrue[k].get(1) + sigmaWMeas*rng.nextGaussian());
            d.zGps[k].set(0, d.xTrue[k].get(0) + sigmaGpsMeas*rng.nextGaussian());
            d.zGps[k].set(1, d.xTrue[k].get(1) + sigmaGpsMeas*rng.nextGaussian());
        }
        return d;
    }

    static double mean(double[] a) {
        double s = 0.0;
        for (double v: a) s += v;
        return s / a.length;
    }

    public static void main(String[] args) {
        double dt = 0.1;
        SimData d = simulate(60.0, dt, 2);

        NoiseParams p = new NoiseParams();
        p.sigmaV = 0.03;
        p.sigmaW = 0.01;
        p.sigmaGps = 0.3;

        SimpleMatrix H = jacobianH();

        for (int it=0; it<4; ++it) {
            SimpleMatrix x = new SimpleMatrix(3,1); // start at 0
            SimpleMatrix P = new SimpleMatrix(3,3);
            P.set(0,0, 1.0);
            P.set(1,1, 1.0);
            double sTh = 10.0*Math.PI/180.0;
            P.set(2,2, sTh*sTh);

            int N = d.xTrue.length;
            double[] nisList = new double[N];

            for (int k=0; k<N; ++k) {
                // Predict
                SimpleMatrix xPred = f(x, d.uMeas[k], dt);
                SimpleMatrix F = jacobianF(x, d.uMeas[k], dt);
                SimpleMatrix Q = makeQ(x, d.uMeas[k], dt, p);
                SimpleMatrix PPred = F.mult(P).mult(F.transpose()).plus(Q);

                // Update
                SimpleMatrix y = d.zGps[k].minus(hGps(xPred));
                SimpleMatrix S = H.mult(PPred).mult(H.transpose()).plus(makeR(p));
                SimpleMatrix K = PPred.mult(H.transpose()).mult(S.invert());
                x = xPred.plus(K.mult(y));
                x.set(2, wrapAngle(x.get(2)));
                P = (SimpleMatrix.identity(3).minus(K.mult(H))).mult(PPred);

                nisList[k] = nis(y, S);
            }

            double nisMean = mean(nisList);
            System.out.println("Iter " + it + ": rScale=" + p.rScale + ", mean NIS=" + nisMean + " (target ~ 2)");
            double gain = Math.min(5.0, Math.max(0.2, nisMean/2.0));
            p.rScale *= gain;
        }

        System.out.println("Final rScale=" + p.rScale);
    }
}
      

Chapter7_Lesson4.m

% Chapter7_Lesson4.m
% Kalman-Filter Localization for AMR — Lesson 4
% EKF tuning via innovation statistics (NIS) for a unicycle + GPS example.
%
% This script is self-contained and uses only base MATLAB.
% Optional: build a simple Simulink shell (requires Simulink) by calling:
%   build_simulink_shell_Chapter7_Lesson4();

function Chapter7_Lesson4()
  dt = 0.1;
  [xTrue, uMeas, zGps] = simulate_data(60.0, dt, 2);

  % Initial guesses (deliberately imperfect)
  p.sigma_v = 0.03;
  p.sigma_w = 0.01;
  p.sigma_gps = 0.3;
  p.q_scale = 1.0;
  p.r_scale = 1.0;

  for it = 1:4
    [nisList, neesList] = run_filter_stats(xTrue, uMeas, zGps, dt, p);

    nisMean = mean(nisList);
    neesMean = mean(neesList);

    fprintf('Iter %d: r_scale=%.3f, q_scale=%.3f\n', it, p.r_scale, p.q_scale);
    fprintf('  mean NIS=%.3f (target ~ 2), mean NEES=%.3f (target ~ 3)\n', nisMean, neesMean);

    % Heuristic update for R using NIS
    gain = min(5.0, max(0.2, nisMean/2.0));
    p.r_scale = p.r_scale * gain;
  end

  disp('Final tuned parameters:');
  disp(p);
end

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

function x2 = f_unicycle(x, u, dt)
  px = x(1); py = x(2); th = x(3);
  v = u(1);  w = u(2);
  x2 = zeros(3,1);
  x2(1) = px + v*dt*cos(th);
  x2(2) = py + v*dt*sin(th);
  x2(3) = wrap_angle(th + w*dt);
end

function F = jacobian_F(x, u, dt)
  th = x(3); v = u(1);
  F = eye(3);
  F(1,3) = -v*dt*sin(th);
  F(2,3) =  v*dt*cos(th);
end

function L = jacobian_L(x, dt)
  th = x(3);
  L = zeros(3,2);
  L(1,1) = dt*cos(th);
  L(2,1) = dt*sin(th);
  L(3,2) = dt;
end

function Q = make_Q(x, u, dt, p)
  L = jacobian_L(x, dt);
  M = diag([p.sigma_v^2, p.sigma_w^2]);
  Q = p.q_scale * (L*M*L');
  %#ok<NASGU>
end

function zhat = h_gps(x)
  zhat = x(1:2);
end

function H = jacobian_H_gps()
  H = zeros(2,3);
  H(1,1) = 1;
  H(2,2) = 1;
end

function R = make_R_gps(p)
  R = p.r_scale * diag([p.sigma_gps^2, p.sigma_gps^2]);
end

function [nisList, neesList] = run_filter_stats(xTrue, uMeas, zGps, dt, p)
  N = size(xTrue,1);
  x = [0;0;0];
  P = diag([1,1,(10*pi/180)^2]);

  H = jacobian_H_gps();
  nisList = zeros(N,1);
  neesList = zeros(N,1);

  for k = 1:N
    % Predict
    xPred = f_unicycle(x, uMeas(k,:)', dt);
    F = jacobian_F(x, uMeas(k,:)', dt);
    Q = make_Q(x, uMeas(k,:)', dt, p);
    PPred = F*P*F' + Q;

    % Update
    y = zGps(k,:)' - h_gps(xPred);
    S = H*PPred*H' + make_R_gps(p);
    K = PPred*H' / S;
    x = xPred + K*y;
    x(3) = wrap_angle(x(3));
    P = (eye(3) - K*H)*PPred;

    nisList(k) = y' / S * y;
    e = x - xTrue(k,:)';
    e(3) = wrap_angle(e(3));
    neesList(k) = e' / P * e;
  end
end

function [xTrue, uMeas, zGps] = simulate_data(T, dt, seed)
  rng(seed);
  N = floor(T/dt);
  xTrue = zeros(N,3);
  uTrue = zeros(N,2);

  for k = 1:N
    t = (k-1)*dt;
    uTrue(k,1) = 1.0 + 0.2*sin(0.2*t);
    uTrue(k,2) = 0.2*sin(0.1*t);
    if k > 1
      xTrue(k,:) = f_unicycle(xTrue(k-1,:)', uTrue(k-1,:)', dt)';
    end
  end

  sigma_v_meas = 0.08;
  sigma_w_meas = 0.04;
  sigma_gps_meas = 0.6;

  uMeas = uTrue + randn(N,2).* [sigma_v_meas, sigma_w_meas];
  zGps  = xTrue(:,1:2) + randn(N,2).* sigma_gps_meas;
end

function build_simulink_shell_Chapter7_Lesson4()
  % Creates a minimal Simulink shell illustrating where EKF prediction/update
  % blocks go. You still need to implement the EKF equations inside a
  % MATLAB Function block or use a suitable toolbox implementation.

  mdl = 'Chapter7_Lesson4_Simulink';
  if bdIsLoaded(mdl), close_system(mdl, 0); end
  new_system(mdl); open_system(mdl);

  add_block('simulink/Sources/From Workspace', [mdl '/uMeas']);
  add_block('simulink/Sources/From Workspace', [mdl '/zGps']);
  add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/EKF_Core']);
  add_block('simulink/Sinks/To Workspace', [mdl '/xEst']);

  set_param([mdl '/uMeas'], 'VariableName', 'uMeas');
  set_param([mdl '/zGps'], 'VariableName', 'zGps');
  set_param([mdl '/xEst'], 'VariableName', 'xEst');

  add_line(mdl, 'uMeas/1', 'EKF_Core/1');
  add_line(mdl, 'zGps/1', 'EKF_Core/2');
  add_line(mdl, 'EKF_Core/1', 'xEst/1');

  set_param(mdl, 'StopTime', '60');
  save_system(mdl);
  disp(['Created Simulink shell: ' mdl '.slx']);
end
      

Chapter7_Lesson4.nb

(* Chapter7_Lesson4.nb
   Wolfram Mathematica Notebook (plain-text form)
   Kalman-Filter Localization for AMR — Lesson 4
   Tuning Q/R using innovation log-likelihood and NIS.
*)

Notebook[{
  Cell["Chapter 7 — Lesson 4: Tuning Process/Measurement Noise for AMR", "Title"],
  Cell["EKF (unicycle + GPS) and innovation-based tuning (NIS).", "Text"],

  Cell[BoxData @ ToBoxes @ HoldForm[
    (* State and models *)
    x[k + 1] == f[x[k], u[k]] + w[k] &&
    z[k] == h[x[k]] + v[k]
  ], "Input"],

  Cell["Discrete-time EKF linearization:", "Text"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    Pminus[k + 1] == F[k] . P[k] . Transpose[F[k]] + Q[k] &&
    S[k] == H[k] . Pminus[k] . Transpose[H[k]] + R[k]
  ], "Input"],

  Cell["Innovation negative log-likelihood (Gaussian):", "Text"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    L == Sum[ Log[Det[S[k]]] + Transpose[nu[k]] . Inverse[S[k]] . nu[k], {k, 1, N}]
  ], "Input"],

  Cell["NIS definition and chi-square property:", "Text"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    NIS[k] == Transpose[nu[k]] . Inverse[S[k]] . nu[k] &&
    NIS[k] \[Distributed] ChiSquareDistribution[m]
  ], "Input"]
}]
      

Robotics libraries to know (context): Python: numpy, scipy, filterpy, ROS2 rclpy; C++: Eigen, ROS2 rclcpp, robot_localization (EKF/UKF nodes); Java: EJML (or Apache Commons Math); MATLAB: Navigation Toolbox (fusionEKF) if available; Simulink for real-time deployment.

Problem 1 (Compute \(\mathbf{Q}_k\) explicitly for unicycle).

Using \( \mathbf{L}_k = \begin{bmatrix} \Delta t \cos\hat{\theta}_k & 0 \\ \Delta t \sin\hat{\theta}_k & 0 \\ 0 & \Delta t \end{bmatrix} \) and \( \mathbf{M}=\mathrm{diag}(\sigma_v^2,\sigma_\omega^2) \), derive a closed form for \( \mathbf{Q}_k=\mathbf{L}_k\mathbf{M}\mathbf{L}_k^\top \).

Solution.

\[ \mathbf{Q}_k = \sigma_v^2 \begin{bmatrix} \Delta t^2\cos^2\hat{\theta}_k & \Delta t^2\sin\hat{\theta}_k\cos\hat{\theta}_k & 0\\ \Delta t^2\sin\hat{\theta}_k\cos\hat{\theta}_k & \Delta t^2\sin^2\hat{\theta}_k & 0\\ 0 & 0 & 0 \end{bmatrix} + \sigma_\omega^2 \begin{bmatrix} 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & \Delta t^2 \end{bmatrix}. \]

Hence translational uncertainty is heading-dependent, while yaw uncertainty from \( \sigma_\omega \) is independent of heading in this state choice.

Problem 2 (Posterior covariance increases with \(\mathbf{R}\)).

Prove that for the linear measurement update in information form \( (\mathbf{P}^+)^{-1} = (\mathbf{P}^-)^{-1} + \mathbf{H}^\top\mathbf{R}^{-1}\mathbf{H} \), if \( \mathbf{R}_1 \preceq \mathbf{R}_2 \), then \( \mathbf{P}^+(\mathbf{R}_1) \preceq \mathbf{P}^+(\mathbf{R}_2) \).

Solution.

Since \( \mathbf{R}_1 \preceq \mathbf{R}_2 \) and both are positive definite, inversion reverses order: \( \mathbf{R}_2^{-1} \preceq \mathbf{R}_1^{-1} \). Left/right multiplication by \( \mathbf{H}^\top \) and \( \mathbf{H} \) preserves PSD order, so \( \mathbf{H}^\top\mathbf{R}_2^{-1}\mathbf{H} \preceq \mathbf{H}^\top\mathbf{R}_1^{-1}\mathbf{H} \). Adding \( (\mathbf{P}^-)^{-1} \) preserves order, then inverting reverses again, yielding the claim.

Problem 3 (NIS gating threshold).

For a 2D GPS measurement (so \( m=2 \)), derive the acceptance test \( \epsilon_k \le \gamma \) such that under correct modeling the false-rejection probability is \( 5\% \).

Solution.

From Theorem 2, \( \epsilon_k \sim \chi^2_2 \). Choose \( \gamma = \chi^2_{2,0.95} \) (the 95th percentile). Then \( \Pr(\epsilon_k \le \gamma)=0.95 \) and \( \Pr(\epsilon_k > \gamma)=0.05 \). (Numerically, \( \chi^2_{2,0.95}\approx 5.991 \).)

Problem 4 (Innovation likelihood objective).

Show that maximizing the innovation likelihood over parameters \( \theta \) is equivalent to minimizing \( J(\theta) = \sum_{k=1}^{N}\left(\log|\mathbf{S}_k(\theta)|+\boldsymbol{\nu}_k^\top\mathbf{S}_k(\theta)^{-1}\boldsymbol{\nu}_k\right) \).

Solution.

For Gaussian \( \boldsymbol{\nu}_k \), the log-likelihood is

\[ \ell(\theta)=\sum_{k=1}^{N}\log\left( \frac{1}{(2\pi)^{m/2}|\mathbf{S}_k(\theta)|^{1/2}} \exp\left(-\tfrac{1}{2}\boldsymbol{\nu}_k^\top\mathbf{S}_k(\theta)^{-1}\boldsymbol{\nu}_k\right) \right). \]

Collecting terms yields \( \ell(\theta)= -\tfrac{1}{2}J(\theta) + c \), where \( c \) does not depend on \( \theta \). Thus maximizing \( \ell(\theta) \) is equivalent to minimizing \( J(\theta) \).

Problem 5 (Mean-NIS scale correction).

In a long run with GPS updates (\( m=2 \)), you observe \( \overline{\epsilon}=5.0 \). Using the multiplicative heuristic \( r\_scale \leftarrow r\_scale \cdot \overline{\epsilon}/m \), compute the update factor and interpret it.

Solution.

The factor is \( 5.0/2 = 2.5 \). Since \( \overline{\epsilon} \) is larger than expected, the innovations are too large relative to predicted \( \mathbf{S} \), suggesting measurements were more variable (or the predictor too confident). Increasing \( r\_scale \) by \( 2.5 \) reduces measurement trust and inflates \( \mathbf{S} \), pushing NIS downward toward consistency.

We formalized process/measurement noise tuning for EKF/UKF localization by: (i) deriving structured \( \mathbf{Q} \) from input uncertainty via Jacobians, (ii) proving how increasing \( \mathbf{R} \) increases posterior covariance (and reduces measurement influence), and (iii) using innovation statistics (NIS/NEES) and innovation likelihood to test and tune consistency. These tools provide a defensible path from datasheets and experiments to stable AMR localization settings, especially under slip and intermittent GPS.