In Chapter 6, we introduced a belief as a probability distribution over pose. For a Gaussian filter, the belief at time \( k \) is summarized by a mean \( \hat{\mathbf{x} }_k \) and covariance \( \mathbf{P}_k \). The filter is called consistent if its reported uncertainty matches the true estimation error statistics (under the assumed model class).

Define the true error \( \tilde{\mathbf{x} }_k \) as:

\[ \tilde{\mathbf{x} }_k \;\triangleq\; \mathbf{x}_k - \hat{\mathbf{x} }_k . \]

A common (Gaussian) consistency target is: \( \mathbb{E}[\tilde{\mathbf{x} }_k] = \mathbf{0} \) and \( \mathbb{E}[\tilde{\mathbf{x} }_k\tilde{\mathbf{x} }_k^\top] = \mathbf{P}_k \). In the linear Kalman filter with correct noise models, these equalities hold exactly. In the EKF, they generally do not, because the EKF replaces nonlinear dynamics/measurements by local linear models.

flowchart TD
  A["Nonlinear system: x_{k+1}=f(x_k,u_k)+w_k; z_k=h(x_k)+v_k"] --> B["Filter keeps Gaussian belief (xhat_k, P_k)"]
  B --> C["Linearize around xhat: F_k, H_k"]
  C --> D["EKF predict/update uses first-order model"]
  D --> E["Neglected higher-order terms accumulate"]
  E --> F["Reported P_k becomes too small (overconfident)"]
  F --> G["Consistency tests (NEES/NIS) exceed bounds"]
        

In AMR localization, inconsistency is not a theoretical curiosity: it directly causes aggressive data-fusion (over-trusting a biased estimate), brittle gating, and sudden divergence after a modest disturbance.

Consider the standard discrete-time nonlinear state-space model used in localization:

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{w}_k,\qquad \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \]

with Gaussian noises \( \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_k) \), \( \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \). The EKF linearizes about the current estimate \( \hat{\mathbf{x} }_k \).

2.1 Taylor expansion with remainder (state transition).

Let \( \delta\mathbf{x}_k \triangleq \mathbf{x}_k - \hat{\mathbf{x} }_k \). By Taylor’s theorem:

\[ f(\mathbf{x}_k,\mathbf{u}_k) = f(\hat{\mathbf{x} }_k,\mathbf{u}_k) + \mathbf{F}_k\,\delta\mathbf{x}_k + \mathbf{r}^f_k, \quad \mathbf{F}_k \triangleq \left.\frac{\partial f}{\partial \mathbf{x} }\right|_{\hat{\mathbf{x} }_k,\mathbf{u}_k}, \]

where \( \mathbf{r}^f_k \) collects second and higher order terms. One explicit form (multivariate) is:

\[ \mathbf{r}^f_k = \frac{1}{2} \begin{bmatrix} \delta\mathbf{x}_k^\top \mathbf{H}^{(1)}_k \delta\mathbf{x}_k \\ \vdots \\ \delta\mathbf{x}_k^\top \mathbf{H}^{(n)}_k \delta\mathbf{x}_k \end{bmatrix} + \mathcal{O}(\|\delta\mathbf{x}_k\|^3), \]

where \( \mathbf{H}^{(i)}_k \) is the Hessian of the \( i \)-th component of \( f \) evaluated along a segment between \( \hat{\mathbf{x} }_k \) and \( \mathbf{x}_k \).

The EKF prediction step implicitly assumes \( \mathbf{r}^f_k \approx \mathbf{0} \). This is accurate only if uncertainty is small and nonlinearity is mild over the support of the belief.

2.2 Consequence for the predicted covariance.

The EKF predicted covariance is: \( \mathbf{P}_{k+1|k}^{\text{EKF} } = \mathbf{F}_k \mathbf{P}_{k|k}\mathbf{F}_k^\top + \mathbf{Q}_k \). But the true covariance of the prediction error includes the remainder:

\[ \mathbf{P}_{k+1|k}^{\text{true} } = \mathbb{E}\!\left[ \left(\mathbf{F}_k\delta\mathbf{x}_k + \mathbf{r}^f_k + \mathbf{w}_k\right) \left(\mathbf{F}_k\delta\mathbf{x}_k + \mathbf{r}^f_k + \mathbf{w}_k\right)^\top \right]. \]

Expanding and using independence between \( \delta\mathbf{x}_k \) and \( \mathbf{w}_k \) (standard in filtering assumptions):

\[ \mathbf{P}_{k+1|k}^{\text{true} } = \mathbf{F}_k \mathbf{P}_{k|k}\mathbf{F}_k^\top + \mathbf{Q}_k + \underbrace{\mathbb{E}[\mathbf{r}^f_k (\mathbf{r}^f_k)^\top]}_{\succeq \mathbf{0} } + \underbrace{\mathbf{F}_k \mathbb{E}[\delta\mathbf{x}_k (\mathbf{r}^f_k)^\top] + \mathbb{E}[\mathbf{r}^f_k (\delta\mathbf{x}_k)^\top]\mathbf{F}_k^\top}_{\text{cross terms} } . \]

Key pitfall: the EKF drops all remainder-related terms, so when nonlinearity is significant, \( \mathbf{P}_{k+1|k}^{\text{EKF} } \) becomes systematically too small, i.e., the filter becomes overconfident.

2.3 Linearization of the measurement model.

Similarly:

\[ h(\mathbf{x}_k) = h(\hat{\mathbf{x} }_{k|k-1}) + \mathbf{H}_k \delta\mathbf{x}_{k|k-1} + \mathbf{r}^h_k, \qquad \mathbf{H}_k \triangleq \left.\frac{\partial h}{\partial \mathbf{x} }\right|_{\hat{\mathbf{x} }_{k|k-1} } . \]

The innovation used by EKF is \( \boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x} }_{k|k-1}) \). The covariance \( \mathbf{S}_k \) is computed as:

\[ \mathbf{S}_k^{\text{EKF} } = \mathbf{H}_k \mathbf{P}_{k|k-1}\mathbf{H}_k^\top + \mathbf{R}_k, \]

but the true innovation covariance includes additional remainder-induced terms, which again biases the filter toward overconfidence, especially in strongly nonlinear measurements (e.g., bearing and range near singular geometries).

EKF/UKF consistency is typically evaluated via chi-square hypothesis tests using Monte Carlo simulations (or ground-truth logs). Two standard statistics are: NEES (state error) and NIS (innovation error).

3.1 NEES definition.

For state dimension \( n \), define the normalized estimation error squared:

\[ \epsilon_k \;\triangleq\; \tilde{\mathbf{x} }_k^\top \mathbf{P}_k^{-1}\tilde{\mathbf{x} }_k . \]

Theorem (NEES distribution under Gaussian consistency).

If \( \tilde{\mathbf{x} }_k \sim \mathcal{N}(\mathbf{0},\mathbf{P}_k) \) and \( \mathbf{P}_k \succ \mathbf{0} \), then \( \epsilon_k \sim \chi^2_n \).

Proof. Let \( \mathbf{P}_k = \mathbf{L}\mathbf{L}^\top \) be a Cholesky factorization (exists since \( \mathbf{P}_k \succ \mathbf{0} \)). Define \( \mathbf{y} = \mathbf{L}^{-1}\tilde{\mathbf{x} }_k \). Then: \( \mathbf{y} \sim \mathcal{N}(\mathbf{0},\mathbf{I}) \) and:

\[ \epsilon_k = \tilde{\mathbf{x} }_k^\top (\mathbf{L}\mathbf{L}^\top)^{-1}\tilde{\mathbf{x} }_k = \tilde{\mathbf{x} }_k^\top \mathbf{L}^{-\top}\mathbf{L}^{-1}\tilde{\mathbf{x} }_k = \|\mathbf{L}^{-1}\tilde{\mathbf{x} }_k\|^2 = \|\mathbf{y}\|^2 = \sum_{i=1}^{n} y_i^2. \]

Since the \( y_i \) are i.i.d. standard normal, the sum of squares is chi-square with \( n \) degrees of freedom. □

3.2 NIS definition.

For measurement dimension \( m \), define the normalized innovation squared:

\[ \eta_k \;\triangleq\; \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1}\boldsymbol{\nu}_k, \qquad \boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x} }_{k|k-1}), \qquad \mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1}\mathbf{H}_k^\top + \mathbf{R}_k . \]

Under correct modeling, \( \eta_k \sim \chi^2_m \). In practice, NIS is especially useful in localization because ground-truth state may be unavailable, but innovation statistics are always available.

3.3 Bounds for averaged NEES/NIS (Monte Carlo).

Over \( N \) independent samples, define \( \bar{\epsilon} = \frac{1}{N}\sum_{i=1}^N \epsilon^{(i)} \). If each \( \epsilon^{(i)} \sim \chi^2_n \), then: \( N\bar{\epsilon} \sim \chi^2_{Nn} \). Hence a two-sided \( (1-\alpha) \) acceptance interval is:

\[ \frac{1}{N}\chi^2_{Nn,\;\alpha/2} \;\le\; \bar{\epsilon} \;\le\; \frac{1}{N}\chi^2_{Nn,\;1-\alpha/2}, \]

and similarly for \( \bar{\eta} \) with \( m \). If the empirical average persistently violates the upper bound, the filter is typically overconfident (inconsistent).

4.1 Large heading uncertainty + turning motion.

In a unicycle/differential-drive model, position increments include \( \cos(\theta) \) and \( \sin(\theta) \). When \( \theta \) uncertainty is large, first-order linearization about \( \hat{\theta} \) poorly approximates the distribution of \( \cos(\theta) \), leading to underestimated position covariance.

4.2 Angle wrapping in innovations.

Bearing measurements produce innovations that must be wrapped to \( (-\pi,\pi] \). Failing to do so makes the innovation appear artificially large, causing an incorrect update step and potentially an unstable Jacobian regime.

4.3 Order dependence from relinearization (multi-measurement updates).

For multiple measurements \( \mathbf{z}_k^{(1)}, \mathbf{z}_k^{(2)}, \dots \), EKF updates are not strictly commutative because the Jacobian \( \mathbf{H}_k \) depends on the current linearization point. Sequentially updating and relinearizing after each measurement can yield different results than batching, especially in high curvature measurement functions (range/bearing near landmarks).

4.4 Weak observability and “information injection”.

If the geometry makes certain directions weakly observed (e.g., near-collinear landmark configurations), linearization may artificially produce a numerically informative Jacobian, shrinking covariance in directions where the true nonlinear system carries little information. This is a core mechanism behind EKF inconsistency discussed in observability-based analyses.

4.5 Discretization mismatch (continuous-time noise treated as discrete).

In AMR stacks, IMU/odometry models are often continuous-time while filters run discretely. If the process noise covariance \( \mathbf{Q}_k \) is not discretized consistently with the motion model and sampling interval, the filter can become overconfident even if linearization is accurate. (Noise tuning itself is handled in Lesson 4.)

5.1 Iterated EKF (IEKF): relinearize in the measurement update.

Instead of one linearization at \( \hat{\mathbf{x} }_{k|k-1} \), the IEKF performs a small Gauss–Newton loop on the measurement residual, relinearizing around the current iterate \( \hat{\mathbf{x} }_{k|k}^{(i)} \). This reduces sensitivity to poor linearization points.

5.2 UKF as a non-derivative alternative.

The UKF (Lesson 2) approximates mean/covariance propagation via sigma points, capturing second-order effects more accurately in many AMR nonlinearities. While UKF is not guaranteed consistent under model mismatch, it can reduce linearization-induced overconfidence.

5.3 Conservative covariance handling (when appropriate).

If diagnostics show persistent overconfidence, practical options include covariance inflation \( \mathbf{P} \leftarrow \gamma \mathbf{P} \) with \( \gamma > 1 \), or increasing process noise slightly (formal tuning is Lesson 4). These are engineering mitigations, not substitutes for correct modeling.

5.4 Innovation monitoring and gating using NIS.

Use NIS bounds to detect mismatch or outliers: if \( \eta_k \) frequently exceeds the upper bound, the filter is either inconsistent (too small \( \mathbf{S}_k \)) or the measurement noise/outlier model is wrong.

The following files (also provided as a downloadable ZIP) implement a reproducible Monte Carlo testbed for range–bearing localization with NEES/NIS reporting. The Python version includes both EKF and UKF comparisons. The C++/Java/MATLAB/Mathematica versions emphasize EKF and the statistical tests.

6.1 Python (EKF vs UKF + ANEES/ANIS)

File: Chapter7_Lesson3.py

#!/usr/bin/env python3
"""
Chapter7_Lesson3.py
Consistency and Linearization Pitfalls — EKF vs UKF on a simple AMR localization example.

Dependencies:
  - numpy
  - scipy (for chi-square bounds)

This script:
  1) Simulates a unicycle-like planar robot (x, y, theta) with process noise.
  2) Uses range-bearing measurements to known landmarks.
  3) Runs EKF and UKF.
  4) Reports ANEES and ANIS consistency statistics with 95% bounds.

Author: (course material)
"""
import math
import numpy as np

try:
    from scipy.stats import chi2
except Exception as e:
    chi2 = None


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


def f_motion(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Discrete-time motion model (no noise inside; noise added externally)."""
    px, py, th = x
    v, om = u
    nx = px + v * dt * math.cos(th)
    ny = py + v * dt * math.sin(th)
    nth = wrap_angle(th + om * dt)
    return np.array([nx, ny, nth], dtype=float)


def jacobian_F(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Jacobian of f w.r.t state x at (x,u)."""
    _, _, 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 h_meas(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Range-bearing measurement to a landmark."""
    px, py, th = x
    dx = landmark[0] - px
    dy = landmark[1] - py
    r = math.hypot(dx, dy)
    b = wrap_angle(math.atan2(dy, dx) - th)
    return np.array([r, b], dtype=float)


def jacobian_H(x: np.ndarray, landmark: np.ndarray) -> np.ndarray:
    """Jacobian of h w.r.t state x."""
    px, py, th = x
    dx = landmark[0] - px
    dy = landmark[1] - py
    r2 = dx*dx + dy*dy
    r = math.sqrt(r2) if r2 > 1e-12 else 1e-6
    H = np.zeros((2, 3))
    H[0, 0] = -dx / r
    H[0, 1] = -dy / r
    H[0, 2] = 0.0
    H[1, 0] =  dy / r2
    H[1, 1] = -dx / r2
    H[1, 2] = -1.0
    return H


def ekf_step(x: np.ndarray, P: np.ndarray, u: np.ndarray, z: np.ndarray,
             dt: float, Q: np.ndarray, R: np.ndarray, landmark: np.ndarray):
    """One EKF predict + update."""
    # Predict
    F = jacobian_F(x, u, dt)
    x_pred = f_motion(x, u, dt)
    P_pred = F @ P @ F.T + Q

    # Update
    H = jacobian_H(x_pred, landmark)
    z_pred = h_meas(x_pred, landmark)
    nu = z - z_pred
    nu[1] = wrap_angle(nu[1])  # bearing innovation
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)

    x_upd = x_pred + K @ nu
    x_upd[2] = wrap_angle(x_upd[2])
    I = np.eye(3)
    P_upd = (I - K @ H) @ P_pred @ (I - K @ H).T + K @ R @ K.T  # Joseph form

    return x_upd, P_upd, nu, S


def sigma_points(x: np.ndarray, P: np.ndarray, alpha=1e-3, beta=2.0, kappa=0.0):
    """Scaled unscented sigma points."""
    n = x.size
    lam = alpha*alpha*(n + kappa) - n
    S = np.linalg.cholesky((n + lam) * P)

    X = np.zeros((2*n + 1, n))
    X[0] = x
    for i in range(n):
        X[1 + i]     = x + S[:, i]
        X[1 + n + i] = x - S[:, i]

    Wm = np.full(2*n + 1, 1.0 / (2.0*(n + lam)))
    Wc = np.full(2*n + 1, 1.0 / (2.0*(n + lam)))
    Wm[0] = lam / (n + lam)
    Wc[0] = lam / (n + lam) + (1.0 - alpha*alpha + beta)

    return X, Wm, Wc


def mean_angle(weights: np.ndarray, angles: np.ndarray) -> float:
    s = np.sum(weights * np.sin(angles))
    c = np.sum(weights * np.cos(angles))
    return math.atan2(s, c)


def ukf_step(x: np.ndarray, P: np.ndarray, u: np.ndarray, z: np.ndarray,
             dt: float, Q: np.ndarray, R: np.ndarray, landmark: np.ndarray):
    """One UKF predict + update (state dimension 3, measurement dimension 2)."""
    n = x.size
    m = z.size

    # Predict
    X, Wm, Wc = sigma_points(x, P)
    X_pred = np.zeros_like(X)
    for i in range(X.shape[0]):
        X_pred[i] = f_motion(X[i], u, dt)

    x_pred = np.zeros(n)
    x_pred[0] = np.sum(Wm * X_pred[:, 0])
    x_pred[1] = np.sum(Wm * X_pred[:, 1])
    x_pred[2] = mean_angle(Wm, X_pred[:, 2])

    P_pred = np.zeros((n, n))
    for i in range(X_pred.shape[0]):
        dx = X_pred[i] - x_pred
        dx[2] = wrap_angle(dx[2])
        P_pred += Wc[i] * np.outer(dx, dx)
    P_pred += Q

    # Update
    Zsig = np.zeros((X_pred.shape[0], m))
    for i in range(X_pred.shape[0]):
        Zsig[i] = h_meas(X_pred[i], landmark)

    z_pred = np.zeros(m)
    z_pred[0] = np.sum(Wm * Zsig[:, 0])
    z_pred[1] = mean_angle(Wm, Zsig[:, 1])

    S = np.zeros((m, m))
    Cxz = np.zeros((n, m))
    for i in range(Zsig.shape[0]):
        dz = Zsig[i] - z_pred
        dz[1] = wrap_angle(dz[1])
        dx = X_pred[i] - x_pred
        dx[2] = wrap_angle(dx[2])
        S += Wc[i] * np.outer(dz, dz)
        Cxz += Wc[i] * np.outer(dx, dz)
    S += R

    nu = z - z_pred
    nu[1] = wrap_angle(nu[1])
    K = Cxz @ np.linalg.inv(S)

    x_upd = x_pred + K @ nu
    x_upd[2] = wrap_angle(x_upd[2])
    P_upd = P_pred - K @ S @ K.T

    return x_upd, P_upd, nu, S


def chi2_bounds_for_average(dof: int, N: int, alpha: float = 0.05):
    """Bounds for average of chi-square/dof statistic: (N * avg) ~ chi2(N*dof)."""
    if chi2 is None:
        return None
    lo = chi2.ppf(alpha/2.0, N*dof) / N
    hi = chi2.ppf(1.0 - alpha/2.0, N*dof) / N
    return lo, hi


def run_mc(filter_step, dt: float, steps: int, trials: int, seed: int = 0):
    rng = np.random.default_rng(seed)

    # Landmarks (known map)
    landmarks = np.array([[5.0,  0.0],
                          [0.0,  5.0],
                          [5.0,  5.0]], dtype=float)

    # Noise settings (intentionally a bit "mismatched" to amplify linearization effects)
    Q = np.diag([0.08**2, 0.08**2, (3.0*math.pi/180.0)**2])  # process noise covariance
    R = np.diag([0.15**2, (2.0*math.pi/180.0)**2])          # measurement noise covariance

    nees_list = []
    nis_list = []

    for tr in range(trials):
        # True initial state (unknown to filter)
        x_true = np.array([0.0, 0.0, 0.0], dtype=float)

        # Filter initial belief (purposely slightly biased)
        x_hat = np.array([0.5, -0.4, 10.0*math.pi/180.0], dtype=float)
        P = np.diag([0.8**2, 0.8**2, (15.0*math.pi/180.0)**2])

        for k in range(steps):
            # Control: forward + turning (induces nonlinearity)
            v = 1.0 + 0.2*math.sin(0.05*k)
            om = 0.35 + 0.25*math.sin(0.03*k)

            u = np.array([v, om], dtype=float)

            # Simulate true process with noise
            w = rng.multivariate_normal(np.zeros(3), Q)
            x_true = f_motion(x_true, u, dt) + w
            x_true[2] = wrap_angle(x_true[2])

            # Choose a landmark based on time (switching adds mild nonlinearity)
            lm = landmarks[k % landmarks.shape[0]]

            # Measurement
            z = h_meas(x_true, lm)
            v_meas = rng.multivariate_normal(np.zeros(2), R)
            z = z + v_meas
            z[1] = wrap_angle(z[1])

            # Filter step
            x_hat, P, nu, S = filter_step(x_hat, P, u, z, dt, Q, R, lm)

            # Consistency stats
            e = x_true - x_hat
            e[2] = wrap_angle(e[2])

            nees = float(e.T @ np.linalg.inv(P) @ e)
            nis  = float(nu.T @ np.linalg.inv(S) @ nu)
            nees_list.append(nees)
            nis_list.append(nis)

    nees_arr = np.array(nees_list)
    nis_arr = np.array(nis_list)
    return nees_arr.mean(), nis_arr.mean(), 3, 2, trials*steps


def main():
    # Two dt values: small dt often improves linearization; large dt worsens it
    configs = [
        ("dt=0.05 (mild nonlinearity)", 0.05),
        ("dt=0.20 (stronger nonlinearity)", 0.20),
    ]
    steps = 200
    trials = 40
    alpha = 0.05

    for label, dt in configs:
        print("\n=== ", label, " ===")
        ekf_anees, ekf_anis, n, m, N = run_mc(ekf_step, dt, steps, trials, seed=1)
        ukf_anees, ukf_anis, _, _, _ = run_mc(ukf_step, dt, steps, trials, seed=1)

        print(f"EKF  ANEES={ekf_anees:.3f}   ANIS={ekf_anis:.3f}")
        print(f"UKF  ANEES={ukf_anees:.3f}   ANIS={ukf_anis:.3f}")

        if chi2 is not None:
            nees_lo, nees_hi = chi2_bounds_for_average(n, N, alpha)
            nis_lo, nis_hi = chi2_bounds_for_average(m, N, alpha)
            print(f"95% bounds for ANEES (dof={n}): [{nees_lo:.3f}, {nees_hi:.3f}]")
            print(f"95% bounds for ANIS  (dof={m}): [{nis_lo:.3f}, {nis_hi:.3f}]")
        else:
            print("SciPy not available: chi-square bounds not computed.")

        print("Interpretation: values systematically above bounds indicate overconfidence (inconsistency).")

if __name__ == "__main__":
    main()

6.2 C++ (Eigen + EKF + ANEES/ANIS)

File: Chapter7_Lesson3.cpp

// Chapter7_Lesson3.cpp
// Consistency and Linearization Pitfalls — EKF + NEES/NIS on a 2D AMR localization example.
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter7_Lesson3.cpp -I /usr/include/eigen3 -o lesson7_3
//
// Optional bounds (requires Boost):
//   (most Linux distros already provide boost headers)
//
// This program prints ANEES/ANIS and 95% bounds (if Boost is available).

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

#include <Eigen/Dense>

#ifdef __has_include
#  if __has_include(<boost/math/distributions/chi_squared.hpp>)
#    include <boost/math/distributions/chi_squared.hpp>
#    define HAS_BOOST_CHI2 1
#  endif
#endif

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

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

static Eigen::Matrix3d jacF(const Eigen::Vector3d& x, const Eigen::Vector2d& u, double dt) {
    const double th = x(2);
    const double v  = u(0);
    Eigen::Matrix3d F = Eigen::Matrix3d::Identity();
    F(0,2) = -v * dt * std::sin(th);
    F(1,2) =  v * dt * std::cos(th);
    return F;
}

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

static Eigen::Matrix<double,2,3> jacH(const Eigen::Vector3d& x, const Eigen::Vector2d& lm) {
    const double px = x(0), py = x(1);
    const double dx = lm(0) - px;
    const double dy = lm(1) - py;
    const double r2 = dx*dx + dy*dy;
    const double r  = std::sqrt(std::max(r2, 1e-12));

    Eigen::Matrix<double,2,3> H;
    H.setZero();
    H(0,0) = -dx / r;
    H(0,1) = -dy / r;
    H(1,0) =  dy / r2;
    H(1,1) = -dx / r2;
    H(1,2) = -1.0;
    return H;
}

struct Stats {
    double ane_es = 0.0;
    double an_is  = 0.0;
    int    dof_x  = 3;
    int    dof_z  = 2;
    int    N      = 0;
};

static Stats runMC(double dt, int steps, int trials, unsigned seed) {
    std::mt19937 rng(seed);
    std::normal_distribution<double> stdn(0.0, 1.0);

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

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

    auto sampleGaussian = [&](const Eigen::MatrixXd& Cov) {
        Eigen::LLT<Eigen::MatrixXd> llt(Cov);
        Eigen::MatrixXd L = llt.matrixL();
        Eigen::VectorXd z(Cov.rows());
        for (int i=0;i<z.size();++i) z(i) = stdn(rng);
        return L * z;
    };

    std::vector<Eigen::Vector2d> lms = {
        Eigen::Vector2d(5.0, 0.0),
        Eigen::Vector2d(0.0, 5.0),
        Eigen::Vector2d(5.0, 5.0)
    };

    double nees_sum = 0.0;
    double nis_sum  = 0.0;
    int    count    = 0;

    for (int tr=0; tr<trials; ++tr) {
        Eigen::Vector3d x_true(0.0, 0.0, 0.0);
        Eigen::Vector3d x_hat (0.5, -0.4, 10.0*M_PI/180.0);
        Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
        P(0,0) = 0.8*0.8;
        P(1,1) = 0.8*0.8;
        P(2,2) = std::pow(15.0*M_PI/180.0, 2);

        for (int k=0; k<steps; ++k) {
            const double v  = 1.0 + 0.2*std::sin(0.05*k);
            const double om = 0.35 + 0.25*std::sin(0.03*k);
            Eigen::Vector2d u(v, om);

            // True propagation with noise
            Eigen::Vector3d w = sampleGaussian(Q);
            x_true = fMotion(x_true, u, dt) + w;
            x_true(2) = wrapAngle(x_true(2));

            // Landmark selection
            const Eigen::Vector2d& lm = lms[k % (int)lms.size()];

            // Measurement with noise
            Eigen::Vector2d z = hMeas(x_true, lm) + sampleGaussian(R);
            z(1) = wrapAngle(z(1));

            // EKF predict
            Eigen::Matrix3d F = jacF(x_hat, u, dt);
            Eigen::Vector3d x_pred = fMotion(x_hat, u, dt);
            Eigen::Matrix3d P_pred = F * P * F.transpose() + Q;

            // EKF update
            Eigen::Matrix<double,2,3> H = jacH(x_pred, lm);
            Eigen::Vector2d z_pred = hMeas(x_pred, lm);
            Eigen::Vector2d nu = z - z_pred;
            nu(1) = wrapAngle(nu(1));

            Eigen::Matrix2d S = H * P_pred * H.transpose() + R;
            Eigen::Matrix<double,3,2> K = P_pred * H.transpose() * S.inverse();

            x_hat = x_pred + K * nu;
            x_hat(2) = wrapAngle(x_hat(2));

            Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
            // Joseph form
            P = (I - K*H) * P_pred * (I - K*H).transpose() + K*R*K.transpose();

            // Stats
            Eigen::Vector3d e = x_true - x_hat;
            e(2) = wrapAngle(e(2));

            const double nees = (e.transpose() * P.inverse() * e)(0);
            const double nis  = (nu.transpose() * S.inverse() * nu)(0);

            nees_sum += nees;
            nis_sum  += nis;
            count++;
        }
    }

    Stats st;
    st.ane_es = nees_sum / (double)count;
    st.an_is  = nis_sum  / (double)count;
    st.N      = count;
    return st;
}

#ifdef HAS_BOOST_CHI2
static std::pair<double,double> chi2BoundsAverage(int dof, int N, double alpha=0.05) {
    using boost::math::chi_squared_distribution;
    chi_squared_distribution<double> dist((double)(N*dof));
    const double lo = boost::math::quantile(dist, alpha/2.0) / (double)N;
    const double hi = boost::math::quantile(dist, 1.0 - alpha/2.0) / (double)N;
    return {lo, hi};
}
#endif

int main() {
    const int steps  = 200;
    const int trials = 40;

    for (auto dt : {0.05, 0.20}) {
        std::cout << "\n=== dt=" << dt << " ===\n";
        Stats st = runMC(dt, steps, trials, 1);

        std::cout << "EKF ANEES=" << st.ane_es << "  (expected ~ " << st.dof_x << ")\n";
        std::cout << "EKF ANIS =" << st.an_is  << "  (expected ~ " << st.dof_z << ")\n";

#ifdef HAS_BOOST_CHI2
        auto neesB = chi2BoundsAverage(st.dof_x, st.N, 0.05);
        auto nisB  = chi2BoundsAverage(st.dof_z, st.N, 0.05);
        std::cout << "95% bounds for ANEES: [" << neesB.first << ", " << neesB.second << "]\n";
        std::cout << "95% bounds for ANIS : [" << nisB.first  << ", " << nisB.second  << "]\n";
#else
        std::cout << "(Boost chi-square not detected; bounds not printed.)\n";
#endif
        std::cout << "Interpretation: ANEES or ANIS systematically above bounds indicates overconfidence.\n";
    }
    return 0;
}

6.3 Java (EJML + Commons-Math chi-square bounds)

File: Chapter7_Lesson3.java

// Chapter7_Lesson3.java
// Consistency and Linearization Pitfalls — EKF + NEES/NIS (Java version).
//
// Dependencies (Maven examples):
//   <dependency>
//     <groupId>org.ejml</groupId>
//     <artifactId>ejml-simple</artifactId>
//     <version>0.43.1</version>
//   </dependency>
//   <dependency>
//     <groupId>org.apache.commons</groupId>
//     <artifactId>commons-math3</artifactId>
//     <version>3.6.1</version>
//   </dependency>
//
// Run idea:
//   javac -cp "ejml-simple.jar:commons-math3.jar:." Chapter7_Lesson3.java
//   java  -cp "ejml-simple.jar:commons-math3.jar:." Chapter7_Lesson3

import java.util.Random;
import org.ejml.simple.SimpleMatrix;
import org.apache.commons.math3.distribution.ChiSquaredDistribution;

public class Chapter7_Lesson3 {

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

    static double[] fMotion(double[] x, double[] u, double dt) {
        double px = x[0], py = x[1], th = x[2];
        double v = u[0], om = u[1];
        return new double[] {
            px + v * dt * Math.cos(th),
            py + v * dt * Math.sin(th),
            wrapAngle(th + om * dt)
        };
    }

    static SimpleMatrix jacF(double[] x, double[] u, double dt) {
        double th = x[2];
        double v  = u[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 double[] hMeas(double[] x, double[] lm) {
        double px = x[0], py = x[1], th = x[2];
        double dx = lm[0] - px;
        double dy = lm[1] - py;
        double r  = Math.hypot(dx, dy);
        double b  = wrapAngle(Math.atan2(dy, dx) - th);
        return new double[] {r, b};
    }

    static SimpleMatrix jacH(double[] x, double[] lm) {
        double px = x[0], py = x[1];
        double dx = lm[0] - px;
        double dy = lm[1] - py;
        double r2 = dx*dx + dy*dy;
        double r  = Math.sqrt(Math.max(r2, 1e-12));

        SimpleMatrix H = new SimpleMatrix(2,3);
        H.set(0,0, -dx / r);
        H.set(0,1, -dy / r);
        H.set(1,0,  dy / r2);
        H.set(1,1, -dx / r2);
        H.set(1,2, -1.0);
        return H;
    }

    static double[] sampleGaussian(SimpleMatrix Cov, Random rng) {
        // Cholesky (SimpleMatrix has chol()).
        SimpleMatrix L = Cov.chol().getT(); // lower-triangular
        double[] z = new double[Cov.numRows()];
        for (int i=0; i<z.length; i++) z[i] = rng.nextGaussian();
        SimpleMatrix zs = new SimpleMatrix(z.length, 1, true, z);
        SimpleMatrix s = L.mult(zs);
        double[] out = new double[z.length];
        for (int i=0;i<z.length;i++) out[i] = s.get(i,0);
        return out;
    }

    static class Stats {
        double anees;
        double anis;
        int N;
    }

    static Stats runMC(double dt, int steps, int trials, long seed) {
        Random rng = new Random(seed);

        SimpleMatrix Q = new SimpleMatrix(3,3);
        Q.set(0,0, 0.08*0.08);
        Q.set(1,1, 0.08*0.08);
        Q.set(2,2, Math.pow(3.0*Math.PI/180.0, 2));

        SimpleMatrix R = new SimpleMatrix(2,2);
        R.set(0,0, 0.15*0.15);
        R.set(1,1, Math.pow(2.0*Math.PI/180.0, 2));

        double[][] landmarks = new double[][] {
            {5.0, 0.0},
            {0.0, 5.0},
            {5.0, 5.0}
        };

        double neesSum = 0.0;
        double nisSum  = 0.0;
        int count = 0;

        for (int tr=0; tr<trials; tr++) {
            double[] xTrue = new double[] {0.0, 0.0, 0.0};

            double[] xHat  = new double[] {0.5, -0.4, 10.0*Math.PI/180.0};
            SimpleMatrix P = new SimpleMatrix(3,3);
            P.set(0,0, 0.8*0.8);
            P.set(1,1, 0.8*0.8);
            P.set(2,2, Math.pow(15.0*Math.PI/180.0, 2));

            for (int k=0; k<steps; k++) {
                double v  = 1.0 + 0.2*Math.sin(0.05*k);
                double om = 0.35 + 0.25*Math.sin(0.03*k);
                double[] u = new double[] {v, om};

                // true propagation
                double[] w = sampleGaussian(Q, rng);
                double[] xNext = fMotion(xTrue, u, dt);
                xTrue = new double[] {xNext[0] + w[0], xNext[1] + w[1], wrapAngle(xNext[2] + w[2])};

                double[] lm = landmarks[k % landmarks.length];

                // measurement
                double[] z = hMeas(xTrue, lm);
                double[] vMeas = sampleGaussian(R, rng);
                z = new double[] {z[0] + vMeas[0], wrapAngle(z[1] + vMeas[1])};

                // EKF predict
                SimpleMatrix F = jacF(xHat, u, dt);
                double[] xPredArr = fMotion(xHat, u, dt);
                SimpleMatrix xPred = new SimpleMatrix(3,1,true, xPredArr);
                SimpleMatrix PPred = F.mult(P).mult(F.transpose()).plus(Q);

                // EKF update
                SimpleMatrix H = jacH(xPredArr, lm);
                double[] zPredArr = hMeas(xPredArr, lm);
                SimpleMatrix zPred = new SimpleMatrix(2,1,true, zPredArr);

                SimpleMatrix zMat = new SimpleMatrix(2,1,true, z);
                SimpleMatrix nu = zMat.minus(zPred);
                nu.set(1,0, wrapAngle(nu.get(1,0)));

                SimpleMatrix S = H.mult(PPred).mult(H.transpose()).plus(R);
                SimpleMatrix K = PPred.mult(H.transpose()).mult(S.invert());

                SimpleMatrix xUpd = xPred.plus(K.mult(nu));
                xUpd.set(2,0, wrapAngle(xUpd.get(2,0)));

                SimpleMatrix I = SimpleMatrix.identity(3);
                P = (I.minus(K.mult(H))).mult(PPred).mult((I.minus(K.mult(H))).transpose()).plus(K.mult(R).mult(K.transpose()));

                // store
                xHat = new double[] {xUpd.get(0,0), xUpd.get(1,0), xUpd.get(2,0)};

                // NEES/NIS
                double[] e = new double[] {xTrue[0]-xHat[0], xTrue[1]-xHat[1], wrapAngle(xTrue[2]-xHat[2])};
                SimpleMatrix eMat = new SimpleMatrix(3,1,true,e);

                double nees = eMat.transpose().mult(P.invert()).mult(eMat).get(0,0);
                double nis  = nu.transpose().mult(S.invert()).mult(nu).get(0,0);

                neesSum += nees;
                nisSum  += nis;
                count++;
            }
        }

        Stats st = new Stats();
        st.anees = neesSum / count;
        st.anis  = nisSum  / count;
        st.N = count;
        return st;
    }

    static double[] chi2BoundsAverage(int dof, int N, double alpha) {
        ChiSquaredDistribution dist = new ChiSquaredDistribution(N * dof);
        double lo = dist.inverseCumulativeProbability(alpha/2.0) / N;
        double hi = dist.inverseCumulativeProbability(1.0 - alpha/2.0) / N;
        return new double[] {lo, hi};
    }

    public static void main(String[] args) {
        int steps = 200;
        int trials = 30;
        double alpha = 0.05;

        double[] dts = new double[] {0.05, 0.20};
        for (double dt : dts) {
            System.out.println("\n=== dt=" + dt + " ===");
            Stats st = runMC(dt, steps, trials, 1);

            System.out.printf("EKF ANEES=%.3f (expected ~ 3)%n", st.anees);
            System.out.printf("EKF ANIS =%.3f (expected ~ 2)%n", st.anis);

            double[] bNees = chi2BoundsAverage(3, st.N, alpha);
            double[] bNis  = chi2BoundsAverage(2, st.N, alpha);
            System.out.printf("95%% bounds for ANEES: [%.3f, %.3f]%n", bNees[0], bNees[1]);
            System.out.printf("95%% bounds for ANIS : [%.3f, %.3f]%n", bNis[0], bNis[1]);

            System.out.println("Interpretation: statistics above the upper bound indicate overconfidence (inconsistency).");
        }
    }
}

6.4 MATLAB/Simulink (script + MATLAB Function friendly EKF step)

File: Chapter7_Lesson3.m

% Chapter7_Lesson3.m
% Consistency and Linearization Pitfalls — EKF + NEES/NIS (MATLAB/Simulink-friendly).
%
% This script:
%   1) Simulates a planar robot x=[px; py; theta] with unicycle kinematics.
%   2) Uses range-bearing measurements to known landmarks.
%   3) Runs EKF and reports ANEES and ANIS with chi-square 95% bounds.
%
% Simulink note:
%   - The functions f_motion(), h_meas(), jacF(), jacH(), and ekf_step()
%     can be pasted into MATLAB Function blocks (one for predict, one for update),
%     and driven by Simulink signals (u, z, dt).
%
% Required toolboxes: none (Statistics Toolbox optional for chi2inv).
clear; clc;

rng(1);

% --- configuration ---
steps  = 200;
trials = 40;
alpha  = 0.05;

landmarks = [5 0; 0 5; 5 5];

Q = diag([0.08^2, 0.08^2, (3*pi/180)^2]);
R = diag([0.15^2, (2*pi/180)^2]);

dts = [0.05, 0.20];

for dt = dts
    nees_sum = 0; nis_sum = 0; count = 0;

    for tr = 1:trials
        x_true = [0; 0; 0];

        x_hat  = [0.5; -0.4; 10*pi/180];
        P      = diag([0.8^2, 0.8^2, (15*pi/180)^2]);

        for k = 1:steps
            v  = 1.0 + 0.2*sin(0.05*(k-1));
            om = 0.35 + 0.25*sin(0.03*(k-1));
            u  = [v; om];

            % true propagation with noise
            w = chol(Q,'lower') * randn(3,1);
            x_true = f_motion(x_true, u, dt) + w;
            x_true(3) = wrap_angle(x_true(3));

            lm = landmarks(mod(k-1,size(landmarks,1))+1, :)';

            % measurement
            z = h_meas(x_true, lm) + chol(R,'lower') * randn(2,1);
            z(2) = wrap_angle(z(2));

            % EKF step
            [x_hat, P, nu, S] = ekf_step(x_hat, P, u, z, dt, Q, R, lm);

            % NEES/NIS
            e = x_true - x_hat;
            e(3) = wrap_angle(e(3));

            nees = e'*(P\e);
            nis  = nu'*(S\nu);

            nees_sum = nees_sum + nees;
            nis_sum  = nis_sum  + nis;
            count = count + 1;
        end
    end

    ANEES = nees_sum / count;
    ANIS  = nis_sum  / count;

    fprintf('\n=== dt=%.3f ===\n', dt);
    fprintf('EKF ANEES=%.3f (expected ~ 3)\n', ANEES);
    fprintf('EKF ANIS =%.3f (expected ~ 2)\n', ANIS);

    if exist('chi2inv','file') == 2
        lo_nees = chi2inv(alpha/2, count*3)/count;
        hi_nees = chi2inv(1-alpha/2, count*3)/count;
        lo_nis  = chi2inv(alpha/2, count*2)/count;
        hi_nis  = chi2inv(1-alpha/2, count*2)/count;

        fprintf('95%% bounds for ANEES: [%.3f, %.3f]\n', lo_nees, hi_nees);
        fprintf('95%% bounds for ANIS : [%.3f, %.3f]\n', lo_nis, hi_nis);
    else
        fprintf('chi2inv not available; install Statistics and Machine Learning Toolbox for bounds.\n');
    end

    fprintf('Interpretation: values above upper bounds indicate overconfidence (inconsistency).\n');
end

% ---------------- local functions ----------------

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

function xn = f_motion(x, u, dt)
    px = x(1); py = x(2); th = x(3);
    v = u(1); om = u(2);
    xn = [px + v*dt*cos(th);
          py + v*dt*sin(th);
          wrap_angle(th + om*dt)];
end

function F = jacF(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 z = h_meas(x, lm)
    px = x(1); py = x(2); th = x(3);
    dx = lm(1) - px;
    dy = lm(2) - py;
    r  = hypot(dx, dy);
    b  = wrap_angle(atan2(dy, dx) - th);
    z = [r; b];
end

function H = jacH(x, lm)
    px = x(1); py = x(2);
    dx = lm(1) - px;
    dy = lm(2) - py;
    r2 = dx^2 + dy^2;
    r  = sqrt(max(r2, 1e-12));
    H = zeros(2,3);
    H(1,1) = -dx/r;
    H(1,2) = -dy/r;
    H(2,1) =  dy/r2;
    H(2,2) = -dx/r2;
    H(2,3) = -1;
end

function [x_upd, P_upd, nu, S] = ekf_step(x, P, u, z, dt, Q, R, lm)
    % Predict
    F = jacF(x, u, dt);
    x_pred = f_motion(x, u, dt);
    P_pred = F*P*F' + Q;

    % Update
    H = jacH(x_pred, lm);
    z_pred = h_meas(x_pred, lm);
    nu = z - z_pred;
    nu(2) = wrap_angle(nu(2));

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

    x_upd = x_pred + K*nu;
    x_upd(3) = wrap_angle(x_upd(3));

    I = eye(3);
    P_upd = (I - K*H)*P_pred*(I - K*H)' + K*R*K'; % Joseph form
end

6.5 Wolfram Mathematica (Notebook source)

File: Chapter7_Lesson3.nb

(* Chapter7_Lesson3.nb *)
(* Consistency and Linearization Pitfalls — EKF + NEES/NIS (Wolfram Language / Notebook).
   This notebook-style source is a plain-text Notebook expression so it can be version-controlled. *)

Notebook[{
  Cell["Chapter 7 — Lesson 3: Consistency and Linearization Pitfalls", "Title"],
  Cell["EKF localization in 2D with range-bearing measurements; compute ANEES/ANIS.", "Text"],

  Cell[BoxData@ToBoxes[
    Module[{wrap, fMotion, jacF, hMeas, jacH, ekfStep, Q, R, landmarks,
            steps = 120, trials = 25, dt = 0.2, seed = 1, rng,
            neesSum = 0., nisSum = 0., count = 0},

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

      fMotion[x_, u_, dt_] := Module[{px = x[[1]], py = x[[2]], th = x[[3]], v = u[[1]], om = u[[2]]},
        {px + v dt Cos[th], py + v dt Sin[th], wrap[th + om dt]}
      ];

      jacF[x_, u_, dt_] := Module[{th = x[[3]], v = u[[1]]},
        { {1, 0, -v dt Sin[th]},
         {0, 1,  v dt Cos[th]},
         {0, 0, 1} }
      ];

      hMeas[x_, lm_] := Module[{px = x[[1]], py = x[[2]], th = x[[3]], dx, dy, r, b},
        dx = lm[[1]] - px; dy = lm[[2]] - py;
        r = Sqrt[dx^2 + dy^2];
        b = wrap[ArcTan[dx, dy] - th]; (* ArcTan[x,y] gives atan2(y,x) *)
        {r, b}
      ];

      jacH[x_, lm_] := Module[{px = x[[1]], py = x[[2]], dx, dy, r2, r},
        dx = lm[[1]] - px; dy = lm[[2]] - py;
        r2 = dx^2 + dy^2; r = Sqrt[Max[r2, 10^-12]];
        {
          {-dx/r, -dy/r, 0},
          { dy/r2, -dx/r2, -1}
        }
      ];

      ekfStep[x_, P_, u_, z_, dt_, Q_, R_, lm_] := Module[
        {F, xPred, PPred, H, zPred, nu, S, K, xUpd, PUpd, I = IdentityMatrix[3]},
        F = jacF[x, u, dt];
        xPred = fMotion[x, u, dt];
        PPred = F.P.Transpose[F] + Q;

        H = jacH[xPred, lm];
        zPred = hMeas[xPred, lm];
        nu = z - zPred; nu[[2]] = wrap[nu[[2]]];

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

        xUpd = xPred + K.nu; xUpd[[3]] = wrap[xUpd[[3]]];
        PUpd = (I - K.H).PPred.Transpose[I - K.H] + K.R.Transpose[K];

        {xUpd, PUpd, nu, S}
      ];

      Q = DiagonalMatrix[{0.08^2, 0.08^2, (3 Degree)^2}];
      R = DiagonalMatrix[{0.15^2, (2 Degree)^2}];
      landmarks = { {5., 0.}, {0., 5.}, {5., 5.} };

      rng = SeedRandom[seed];

      Do[
        Module[{xTrue = {0., 0., 0.},
                xHat  = {0.5, -0.4, 10 Degree},
                P = DiagonalMatrix[{0.8^2, 0.8^2, (15 Degree)^2}],
                lm, v, om, u, w, z, vMeas, stepOut, e, nees, nis},

          Do[
            v  = 1.0 + 0.2 Sin[0.05 (k - 1)];
            om = 0.35 + 0.25 Sin[0.03 (k - 1)];
            u = {v, om};

            w = RandomVariate[MultinormalDistribution[{0, 0, 0}, Q]];
            xTrue = fMotion[xTrue, u, dt] + w; xTrue[[3]] = wrap[xTrue[[3]]];

            lm = landmarks[[Mod[k - 1, Length[landmarks]] + 1]];

            vMeas = RandomVariate[MultinormalDistribution[{0, 0}, R]];
            z = hMeas[xTrue, lm] + vMeas; z[[2]] = wrap[z[[2]]];

            stepOut = ekfStep[xHat, P, u, z, dt, Q, R, lm];
            xHat = stepOut[[1]]; P = stepOut[[2]];

            e = xTrue - xHat; e[[3]] = wrap[e[[3]]];
            nees = e . LinearSolve[P, e];
            nis  = stepOut[[3]] . LinearSolve[stepOut[[4]], stepOut[[3]]];

            neesSum += nees; nisSum += nis; count += 1;
          , {k, 1, steps}]
        ]
      , {tr, 1, trials}];

      <|"ANEES" -> N[neesSum/count], "ANIS" -> N[nisSum/count], "ExpectedNEES" -> 3, "ExpectedNIS" -> 2|>
    ]
  ], "Input"],

  Cell["Interpretation: ANEES significantly above 3 indicates overconfidence (inconsistency).", "Text"]
},
WindowSize -> {900, 700},
StyleDefinitions -> "Default.nb"
]

Problem 1 (Remainder term and covariance underestimation).
Consider a scalar nonlinear map \( y = g(x) \) with \( x \sim \mathcal{N}(\mu,\sigma^2) \). The EKF-style approximation linearizes at \( \mu \): \( g(x) \approx g(\mu) + g'(\mu)(x-\mu) \). Show that the true output variance satisfies: \( \mathrm{Var}(y) = g'(\mu)^2\sigma^2 + \Delta \), where \( \Delta \) depends on higher derivatives, and argue why ignoring \( \Delta \) tends to underestimate uncertainty when curvature is significant.

Solution. By Taylor expansion with remainder: \( g(x) = g(\mu) + g'(\mu)(x-\mu) + \frac{1}{2}g''(\xi)(x-\mu)^2 \) for some \( \xi \) between \( x \) and \( \mu \). Define \( r(x) = \frac{1}{2}g''(\xi)(x-\mu)^2 \). Then:

\[ \mathrm{Var}(y) = \mathrm{Var}\!\big(g'(\mu)(x-\mu) + r(x)\big) = g'(\mu)^2\sigma^2 + \mathrm{Var}(r(x)) + 2\,\mathrm{Cov}(g'(\mu)(x-\mu),r(x)). \]

The EKF keeps only the first term \( g'(\mu)^2\sigma^2 \). When \( g'' \) is large over the uncertainty region (high curvature), the neglected terms are not small and the EKF variance is typically optimistic.

Problem 2 (NEES chi-square proof).
Prove that if \( \tilde{\mathbf{x} } \sim \mathcal{N}(\mathbf{0},\mathbf{P}) \) with \( \mathbf{P} \succ \mathbf{0} \), then \( \tilde{\mathbf{x} }^\top \mathbf{P}^{-1}\tilde{\mathbf{x} } \sim \chi^2_n \).

Solution. Use whitening: choose \( \mathbf{P} = \mathbf{L}\mathbf{L}^\top \), define \( \mathbf{y} = \mathbf{L}^{-1}\tilde{\mathbf{x} } \). Then \( \mathbf{y} \sim \mathcal{N}(\mathbf{0},\mathbf{I}) \) and:

\[ \tilde{\mathbf{x} }^\top \mathbf{P}^{-1}\tilde{\mathbf{x} } = \tilde{\mathbf{x} }^\top \mathbf{L}^{-\top}\mathbf{L}^{-1}\tilde{\mathbf{x} } = \|\mathbf{L}^{-1}\tilde{\mathbf{x} }\|^2 = \sum_{i=1}^{n} y_i^2 \sim \chi^2_n. \]

□

Problem 3 (Order dependence of EKF sequential updates).
Consider a scalar state \( x \) with two scalar measurements \( z^{(1)} = h_1(x) + v_1 \) and \( z^{(2)} = h_2(x) + v_2 \). Explain why sequential EKF updates (relinearizing after the first update) can yield a different result than swapping the order, when \( h_1, h_2 \) are nonlinear.

Solution. In the EKF, the measurement Jacobian is evaluated at the current linearization point. If we apply measurement 1 first, we linearize at \( \hat{x}^{(0)} \), obtain an updated estimate \( \hat{x}^{(1)} \), and then linearize measurement 2 at \( \hat{x}^{(1)} \). Swapping the order changes the intermediate estimate, hence changes the Jacobians and gains. Since the EKF update is only first-order correct, these differences do not cancel, so the final result can differ.

Problem 4 (Bearing innovation wrapping).
Let a bearing measurement be \( z_\beta = \beta(x) + v \), with predicted bearing \( \hat{\beta} \). Show that the innovation must be computed with wrapping: \( \nu_\beta = \mathrm{wrap}(z_\beta - \hat{\beta}) \). Give a numerical example where failing to wrap produces an incorrect update direction.

Solution. Bearings are angles on a circle; \( \beta \) and \( \beta + 2\pi \) represent the same direction. Example: \( z_\beta = -179^\circ \) and \( \hat{\beta} = +179^\circ \). The naïve difference is \( -358^\circ \), but the correct wrapped innovation is \( +2^\circ \). Without wrapping, the EKF will apply a massive correction in the wrong direction, often destroying consistency.

Problem 5 (End-to-end consistency-check procedure).
Propose a practical workflow for checking whether an EKF localization configuration is consistent using ANEES and ANIS, including decision thresholds based on chi-square bounds.

Solution. A standard workflow is:

flowchart TD
  S["Collect data: Monte Carlo truth OR logged ground-truth + filter outputs"] --> E1["Compute state errors \ne_k = x_k - xhat_k (wrap angles)"]
  S --> E2["Compute innovations nu_k \nand S_k from filter"]
  E1 --> N1["Compute NEES_k = \ne_k^T P_k^{-1} e_k"]
  E2 --> N2["Compute NIS_k = \nnu_k^T S_k^{-1} nu_k"]
  N1 --> A1["Average across samples: \nANEES"]
  N2 --> A2["Average across samples: \nANIS"]
  A1 --> C1["Chi-square bounds \nfor dof=n (two-sided)"]
  A2 --> C2["Chi-square bounds \nfor dof=m (two-sided)"]
  C1 --> D["Decision: if ANEES above upper bound \n=> overconfident"]
  C2 --> D
        

Concretely, with \( N \) samples, state dimension \( n \), and significance \( \alpha \), accept if: \( \frac{1}{N}\chi^2_{Nn,\alpha/2} \le \text{ANEES} \le \frac{1}{N}\chi^2_{Nn,1-\alpha/2} \), and similarly for ANIS with \( m \). Persistent violations suggest inconsistency or noise mismatch.

We formalized EKF inconsistency as a consequence of neglected higher-order terms in nonlinear propagation and measurement updates. We proved that NEES/NIS follow chi-square laws under Gaussian consistency, enabling objective statistical checks. For AMR localization, the most common practical pitfalls are large heading uncertainty under turning motion, bearing wrapping errors, relinearization order dependence, and weak observability geometries. The provided multi-language implementations demonstrate how to measure inconsistency and compare EKF vs UKF behavior using ANEES/ANIS bounds.