In short indoor runs, odometry and IMU errors may remain tolerable. In field AMR missions (agriculture, inspection, delivery, mining, campus logistics), the robot can travel hundreds of meters or kilometers, and small heading or bias errors integrate into large lateral displacement. The key issue is not only instantaneous accuracy, but drift growth rate when GNSS, RTK, or map matches are degraded.

We model the localization stack as a multi-rate estimator combining relative motion increments (wheel/IMU/visual odometry) with occasional absolute constraints (GNSS/RTK, map anchors, loop closures). The estimator must (i) predict drift during outages, (ii) detect inconsistent measurements, and (iii) re-anchor safely when a reliable absolute observation returns.

flowchart TD
  A["Wheel + IMU + VIO increments"] --> B["Prediction step (state + covariance)"]
  B --> C["Integrity checks (innovation, quality flags)"]
  C --> D["Absolute updates (GNSS/RTK when valid)"]
  C --> E["Anchor updates (map match / loop closure)"]
  D --> F["Bias correction and covariance contraction"]
  E --> F
  F --> G["Pose for navigation + drift health metrics"]
  G --> B
        

The dominant long-range drift mechanisms in outdoor ground robots are:

• Gyro bias: heading bias causes cross-track position drift that grows with traveled distance.
• Wheel slip / terrain interaction: unmodeled slip changes effective motion increments.
• GNSS intermittency: under canopy, buildings, or multipath, absolute updates become infrequent or unreliable.
• Map registration failures: visual/LiDAR relocalization can produce outliers if the scene changed.
• Time synchronization and calibration residuals: small delays become state estimation bias during fast turns.

A useful decomposition is \( \mathbf{e}_p = \mathbf{e}_{\parallel} + \mathbf{e}_{\perp} \), where the along-track error is often dominated by speed/scale error, while the cross-track error is dominated by heading drift.

We use a compact 2D error-aware state (sufficient for the lesson topic): \( \mathbf{x} = [x,\; y,\; \psi,\; b_g]^T \), where \( b_g \) is gyro bias. Let \( v_m \) and \( \omega_m \) be measured forward speed and yaw rate.

\[ \dot x = v_m \cos\psi, \qquad \dot y = v_m \sin\psi, \qquad \dot \psi = \omega_m - b_g, \qquad \dot b_g = n_b \]

Here \( n_b \) is a bias random-walk term. In discrete time with sample period \( \Delta t \):

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k, u_k) + \mathbf{w}_k, \qquad u_k = [v_m,\; \omega_m]^T \]

The first-order linearization around the current estimate gives the state-transition Jacobian \( \mathbf{F}_k = \partial f / \partial \mathbf{x} \):

\[ \mathbf{F}_k = \begin{bmatrix} 1 & 0 & -v_m \Delta t \sin\hat\psi_k & 0 \\ 0 & 1 & v_m \Delta t \cos\hat\psi_k & 0 \\ 0 & 0 & 1 & -\Delta t \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

The covariance propagation step is the standard EKF prediction:

\[ \mathbf{P}_{k+1|k} = \mathbf{F}_k\mathbf{P}_{k|k}\mathbf{F}_k^T + \mathbf{Q}_k \]

In outdoor missions, \( \mathbf{Q}_k \) should be made terrain-dependent (rough/soft/sloped terrain → larger process uncertainty).

Proposition 1 (Heading-error to cross-track drift): Suppose the robot traverses a path segment of arc length \( L \) while the estimated heading has constant error \( \delta\psi \). Then the cross-track displacement satisfies

\[ e_{\perp} = L\sin(\delta\psi), \qquad |e_{\perp}| \le L|\delta\psi| \]

Proof: Resolve the true displacement vector and the estimated displacement vector in a frame aligned with the estimated heading. The normal component difference is exactly \( L\sin(\delta\psi) \). Since \( |\sin z| \le |z| \) for all real \( z \), the bound follows.

This is the main reason small heading bias becomes dangerous in long-range missions: the drift scales approximately linearly with distance traveled.

Proposition 2 (Outage drift with uncertain gyro bias): Consider a straight-motion outage interval of duration \( T \) and constant speed \( v \). Let heading error evolve as \( \delta\psi(t) = \delta\psi_0 - \delta b_g t \). Under the small-angle approximation, the cross-track error at the end of the outage is

\[ e_{\perp}(T) \approx v \int_0^T \delta\psi(t)\,dt = v\left(T\delta\psi_0 - \frac{T^2}{2}\delta b_g\right) \]

If \( \delta\psi_0 \) and \( \delta b_g \) are zero-mean independent random variables with variances \( \sigma_\psi^2 \) and \( \sigma_b^2 \), then

\[ \operatorname{Var}[e_{\perp}(T)] \approx v^2\left(T^2\sigma_\psi^2 + \frac{1}{4}T^4\sigma_b^2\right) \]

The \( T^4 \) term (in variance) shows why bias uncertainty is a severe long-horizon failure mode during GNSS outages.

Absolute updates enter the estimator as measurements \( \mathbf{z}_k = h(\mathbf{x}_k)+\mathbf{v}_k \). For GNSS/RTK position:

\[ \mathbf{z}_k^{\mathrm{gnss} } = \begin{bmatrix} x_k \\ y_k \end{bmatrix} + \mathbf{v}_k, \qquad \mathbf{R}_k^{\mathrm{gnss} } = \begin{bmatrix} \sigma_x^2 & 0 \\ 0 & \sigma_y^2 \end{bmatrix} \]

The EKF innovation and its covariance are

\[ \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^T + \mathbf{R}_k \]

We reject suspicious absolute measurements using the Mahalanobis distance:

\[ d_k^2 = \boldsymbol{\nu}_k^T \mathbf{S}_k^{-1} \boldsymbol{\nu}_k \]

Gate acceptance uses a chi-square threshold \( d_k^2 < \gamma_\alpha \) (with degrees of freedom equal to the measurement dimension). This protects the filter from GNSS multipath spikes or false map matches.

When a reliable map-based relocalization or loop closure is available, we apply an anchor update (position and often heading), which sharply contracts the covariance and corrects accumulated drift.

Chapter 12 introduced pose graphs. For long-range outdoor missions, drift handling is often best implemented in a factor-graph backend while a filter runs online. Let \( \mathbf{x}_0,\dots,\mathbf{x}_N \) be robot poses. A standard objective is

\[ J(\mathbf{X}) = \sum_{k=0}^{N-1} \|\mathbf{r}_k^{\mathrm{odom} }\|_{\mathbf{\Omega}_k}^2 + \sum_{j \in \mathcal{G} } \|\mathbf{r}_j^{\mathrm{gnss} }\|_{\mathbf{\Lambda}_j}^2 + \sum_{\ell \in \mathcal{A} } \rho\!\left(\|\mathbf{r}_\ell^{\mathrm{anchor} }\|_{\mathbf{\Sigma}_\ell}^2\right) \]

where \( \rho(\cdot) \) is a robust loss (e.g., Huber) for anchor/loop-closure outlier suppression.

\[ \rho_{\mathrm{Huber} }(s)= \begin{cases} s, & s \le \delta^2 \\ 2\delta\sqrt{s} - \delta^2, & s > \delta^2 \end{cases} \]

Why this helps drift: odometry factors alone create an estimator with weak long-range observability to global position and heading. Anchor and GNSS factors inject absolute information that remove (or reduce) the low-frequency drift modes.

In practice, many systems use a hybrid design: EKF/ESKF for real-time navigation plus a smoothing backend to feed periodic bias and pose corrections.

Drift handling is not only about adding corrections; it also requires preventing overconfidence. A common consistency metric is the normalized innovation squared (NIS):

\[ \mathrm{NIS}_k = \boldsymbol{\nu}_k^T \mathbf{S}_k^{-1} \boldsymbol{\nu}_k \]

If the filter model is consistent, the expected NIS is near the measurement dimension. Sustained NIS above expectation suggests underestimated process noise \( \mathbf{Q} \), underestimated measurement noise \( \mathbf{R} \), or unmodeled slip/bias changes.

A simple adaptive mechanism is covariance inflation:

\[ \mathbf{P}_{k|k} \leftarrow \alpha\,\mathbf{P}_{k|k}, \qquad \alpha > 1 \]

or terrain-aware process inflation:

\[ \mathbf{Q}_k = \mathbf{Q}_{\mathrm{nom} } + \beta_k\,\mathbf{Q}_{\mathrm{rough} }, \qquad \beta_k \in [0,1] \]

where \( \beta_k \) can come from traversability/terrain estimates (Chapter 18 Lesson 2), IMU vibration metrics, wheel slip detectors, or vision confidence.

A practical field system uses a mode-based logic around the estimator, especially for GNSS dropouts and relocalization opportunities.

flowchart TD
  N["Normal fused localization"] --> Q["Quality degraded?"]
  Q -->|no| N
  Q -->|yes| D["Degraded mode: inflate Q/R, tighten gates"]
  D --> O["GNSS outage persists?"]
  O -->|no| N
  O -->|yes| P["Predictive mode: drift budget + speed limits"]
  P --> A["Reliable anchor found?"]
  A -->|no| P
  A -->|yes| R["Re-anchor update and bias correction"]
  R --> N
        

The drift budget in predictive mode should be explicit (e.g., maximum allowed position covariance trace, maximum heading variance, or predicted cross-track uncertainty ahead of the vehicle).

Proposition 3 (Covariance boundedness with periodic informative anchors): Consider the linearized estimator with bounded process noise \( \mathbf{Q}_k \preceq \bar{\mathbf{Q} } \). Suppose that within every window of length \( M \) steps, the robot receives at least one accepted anchor update with measurement Jacobian \( \mathbf{H} \) that observes position and heading, and with positive definite covariance \( \mathbf{R} \). Then the estimator covariance remains bounded.

Proof sketch: Prediction adds bounded uncertainty each step. An anchor update applies the Riccati contraction

\[ \mathbf{P}^+ = \mathbf{P}^- - \mathbf{P}^-\mathbf{H}^T (\mathbf{H}\mathbf{P}^-\mathbf{H}^T + \mathbf{R})^{-1} \mathbf{H}\mathbf{P}^- \]

which is positive semidefinite subtractive, so \( \mathbf{P}^+ \preceq \mathbf{P}^- \). Over each window, prediction can only increase covariance by a finite amount, while the anchor step removes uncertainty in the drift-observable subspace (position, heading). Therefore covariance cannot diverge in those states if anchors persist.

This proposition formalizes the engineering intuition: long-range drift is manageable if absolute corrections are intermittent but reliable and the filter is not overconfident between them.

The Python implementation below simulates a long mission with rough-terrain segments, GNSS outages, and occasional anchor updates (map/loop-closure analogs). It demonstrates sequential scalar gating for robustness and adaptive covariance inflation from NIS.

Chapter18_Lesson3.py

# Chapter18_Lesson3.py
# Long-Range Localization Drift Handling (2D EKF with drift mitigation)
# State: [x, y, psi, b_g]^T
# - Propagation from wheel speed + gyro
# - GNSS/RTK updates with innovation gating
# - Terrain-aware process noise inflation
# - Anchor (map/loop-closure) re-localization updates
# - Simple integrity monitor and covariance inflation
#
# Requires: numpy

import numpy as np

np.random.seed(18)


def wrap_angle(a: float) -> float:
    return (a + np.pi) % (2.0 * np.pi) - np.pi


def scalar_kf_update(x, P, idx, z, R, angle=False):
    """Kalman update with scalar measurement z = x[idx] + noise."""
    y = z - x[idx]
    if angle:
        y = wrap_angle(y)

    S = P[idx, idx] + R
    K = P[:, idx] / S
    x = x + K * y

    # Joseph-like simplified rank-1 covariance update
    P = P - np.outer(K, P[idx, :])
    P = 0.5 * (P + P.T)  # symmetry repair
    if angle:
        x[2] = wrap_angle(x[2])
    return x, P, float((y * y) / S)  # scalar NIS


def propagate(x, P, v_m, w_m, dt, rough=False):
    # Nominal propagation
    psi = x[2]
    bg = x[3]
    x_next = x.copy()
    x_next[0] += v_m * dt * np.cos(psi)
    x_next[1] += v_m * dt * np.sin(psi)
    x_next[2] = wrap_angle(x_next[2] + (w_m - bg) * dt)
    x_next[3] = x_next[3]

    # Linearized state transition
    F = np.eye(4)
    F[0, 2] = -v_m * dt * np.sin(psi)
    F[1, 2] =  v_m * dt * np.cos(psi)
    F[2, 3] = -dt

    # Process noise (terrain-dependent)
    sigma_v = 0.03 if not rough else 0.12
    sigma_w = 0.01 if not rough else 0.06
    sigma_bg_rw = 0.0008 if not rough else 0.0030

    qx = (sigma_v * dt) ** 2
    qy = (sigma_v * dt) ** 2
    qpsi = (sigma_w * dt) ** 2
    qbg = (sigma_bg_rw ** 2) * dt
    Q = np.diag([qx, qy, qpsi, qbg])

    P_next = F @ P @ F.T + Q
    P_next = 0.5 * (P_next + P_next.T)
    return x_next, P_next


# Mission setup
dt = 0.1
N = 1800
t = np.arange(N) * dt

# Truth state and sensor biases
x_true = np.zeros((N, 4))  # [x, y, psi, bg_true]
bg_true = 0.015  # rad/s gyro bias (unmodeled by pure dead reckoning)

# EKF estimate
x_est = np.array([0.0, 0.0, 0.05, 0.0])
P_est = np.diag([1.0, 1.0, (5*np.pi/180.0)**2, (0.03)**2])

# Storage
est_hist = np.zeros((N, 4))
P_trace = np.zeros(N)
nis_gnss = []
nis_anchor = []

# Outages and anchor events
gnss_outage_1 = (420, 980)
gnss_outage_2 = (1250, 1500)
anchor_times = {1000, 1510, 1710}  # pseudo loop-closure / map-anchor moments

# Simulate + filter
for k in range(1, N):
    # Truth command profile (long-range path with turns)
    tk = t[k]
    v_cmd = 1.5 + 0.3 * np.sin(0.015 * tk)
    w_cmd = 0.08 * np.sin(0.010 * tk) + 0.04 * np.sin(0.040 * tk)

    # "Rough terrain" intervals (slip/vibration increase)
    rough = (300 <= k < 520) or (840 <= k < 1100) or (1400 <= k < 1620)

    # True propagation
    sigma_w_true = 0.008 if not rough else 0.03
    sigma_v_true = 0.02 if not rough else 0.10
    v_true = v_cmd + np.random.randn() * sigma_v_true
    w_true = w_cmd + np.random.randn() * sigma_w_true

    x_true[k, 0] = x_true[k-1, 0] + v_true * dt * np.cos(x_true[k-1, 2])
    x_true[k, 1] = x_true[k-1, 1] + v_true * dt * np.sin(x_true[k-1, 2])
    x_true[k, 2] = wrap_angle(x_true[k-1, 2] + (w_true - bg_true) * dt)
    x_true[k, 3] = bg_true

    # Sensor measurements (wheel speed + gyro)
    v_m = v_true + np.random.randn() * (0.03 if not rough else 0.12)
    w_m = (w_true - bg_true) + bg_true + np.random.randn() * (0.01 if not rough else 0.05)
    # (equivalently w_m = w_true + noise, but model estimates bg)

    # EKF propagation
    x_est, P_est = propagate(x_est, P_est, v_m, w_m, dt, rough=rough)

    # GNSS/RTK update (degrades or disappears)
    in_outage = (gnss_outage_1[0] <= k < gnss_outage_1[1]) or (gnss_outage_2[0] <= k < gnss_outage_2[1])
    if (k % 10 == 0) and (not in_outage):
        # RTK fixed/float mode emulation
        rtk_fixed = not rough and (k % 90 != 0)
        sigma_gps = 0.15 if rtk_fixed else 0.75
        zgx = x_true[k, 0] + np.random.randn() * sigma_gps
        zgy = x_true[k, 1] + np.random.randn() * sigma_gps

        # Sequential scalar gating (x then y); 99% chi-square for 1 dof ~ 6.63
        x_pred = x_est.copy()
        P_pred = P_est.copy()

        # x measurement
        innov_x = zgx - x_pred[0]
        Sx = P_pred[0, 0] + sigma_gps**2
        d2x = innov_x**2 / Sx
        if d2x < 6.63:
            x_est, P_est, nisx = scalar_kf_update(x_est, P_est, 0, zgx, sigma_gps**2, angle=False)
            nis_gnss.append(nisx)

        # y measurement
        innov_y = zgy - x_est[1]
        Sy = P_est[1, 1] + sigma_gps**2
        d2y = innov_y**2 / Sy
        if d2y < 6.63:
            x_est, P_est, nisy = scalar_kf_update(x_est, P_est, 1, zgy, sigma_gps**2, angle=False)
            nis_gnss.append(nisy)

    # Anchor re-localization update (e.g., scan-map/VIO-map loop closure)
    if k in anchor_times:
        z_anchor = np.array([
            x_true[k, 0] + np.random.randn() * 0.10,
            x_true[k, 1] + np.random.randn() * 0.10,
            wrap_angle(x_true[k, 2] + np.random.randn() * (2.0*np.pi/180.0))
        ])

        # Sequential x, y, psi anchor update with tight covariance
        x_est, P_est, n1 = scalar_kf_update(x_est, P_est, 0, z_anchor[0], 0.10**2, angle=False)
        x_est, P_est, n2 = scalar_kf_update(x_est, P_est, 1, z_anchor[1], 0.10**2, angle=False)
        x_est, P_est, n3 = scalar_kf_update(x_est, P_est, 2, z_anchor[2], (2.0*np.pi/180.0)**2, angle=True)
        nis_anchor.extend([n1, n2, n3])

    # Integrity monitor: inflate if innovation statistics remain high
    if len(nis_gnss) >= 20:
        window_mean = float(np.mean(nis_gnss[-20:]))
        if window_mean > 1.8:  # expected mean ~= 1 for scalar NIS
            P_est *= 1.02

    est_hist[k] = x_est
    P_trace[k] = np.trace(P_est)

# Metrics
pos_err = est_hist[:, :2] - x_true[:, :2]
rmse = np.sqrt(np.mean(np.sum(pos_err**2, axis=1)))
final_err = np.linalg.norm(pos_err[-1])
heading_err_deg = np.rad2deg(wrap_angle(est_hist[-1, 2] - x_true[-1, 2]))

print("=== Chapter18_Lesson3.py : Long-Range Localization Drift Handling ===")
print(f"Mission duration: {N*dt:.1f} s")
print(f"Position RMSE: {rmse:.3f} m")
print(f"Final position error: {final_err:.3f} m")
print(f"Final heading error: {heading_err_deg:.3f} deg")
print(f"Mean scalar NIS (GNSS accepted): {np.mean(nis_gnss) if nis_gnss else np.nan:.3f}")
print(f"Mean scalar NIS (Anchor): {np.mean(nis_anchor) if nis_anchor else np.nan:.3f}")
print(f"Final covariance trace: {P_trace[-1]:.5f}")

# A tiny textual drift summary across outage segments
for label, (a, b) in {"Outage1": gnss_outage_1, "Outage2": gnss_outage_2}.items():
    e_start = np.linalg.norm(est_hist[a, :2] - x_true[a, :2])
    e_end = np.linalg.norm(est_hist[b-1, :2] - x_true[b-1, :2])
    print(f"{label}: error at start={e_start:.3f} m, end={e_end:.3f} m, growth={e_end-e_start:.3f} m")
      

This C++ version uses fixed-size arrays and explicit 4x4 matrix operations, which is useful for embedded deployment or educational clarity. In production, Eigen or a similar linear algebra library is recommended for maintainability.

Chapter18_Lesson3.cpp

// Chapter18_Lesson3.cpp
// Long-Range Localization Drift Handling (2D EKF, no external dependencies)
// State: [x, y, psi, b_g]^T
// Compile: g++ -O2 -std=c++17 Chapter18_Lesson3.cpp -o Chapter18_Lesson3

#include <iostream>
#include <array>
#include <vector>
#include <cmath>
#include <random>
#include <iomanip>
#include <numeric>

using Vec4 = std::array<double, 4>;
using Mat4 = std::array<std::array<double, 4>, 4>;

static constexpr double PI = 3.14159265358979323846;

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

Mat4 eye4() { {
    Mat4 I{ {} };
    for (int i = 0; i < 4; ++i) I[i][i] = 1.0;
    return I;
} }

Mat4 matAdd(const Mat4& A, const Mat4& B) { {
    Mat4 C{ {} };
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
            C[i][j] = A[i][j] + B[i][j];
    return C;
} }

Mat4 matSub(const Mat4& A, const Mat4& B) { {
    Mat4 C{ {} };
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
            C[i][j] = A[i][j] - B[i][j];
    return C;
} }

Mat4 matMul(const Mat4& A, const Mat4& B) { {
    Mat4 C{ {} };
    for (int i = 0; i < 4; ++i)
        for (int k = 0; k < 4; ++k)
            for (int j = 0; j < 4; ++j)
                C[i][j] += A[i][k] * B[k][j];
    return C;
} }

Mat4 matT(const Mat4& A) { {
    Mat4 T{ {} };
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
            T[i][j] = A[j][i];
    return T;
} }

void symmetrize(Mat4& P) { {
    for (int i = 0; i < 4; ++i) { {
        for (int j = i + 1; j < 4; ++j) { {
            double s = 0.5 * (P[i][j] + P[j][i]);
            P[i][j] = s;
            P[j][i] = s;
        } }
    } }
} }

double trace4(const Mat4& A) { {
    return A[0][0] + A[1][1] + A[2][2] + A[3][3];
} }

struct FilterState { {
    Vec4 x;
    Mat4 P;
} };

void propagate(FilterState& s, double vm, double wm, double dt, bool rough) { {
    double psi = s.x[2];
    double bg  = s.x[3];

    // Nominal propagation
    s.x[0] += vm * dt * std::cos(psi);
    s.x[1] += vm * dt * std::sin(psi);
    s.x[2] = wrapAngle(s.x[2] + (wm - bg) * dt);

    // Jacobian
    Mat4 F = eye4();
    F[0][2] = -vm * dt * std::sin(psi);
    F[1][2] =  vm * dt * std::cos(psi);
    F[2][3] = -dt;

    // Process noise
    double sigmaV = rough ? 0.12 : 0.03;
    double sigmaW = rough ? 0.06 : 0.01;
    double sigmaBg = rough ? 0.0030 : 0.0008;

    Mat4 Q{ {} };
    Q[0][0] = std::pow(sigmaV * dt, 2);
    Q[1][1] = std::pow(sigmaV * dt, 2);
    Q[2][2] = std::pow(sigmaW * dt, 2);
    Q[3][3] = std::pow(sigmaBg, 2) * dt;

    s.P = matAdd(matMul(matMul(F, s.P), matT(F)), Q);
    symmetrize(s.P);
} }

double scalarUpdate(FilterState& s, int idx, double z, double R, bool angleResidual) { {
    double innov = z - s.x[idx];
    if (angleResidual) innov = wrapAngle(innov);

    double S = s.P[idx][idx] + R;

    // K = P[:,idx] / S
    double K[4];
    for (int i = 0; i < 4; ++i) K[i] = s.P[i][idx] / S;

    // x = x + K * innov
    for (int i = 0; i < 4; ++i) s.x[i] += K[i] * innov;
    s.x[2] = wrapAngle(s.x[2]);

    // P = P - K * P[idx,:]
    Mat4 Pnew = s.P;
    for (int i = 0; i < 4; ++i) { {
        for (int j = 0; j < 4; ++j) { {
            Pnew[i][j] -= K[i] * s.P[idx][j];
        } }
    } }
    s.P = Pnew;
    symmetrize(s.P);

    return (innov * innov) / S; // scalar NIS
} }

int main() { {
    const double dt = 0.1;
    const int N = 1800;
    const double bgTrue = 0.015;

    std::mt19937 rng(18);
    std::normal_distribution<double> n01(0.0, 1.0);

    std::vector<Vec4> xTrue(N, Vec4{ {0.0, 0.0, 0.0, bgTrue} });
    std::vector<Vec4> xEstHist(N, Vec4{ {0.0, 0.0, 0.0, 0.0} });
    std::vector<double> Ptrace(N, 0.0);
    std::vector<double> nisGnss, nisAnchor;

    FilterState f;
    f.x = { {0.0, 0.0, 0.05, 0.0} };
    f.P = { {} };
    f.P[0][0] = 1.0;
    f.P[1][1] = 1.0;
    f.P[2][2] = std::pow(5.0 * PI / 180.0, 2);
    f.P[3][3] = std::pow(0.03, 2);

    auto isRough = [](int k) { {
        return (300 <= k && k < 520) || (840 <= k && k < 1100) || (1400 <= k && k < 1620);
    } };

    auto inOutage = [](int k) { {
        bool o1 = (420 <= k && k < 980);
        bool o2 = (1250 <= k && k < 1500);
        return o1 || o2;
    } };

    for (int k = 1; k < N; ++k) { {
        double tk = k * dt;
        double vCmd = 1.5 + 0.3 * std::sin(0.015 * tk);
        double wCmd = 0.08 * std::sin(0.010 * tk) + 0.04 * std::sin(0.040 * tk);

        bool rough = isRough(k);

        double sigmaWTrue = rough ? 0.03 : 0.008;
        double sigmaVTrue = rough ? 0.10 : 0.02;

        double vTrue = vCmd + sigmaVTrue * n01(rng);
        double wTrue = wCmd + sigmaWTrue * n01(rng);

        xTrue[k][0] = xTrue[k-1][0] + vTrue * dt * std::cos(xTrue[k-1][2]);
        xTrue[k][1] = xTrue[k-1][1] + vTrue * dt * std::sin(xTrue[k-1][2]);
        xTrue[k][2] = wrapAngle(xTrue[k-1][2] + (wTrue - bgTrue) * dt);
        xTrue[k][3] = bgTrue;

        double vm = vTrue + (rough ? 0.12 : 0.03) * n01(rng);
        double wm = wTrue + (rough ? 0.05 : 0.01) * n01(rng);

        propagate(f, vm, wm, dt, rough);

        // GNSS/RTK sequential scalar updates (x, y)
        if ((k % 10 == 0) && !inOutage(k)) { {
            bool rtkFixed = (!rough) && (k % 90 != 0);
            double sigmaGps = rtkFixed ? 0.15 : 0.75;
            double zgx = xTrue[k][0] + sigmaGps * n01(rng);
            double zgy = xTrue[k][1] + sigmaGps * n01(rng);

            double innovX = zgx - f.x[0];
            double Sx = f.P[0][0] + sigmaGps * sigmaGps;
            double d2x = (innovX * innovX) / Sx;
            if (d2x < 6.63) { {
                nisGnss.push_back(scalarUpdate(f, 0, zgx, sigmaGps * sigmaGps, false));
            } }

            double innovY = zgy - f.x[1];
            double Sy = f.P[1][1] + sigmaGps * sigmaGps;
            double d2y = (innovY * innovY) / Sy;
            if (d2y < 6.63) { {
                nisGnss.push_back(scalarUpdate(f, 1, zgy, sigmaGps * sigmaGps, false));
            } }
        } }

        // Anchor re-localization at selected times (x, y, psi)
        if (k == 1000 || k == 1510 || k == 1710) { {
            double zax = xTrue[k][0] + 0.10 * n01(rng);
            double zay = xTrue[k][1] + 0.10 * n01(rng);
            double zap = wrapAngle(xTrue[k][2] + (2.0 * PI / 180.0) * n01(rng));

            nisAnchor.push_back(scalarUpdate(f, 0, zax, 0.10 * 0.10, false));
            nisAnchor.push_back(scalarUpdate(f, 1, zay, 0.10 * 0.10, false));
            nisAnchor.push_back(scalarUpdate(f, 2, zap, std::pow(2.0 * PI / 180.0, 2), true));
        } }

        // Integrity-based covariance inflation
        if (nisGnss.size() >= 20) { {
            double sum = 0.0;
            for (size_t i = nisGnss.size() - 20; i < nisGnss.size(); ++i) sum += nisGnss[i];
            double meanNIS = sum / 20.0;
            if (meanNIS > 1.8) { {
                for (int i = 0; i < 4; ++i)
                    for (int j = 0; j < 4; ++j)
                        f.P[i][j] *= 1.02;
            } }
        } }

        xEstHist[k] = f.x;
        Ptrace[k] = trace4(f.P);
    } }

    // Metrics
    double sumSq = 0.0;
    for (int k = 0; k < N; ++k) { {
        double ex = xEstHist[k][0] - xTrue[k][0];
        double ey = xEstHist[k][1] - xTrue[k][1];
        sumSq += ex * ex + ey * ey;
    } }
    double rmse = std::sqrt(sumSq / N);

    double exf = xEstHist[N-1][0] - xTrue[N-1][0];
    double eyf = xEstHist[N-1][1] - xTrue[N-1][1];
    double finalErr = std::sqrt(exf * exf + eyf * eyf);
    double finalHeadingDeg = wrapAngle(xEstHist[N-1][2] - xTrue[N-1][2]) * 180.0 / PI;

    auto meanVec = [](const std::vector<double>& v) { {
        if (v.empty()) return std::nan("");
        double s = std::accumulate(v.begin(), v.end(), 0.0);
        return s / static_cast<double>(v.size());
    } };

    std::cout << "=== Chapter18_Lesson3.cpp : Long-Range Localization Drift Handling ===\n";
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "Mission duration: " << N * dt << " s\n";
    std::cout << "Position RMSE: " << rmse << " m\n";
    std::cout << "Final position error: " << finalErr << " m\n";
    std::cout << "Final heading error: " << finalHeadingDeg << " deg\n";
    std::cout << "Mean scalar NIS (GNSS accepted): " << meanVec(nisGnss) << "\n";
    std::cout << "Mean scalar NIS (Anchor): " << meanVec(nisAnchor) << "\n";
    std::cout << "Final covariance trace: " << Ptrace[N-1] << "\n";

    auto outageReport = [&](const char* name, int a, int b) { {
        double esx = xEstHist[a][0] - xTrue[a][0];
        double esy = xEstHist[a][1] - xTrue[a][1];
        double eex = xEstHist[b-1][0] - xTrue[b-1][0];
        double eey = xEstHist[b-1][1] - xTrue[b-1][1];
        double estart = std::sqrt(esx * esx + esy * esy);
        double eend = std::sqrt(eex * eex + eey * eey);
        std::cout << name << ": error at start=" << estart
                  << " m, end=" << eend
                  << " m, growth=" << (eend - estart) << " m\n";
    } };
    outageReport("Outage1", 420, 980);
    outageReport("Outage2", 1250, 1500);

    return 0;
} }
      

The Java implementation mirrors the C++ logic and is suitable for simulation tools, Android-based robot software, or backend telemetry validation. The scalar update form avoids heavy matrix dependencies while preserving the estimator structure.

Chapter18_Lesson3.java

// Chapter18_Lesson3.java
// Long-Range Localization Drift Handling (2D EKF, pure Java implementation)
// State: [x, y, psi, b_g]^T
// Compile: javac Chapter18_Lesson3.java
// Run:     java Chapter18_Lesson3

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

public class Chapter18_Lesson3 { {
    static final double PI = Math.PI;

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

    static class FilterState { {
        double[] x = new double[4];
        double[][] P = new double[4][4];
    } }

    static double[][] eye4() { {
        double[][] I = new double[4][4];
        for (int i = 0; i < 4; i++) I[i][i] = 1.0;
        return I;
    } }

    static double[][] matMul(double[][] A, double[][] B) { {
        double[][] C = new double[4][4];
        for (int i = 0; i < 4; i++)
            for (int k = 0; k < 4; k++)
                for (int j = 0; j < 4; j++)
                    C[i][j] += A[i][k] * B[k][j];
        return C;
    } }

    static double[][] matT(double[][] A) { {
        double[][] T = new double[4][4];
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                T[i][j] = A[j][i];
        return T;
    } }

    static void symmetrize(double[][] P) { {
        for (int i = 0; i < 4; i++) { {
            for (int j = i + 1; j < 4; j++) { {
                double s = 0.5 * (P[i][j] + P[j][i]);
                P[i][j] = s;
                P[j][i] = s;
            } }
        } }
    } }

    static void propagate(FilterState s, double vm, double wm, double dt, boolean rough) { {
        double psi = s.x[2];
        double bg = s.x[3];

        s.x[0] += vm * dt * Math.cos(psi);
        s.x[1] += vm * dt * Math.sin(psi);
        s.x[2] = wrapAngle(s.x[2] + (wm - bg) * dt);

        double[][] F = eye4();
        F[0][2] = -vm * dt * Math.sin(psi);
        F[1][2] =  vm * dt * Math.cos(psi);
        F[2][3] = -dt;

        double sigmaV = rough ? 0.12 : 0.03;
        double sigmaW = rough ? 0.06 : 0.01;
        double sigmaBg = rough ? 0.0030 : 0.0008;

        double[][] Q = new double[4][4];
        Q[0][0] = Math.pow(sigmaV * dt, 2);
        Q[1][1] = Math.pow(sigmaV * dt, 2);
        Q[2][2] = Math.pow(sigmaW * dt, 2);
        Q[3][3] = sigmaBg * sigmaBg * dt;

        double[][] tmp = matMul(matMul(F, s.P), matT(F));
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                s.P[i][j] = tmp[i][j] + Q[i][j];
        symmetrize(s.P);
    } }

    static double scalarUpdate(FilterState s, int idx, double z, double R, boolean angleResidual) { {
        double innov = z - s.x[idx];
        if (angleResidual) innov = wrapAngle(innov);

        double S = s.P[idx][idx] + R;
        double[] K = new double[4];
        for (int i = 0; i < 4; i++) K[i] = s.P[i][idx] / S;

        for (int i = 0; i < 4; i++) s.x[i] += K[i] * innov;
        s.x[2] = wrapAngle(s.x[2]);

        double[][] Pnew = new double[4][4];
        for (int i = 0; i < 4; i++) { {
            for (int j = 0; j < 4; j++) { {
                Pnew[i][j] = s.P[i][j] - K[i] * s.P[idx][j];
            } }
        } }
        s.P = Pnew;
        symmetrize(s.P);

        return (innov * innov) / S;
    } }

    static boolean isRough(int k) { {
        return (300 <= k && k < 520) || (840 <= k && k < 1100) || (1400 <= k && k < 1620);
    } }

    static boolean inOutage(int k) { {
        boolean o1 = (420 <= k && k < 980);
        boolean o2 = (1250 <= k && k < 1500);
        return o1 || o2;
    } }

    static double mean(List<Double> v) { {
        if (v.isEmpty()) return Double.NaN;
        double s = 0.0;
        for (double x : v) s += x;
        return s / v.size();
    } }

    static double trace4(double[][] A) { {
        return A[0][0] + A[1][1] + A[2][2] + A[3][3];
    } }

    public static void main(String[] args) { {
        final double dt = 0.1;
        final int N = 1800;
        final double bgTrue = 0.015;

        Random rng = new Random(18);

        double[][] xTrue = new double[N][4];
        double[][] xEstHist = new double[N][4];
        double[] pTrace = new double[N];
        List<Double> nisGnss = new ArrayList<>();
        List<Double> nisAnchor = new ArrayList<>();

        FilterState f = new FilterState();
        f.x[0] = 0.0; f.x[1] = 0.0; f.x[2] = 0.05; f.x[3] = 0.0;
        f.P[0][0] = 1.0;
        f.P[1][1] = 1.0;
        f.P[2][2] = Math.pow(5.0 * PI / 180.0, 2);
        f.P[3][3] = Math.pow(0.03, 2);

        for (int k = 1; k < N; k++) { {
            double tk = k * dt;
            double vCmd = 1.5 + 0.3 * Math.sin(0.015 * tk);
            double wCmd = 0.08 * Math.sin(0.010 * tk) + 0.04 * Math.sin(0.040 * tk);

            boolean rough = isRough(k);
            double sigmaWTrue = rough ? 0.03 : 0.008;
            double sigmaVTrue = rough ? 0.10 : 0.02;

            double vTrue = vCmd + sigmaVTrue * rng.nextGaussian();
            double wTrue = wCmd + sigmaWTrue * rng.nextGaussian();

            xTrue[k][0] = xTrue[k-1][0] + vTrue * dt * Math.cos(xTrue[k-1][2]);
            xTrue[k][1] = xTrue[k-1][1] + vTrue * dt * Math.sin(xTrue[k-1][2]);
            xTrue[k][2] = wrapAngle(xTrue[k-1][2] + (wTrue - bgTrue) * dt);
            xTrue[k][3] = bgTrue;

            double vm = vTrue + (rough ? 0.12 : 0.03) * rng.nextGaussian();
            double wm = wTrue + (rough ? 0.05 : 0.01) * rng.nextGaussian();

            propagate(f, vm, wm, dt, rough);

            if ((k % 10 == 0) && !inOutage(k)) { {
                boolean rtkFixed = (!rough) && (k % 90 != 0);
                double sigmaGps = rtkFixed ? 0.15 : 0.75;

                double zgx = xTrue[k][0] + sigmaGps * rng.nextGaussian();
                double zgy = xTrue[k][1] + sigmaGps * rng.nextGaussian();

                double innovX = zgx - f.x[0];
                double Sx = f.P[0][0] + sigmaGps * sigmaGps;
                double d2x = innovX * innovX / Sx;
                if (d2x < 6.63) { {
                    nisGnss.add(scalarUpdate(f, 0, zgx, sigmaGps * sigmaGps, false));
                } }

                double innovY = zgy - f.x[1];
                double Sy = f.P[1][1] + sigmaGps * sigmaGps;
                double d2y = innovY * innovY / Sy;
                if (d2y < 6.63) { {
                    nisGnss.add(scalarUpdate(f, 1, zgy, sigmaGps * sigmaGps, false));
                } }
            } }

            if (k == 1000 || k == 1510 || k == 1710) { {
                double zax = xTrue[k][0] + 0.10 * rng.nextGaussian();
                double zay = xTrue[k][1] + 0.10 * rng.nextGaussian();
                double zap = wrapAngle(xTrue[k][2] + (2.0 * PI / 180.0) * rng.nextGaussian());

                nisAnchor.add(scalarUpdate(f, 0, zax, 0.10 * 0.10, false));
                nisAnchor.add(scalarUpdate(f, 1, zay, 0.10 * 0.10, false));
                nisAnchor.add(scalarUpdate(f, 2, zap, Math.pow(2.0 * PI / 180.0, 2), true));
            } }

            if (nisGnss.size() >= 20) { {
                double m = 0.0;
                for (int i = nisGnss.size() - 20; i < nisGnss.size(); i++) m += nisGnss.get(i);
                m /= 20.0;
                if (m > 1.8) { {
                    for (int i = 0; i < 4; i++)
                        for (int j = 0; j < 4; j++)
                            f.P[i][j] *= 1.02;
                } }
            } }

            for (int i = 0; i < 4; i++) xEstHist[k][i] = f.x[i];
            pTrace[k] = trace4(f.P);
        } }

        double sumSq = 0.0;
        for (int k = 0; k < N; k++) { {
            double ex = xEstHist[k][0] - xTrue[k][0];
            double ey = xEstHist[k][1] - xTrue[k][1];
            sumSq += ex * ex + ey * ey;
        } }
        double rmse = Math.sqrt(sumSq / N);

        double exf = xEstHist[N-1][0] - xTrue[N-1][0];
        double eyf = xEstHist[N-1][1] - xTrue[N-1][1];
        double finalErr = Math.sqrt(exf * exf + eyf * eyf);
        double finalHeadingDeg = wrapAngle(xEstHist[N-1][2] - xTrue[N-1][2]) * 180.0 / PI;

        System.out.println("=== Chapter18_Lesson3.java : Long-Range Localization Drift Handling ===");
        System.out.printf("Mission duration: %.1f s%n", N * dt);
        System.out.printf("Position RMSE: %.3f m%n", rmse);
        System.out.printf("Final position error: %.3f m%n", finalErr);
        System.out.printf("Final heading error: %.3f deg%n", finalHeadingDeg);
        System.out.printf("Mean scalar NIS (GNSS accepted): %.3f%n", mean(nisGnss));
        System.out.printf("Mean scalar NIS (Anchor): %.3f%n", mean(nisAnchor));
        System.out.printf("Final covariance trace: %.5f%n", pTrace[N-1]);

        int[][] outages = { { { {420, 980} }, { {1250, 1500} } } };
        String[] names = { {"Outage1", "Outage2"} };
        for (int i = 0; i < outages.length; i++) { {
            int a = outages[i][0];
            int b = outages[i][1];
            double esx = xEstHist[a][0] - xTrue[a][0];
            double esy = xEstHist[a][1] - xTrue[a][1];
            double eex = xEstHist[b-1][0] - xTrue[b-1][0];
            double eey = xEstHist[b-1][1] - xTrue[b-1][1];
            double estart = Math.sqrt(esx * esx + esy * esy);
            double eend = Math.sqrt(eex * eex + eey * eey);
            System.out.printf("%s: error at start=%.3f m, end=%.3f m, growth=%.3f m%n",
                    names[i], estart, eend, eend - estart);
        } }
    } }
} }
      

The MATLAB code below implements the same estimator. For Simulink, map the logic into: (1) a propagation subsystem at IMU/wheel rate, (2) a GNSS update subsystem at lower rate, and (3) an event-triggered anchor subsystem for map relocalization.

Chapter18_Lesson3.m

% Chapter18_Lesson3.m
% Long-Range Localization Drift Handling (2D EKF)
% State: [x; y; psi; b_g]
% Features:
%   - Propagation with wheel speed + gyro
%   - GNSS/RTK updates (sequential scalar updates + gating)
%   - Terrain-aware process noise inflation
%   - Anchor re-localization updates
%   - Integrity monitor (covariance inflation)
%
% Simulink note:
%   The same logic maps naturally to a multi-rate model:
%   (1) Propagation subsystem at dt=0.1 s
%   (2) GNSS update subsystem at 1 Hz
%   (3) Anchor update trigger subsystem (event-based)

rng(18);

dt = 0.1;
N  = 1800;
bg_true = 0.015;

x_true = zeros(4, N);   % [x; y; psi; bg]
x_est  = [0; 0; 0.05; 0];
P_est  = diag([1, 1, deg2rad(5)^2, 0.03^2]);

x_hist = zeros(4, N);
P_trace = zeros(1, N);
nis_gnss = [];
nis_anchor = [];

for k = 2:N
    tk = (k-1) * dt;
    v_cmd = 1.5 + 0.3 * sin(0.015 * tk);
    w_cmd = 0.08 * sin(0.010 * tk) + 0.04 * sin(0.040 * tk);

    rough = ((k-1) >= 300 && (k-1) < 520) || ...
            ((k-1) >= 840 && (k-1) < 1100) || ...
            ((k-1) >= 1400 && (k-1) < 1620);

    sigma_w_true = ternary(rough, 0.03, 0.008);
    sigma_v_true = ternary(rough, 0.10, 0.02);

    v_true = v_cmd + sigma_v_true * randn;
    w_true = w_cmd + sigma_w_true * randn;

    x_true(1,k) = x_true(1,k-1) + v_true * dt * cos(x_true(3,k-1));
    x_true(2,k) = x_true(2,k-1) + v_true * dt * sin(x_true(3,k-1));
    x_true(3,k) = wrap_angle(x_true(3,k-1) + (w_true - bg_true) * dt);
    x_true(4,k) = bg_true;

    vm = v_true + ternary(rough, 0.12, 0.03) * randn;
    wm = w_true + ternary(rough, 0.05, 0.01) * randn;

    [x_est, P_est] = propagate_ekf(x_est, P_est, vm, wm, dt, rough);

    outage = (((k-1) >= 420 && (k-1) < 980) || ((k-1) >= 1250 && (k-1) < 1500));

    if mod(k-1, 10) == 0 && ~outage
        rtk_fixed = (~rough) && (mod(k-1, 90) ~= 0);
        sigma_gps = ternary(rtk_fixed, 0.15, 0.75);

        zgx = x_true(1,k) + sigma_gps * randn;
        zgy = x_true(2,k) + sigma_gps * randn;

        innov_x = zgx - x_est(1);
        Sx = P_est(1,1) + sigma_gps^2;
        d2x = innov_x^2 / Sx;
        if d2x < 6.63
            [x_est, P_est, nisx] = scalar_update(x_est, P_est, 1, zgx, sigma_gps^2, false);
            nis_gnss(end+1) = nisx; %#ok<AGROW>
        end

        innov_y = zgy - x_est(2);
        Sy = P_est(2,2) + sigma_gps^2;
        d2y = innov_y^2 / Sy;
        if d2y < 6.63
            [x_est, P_est, nisy] = scalar_update(x_est, P_est, 2, zgy, sigma_gps^2, false);
            nis_gnss(end+1) = nisy; %#ok<AGROW>
        end
    end

    if any((k-1) == [1000, 1510, 1710])
        zax = x_true(1,k) + 0.10 * randn;
        zay = x_true(2,k) + 0.10 * randn;
        zap = wrap_angle(x_true(3,k) + deg2rad(2) * randn);

        [x_est, P_est, n1] = scalar_update(x_est, P_est, 1, zax, 0.10^2, false);
        [x_est, P_est, n2] = scalar_update(x_est, P_est, 2, zay, 0.10^2, false);
        [x_est, P_est, n3] = scalar_update(x_est, P_est, 3, zap, deg2rad(2)^2, true);
        nis_anchor = [nis_anchor, n1, n2, n3]; %#ok<AGROW>
    end

    if numel(nis_gnss) >= 20
        if mean(nis_gnss(end-19:end)) > 1.8
            P_est = 1.02 * P_est;
        end
    end

    x_hist(:,k) = x_est;
    P_trace(k) = trace(P_est);
end

pos_err = x_hist(1:2,:) - x_true(1:2,:);
rmse = sqrt(mean(sum(pos_err.^2, 1)));
final_err = norm(pos_err(:,end));
final_heading_deg = rad2deg(wrap_angle(x_hist(3,end) - x_true(3,end)));

fprintf('=== Chapter18_Lesson3.m : Long-Range Localization Drift Handling ===\n');
fprintf('Mission duration: %.1f s\n', N*dt);
fprintf('Position RMSE: %.3f m\n', rmse);
fprintf('Final position error: %.3f m\n', final_err);
fprintf('Final heading error: %.3f deg\n', final_heading_deg);
fprintf('Mean scalar NIS (GNSS accepted): %.3f\n', mean_or_nan(nis_gnss));
fprintf('Mean scalar NIS (Anchor): %.3f\n', mean_or_nan(nis_anchor));
fprintf('Final covariance trace: %.5f\n', P_trace(end));

report_outage('Outage1', 421, 980, x_hist, x_true);
report_outage('Outage2', 1251, 1500, x_hist, x_true);

%% --- Local functions ---

function [x, P] = propagate_ekf(x, P, vm, wm, dt, rough)
    psi = x(3);
    bg  = x(4);

    x(1) = x(1) + vm * dt * cos(psi);
    x(2) = x(2) + vm * dt * sin(psi);
    x(3) = wrap_angle(x(3) + (wm - bg) * dt);

    F = eye(4);
    F(1,3) = -vm * dt * sin(psi);
    F(2,3) =  vm * dt * cos(psi);
    F(3,4) = -dt;

    sigma_v = ternary(rough, 0.12, 0.03);
    sigma_w = ternary(rough, 0.06, 0.01);
    sigma_bg = ternary(rough, 0.0030, 0.0008);

    Q = diag([(sigma_v*dt)^2, (sigma_v*dt)^2, (sigma_w*dt)^2, sigma_bg^2*dt]);

    P = F * P * F' + Q;
    P = 0.5 * (P + P');
end

function [x, P, nis] = scalar_update(x, P, idx, z, R, angleResidual)
    innov = z - x(idx);
    if angleResidual
        innov = wrap_angle(innov);
    end

    S = P(idx,idx) + R;
    K = P(:,idx) / S;

    x = x + K * innov;
    x(3) = wrap_angle(x(3));

    P = P - K * P(idx,:);
    P = 0.5 * (P + P');

    nis = innov^2 / S;
end

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

function y = ternary(cond, a, b)
    if cond
        y = a;
    else
        y = b;
    end
end

function m = mean_or_nan(v)
    if isempty(v)
        m = NaN;
    else
        m = mean(v);
    end
end

function report_outage(name, a, b, x_hist, x_true)
    e_start = norm(x_hist(1:2, a) - x_true(1:2, a));
    e_end = norm(x_hist(1:2, b) - x_true(1:2, b));
    fprintf('%s: error at start=%.3f m, end=%.3f m, growth=%.3f m\n', ...
        name, e_start, e_end, e_end - e_start);
end
      

Mathematica is helpful for rapid numerical experimentation and symbolic checks of the drift equations. The following notebook-style script reproduces the EKF simulation and drift mitigation logic.

Chapter18_Lesson3.nb

(* Chapter18_Lesson3.nb *)
(* Long-Range Localization Drift Handling (2D EKF, Wolfram Language) *)
(* State: {x, y, psi, bg} *)

SeedRandom[18];

wrapAngle[a_] := Module[{b = a}, Mod[b + Pi, 2 Pi] - Pi]

isRough[k_] := (300 <= k < 520) || (840 <= k < 1100) || (1400 <= k < 1620);
inOutage[k_] := (420 <= k < 980) || (1250 <= k < 1500);

propagate[x_, P_, vm_, wm_, dt_, rough_] := Module[
  {psi, bg, xn, F, sigmaV, sigmaW, sigmaBg, Q, Pn},
  psi = x[[3]];
  bg  = x[[4]];
  xn = x;
  xn[[1]] = xn[[1]] + vm dt Cos[psi];
  xn[[2]] = xn[[2]] + vm dt Sin[psi];
  xn[[3]] = wrapAngle[xn[[3]] + (wm - bg) dt];

  F = IdentityMatrix[4];
  F[[1, 3]] = -vm dt Sin[psi];
  F[[2, 3]] =  vm dt Cos[psi];
  F[[3, 4]] = -dt;

  sigmaV = If[rough, 0.12, 0.03];
  sigmaW = If[rough, 0.06, 0.01];
  sigmaBg = If[rough, 0.0030, 0.0008];

  Q = DiagonalMatrix[{(sigmaV dt)^2, (sigmaV dt)^2, (sigmaW dt)^2, sigmaBg^2 dt}];
  Pn = F.P.Transpose[F] + Q;
  Pn = 0.5 (Pn + Transpose[Pn]);
  {xn, Pn}
];

scalarUpdate[x_, P_, idx_, z_, R_, angleResidual_] := Module[
  {innov, S, K, xn, Pn, nis},
  innov = z - x[[idx]];
  If[angleResidual, innov = wrapAngle[innov]];
  S = P[[idx, idx]] + R;
  K = P[[All, idx]]/S;
  xn = x + K innov;
  xn[[3]] = wrapAngle[xn[[3]]];
  Pn = P - Outer[Times, K, P[[idx, All]]];
  Pn = 0.5 (Pn + Transpose[Pn]);
  nis = innov^2/S;
  {xn, Pn, nis}
];

dt = 0.1;
N = 1800;
bgTrue = 0.015;

xTrue = ConstantArray[0., {N, 4}];
xHist = ConstantArray[0., {N, 4}];
pTrace = ConstantArray[0., N];
nisGnss = {};
nisAnchor = {};

xEst = {0., 0., 0.05, 0.};
PEst = DiagonalMatrix[{1., 1., (5 Degree)^2, 0.03^2}];

Do[
  tk = (k - 1) dt;
  vCmd = 1.5 + 0.3 Sin[0.015 tk];
  wCmd = 0.08 Sin[0.010 tk] + 0.04 Sin[0.040 tk];

  rough = isRough[k - 1];
  sigmaWTrue = If[rough, 0.03, 0.008];
  sigmaVTrue = If[rough, 0.10, 0.02];

  vTrue = vCmd + sigmaVTrue RandomVariate[NormalDistribution[0, 1]];
  wTrue = wCmd + sigmaWTrue RandomVariate[NormalDistribution[0, 1]];

  xTrue[[k, 1]] = xTrue[[k - 1, 1]] + vTrue dt Cos[xTrue[[k - 1, 3]]];
  xTrue[[k, 2]] = xTrue[[k - 1, 2]] + vTrue dt Sin[xTrue[[k - 1, 3]]];
  xTrue[[k, 3]] = wrapAngle[xTrue[[k - 1, 3]] + (wTrue - bgTrue) dt];
  xTrue[[k, 4]] = bgTrue;

  vm = vTrue + If[rough, 0.12, 0.03] RandomVariate[NormalDistribution[0, 1]];
  wm = wTrue + If[rough, 0.05, 0.01] RandomVariate[NormalDistribution[0, 1]];

  {xEst, PEst} = propagate[xEst, PEst, vm, wm, dt, rough];

  If[Mod[k - 1, 10] == 0 && ! inOutage[k - 1],
    rtkFixed = (! rough) && (Mod[k - 1, 90] != 0);
    sigmaGps = If[rtkFixed, 0.15, 0.75];

    zgx = xTrue[[k, 1]] + sigmaGps RandomVariate[NormalDistribution[0, 1]];
    zgy = xTrue[[k, 2]] + sigmaGps RandomVariate[NormalDistribution[0, 1]];

    innovX = zgx - xEst[[1]];
    sx = PEst[[1, 1]] + sigmaGps^2;
    d2x = innovX^2/sx;
    If[d2x < 6.63,
      {xEst, PEst, nisx} = scalarUpdate[xEst, PEst, 1, zgx, sigmaGps^2, False];
      nisGnss = Append[nisGnss, nisx];
    ];

    innovY = zgy - xEst[[2]];
    sy = PEst[[2, 2]] + sigmaGps^2;
    d2y = innovY^2/sy;
    If[d2y < 6.63,
      {xEst, PEst, nisy} = scalarUpdate[xEst, PEst, 2, zgy, sigmaGps^2, False];
      nisGnss = Append[nisGnss, nisy];
    ];
  ];

  If[MemberQ[{1000, 1510, 1710}, k - 1],
    zax = xTrue[[k, 1]] + 0.10 RandomVariate[NormalDistribution[0, 1]];
    zay = xTrue[[k, 2]] + 0.10 RandomVariate[NormalDistribution[0, 1]];
    zap = wrapAngle[xTrue[[k, 3]] + (2 Degree) RandomVariate[NormalDistribution[0, 1]]];

    {xEst, PEst, n1} = scalarUpdate[xEst, PEst, 1, zax, 0.10^2, False];
    {xEst, PEst, n2} = scalarUpdate[xEst, PEst, 2, zay, 0.10^2, False];
    {xEst, PEst, n3} = scalarUpdate[xEst, PEst, 3, zap, (2 Degree)^2, True];
    nisAnchor = Join[nisAnchor, {n1, n2, n3}];
  ];

  If[Length[nisGnss] >= 20 && Mean[Take[nisGnss, -20]] > 1.8,
    PEst = 1.02 PEst;
  ];

  xHist[[k]] = xEst;
  pTrace[[k]] = Tr[PEst];
, {k, 2, N}];

posErr = xHist[[All, 1 ;; 2]] - xTrue[[All, 1 ;; 2]];
rmse = Sqrt[Mean[Total /@ (posErr^2)]];
finalErr = Norm[posErr[[-1]]];
finalHeadingDeg = (180/Pi) wrapAngle[xHist[[-1, 3]] - xTrue[[-1, 3]]];

meanOrNaN[v_] := If[Length[v] == 0, Indeterminate, Mean[v]];

Print["=== Chapter18_Lesson3.nb : Long-Range Localization Drift Handling ==="];
Print["Mission duration: ", NumberForm[N dt, {8, 1}], " s"];
Print["Position RMSE: ", NumberForm[rmse, {8, 3}], " m"];
Print["Final position error: ", NumberForm[finalErr, {8, 3}], " m"];
Print["Final heading error: ", NumberForm[finalHeadingDeg, {8, 3}], " deg"];
Print["Mean scalar NIS (GNSS accepted): ", NumberForm[meanOrNaN[nisGnss], {8, 3}]];
Print["Mean scalar NIS (Anchor): ", NumberForm[meanOrNaN[nisAnchor], {8, 3}]];
Print["Final covariance trace: ", NumberForm[pTrace[[-1]], {8, 5}]];

outageReport[name_, a_, b_] := Module[{es, ee},
  es = Norm[xHist[[a + 1, 1 ;; 2]] - xTrue[[a + 1, 1 ;; 2]]];
  ee = Norm[xHist[[b, 1 ;; 2]] - xTrue[[b, 1 ;; 2]]];
  Print[name, ": error at start=", NumberForm[es, {8, 3}],
        " m, end=", NumberForm[ee, {8, 3}],
        " m, growth=", NumberForm[ee - es, {8, 3}], " m"];
];

outageReport["Outage1", 420, 979];
outageReport["Outage2", 1250, 1499];
      

Problem 1 (Heading drift bound by travel distance): A robot travels \( L=250\,\mathrm{m} \) during a GNSS outage. If the heading estimation error is bounded by \( |\delta\psi| \le 1.2^\circ \), derive a worst-case cross-track drift bound using Proposition 1.

Solution: Convert angle to radians: \( 1.2^\circ \approx 0.02094 \). From \( |e_{\perp}| \le L|\delta\psi| \):

\[ |e_{\perp}| \le 250 \times 0.02094 \approx 5.235\,\mathrm{m} \]

So even a small heading error can produce several meters of lateral drift over a long outage. This motivates aggressive bias tracking and periodic heading anchors.

Problem 2 (Bias uncertainty growth): Assume straight motion at \( v=1.8\,\mathrm{m/s} \) for \( T=120\,\mathrm{s} \). Let \( \sigma_\psi = 0.5^\circ \) and \( \sigma_b = 0.004^\circ/\mathrm{s} \). Approximate the standard deviation of cross-track error using Proposition 2.

Solution: Convert to radians: \( \sigma_\psi \approx 8.727\times 10^{-3} \), \( \sigma_b \approx 6.981\times 10^{-5} \). Then

\[ \sigma_{e_{\perp} } \approx v\sqrt{T^2\sigma_\psi^2 + \frac{1}{4}T^4\sigma_b^2} \]

Numerically:

\[ \sigma_{e_{\perp} } \approx 1.8\sqrt{120^2(8.727\times10^{-3})^2 + \frac{1}{4}120^4(6.981\times10^{-5})^2} \approx 4.24\,\mathrm{m} \]

The bias term is substantial for long outages because of the \( T^4 \) variance scaling.

Problem 3 (Innovation gating threshold): A 2D GNSS measurement has innovation covariance \( \mathbf{S} = \operatorname{diag}(0.49, 0.49) \). The innovation is \( \boldsymbol{\nu}=[0.9,\;0.4]^T \) meters. Compute \( d^2 \) and test it against a 99% gate for 2 DOF (use \( \gamma=9.21 \)).

Solution:

\[ d^2 = \boldsymbol{\nu}^T\mathbf{S}^{-1}\boldsymbol{\nu} = \frac{0.9^2}{0.49} + \frac{0.4^2}{0.49} = \frac{0.81 + 0.16}{0.49} = \frac{0.97}{0.49} \approx 1.98 \]

Since \( 1.98 < 9.21 \), the update passes the gate and is accepted.

Problem 4 (Effect of covariance inflation): Let a scalar state variance before inflation be \( P=0.25 \). An innovation consistency monitor triggers inflation with \( \alpha=1.08 \). What is the new variance, and how does it affect the Kalman gain qualitatively?

Solution: The new variance is

\[ P^{\mathrm{new} } = \alpha P = 1.08 \times 0.25 = 0.27 \]

A larger prior variance generally increases the Kalman gain for the same measurement noise, so the estimator becomes more willing to correct from absolute measurements. This is useful when the filter has become overconfident.

Problem 5 (Anchor scheduling policy): Suppose the navigation stack requires the predicted 2D position standard deviation to remain below \( 1.5\,\mathrm{m} \). Propose a rule for triggering re-localization attempts using covariance.

Solution: Let \( \mathbf{P}_{xy} = \mathbf{P}[1:2,1:2] \) be the position covariance block. A conservative scalar metric is \( \sqrt{\lambda_{\max}(\mathbf{P}_{xy})} \). Trigger re-localization when

\[ \sqrt{\lambda_{\max}(\mathbf{P}_{xy})} > 1.5 \]

or equivalently when \( \lambda_{\max}(\mathbf{P}_{xy}) > 2.25 \). This aligns the estimator with a task-level uncertainty budget.

Long-range localization drift in outdoor AMR is fundamentally a bias-and-observability problem: small heading errors grow with path length, and bias uncertainty grows rapidly during absolute-sensing outages. Robust drift handling therefore combines (i) a physically meaningful error-state model, (ii) innovation gating and consistency checks, (iii) terrain-aware noise inflation, and (iv) periodic re-anchoring from GNSS/RTK or map-based loop closures. The provided implementations show a minimal but complete pipeline in Python, C++, Java, MATLAB/Simulink style, and Wolfram Mathematica.