In Chapters 5–13, we developed dead-reckoning and probabilistic localization (EKF/UKF/MCL/SLAM). In open outdoor spaces, loop closures may be rare and long paths amplify drift. GNSS provides a drift-bounding measurement in a global geodetic frame. However, standard GNSS position fixes typically have meter-level uncertainty and can be biased by multipath and NLOS. RTK (Real-Time Kinematic) leverages carrier-phase and differential corrections to reach centimeter-level accuracy when conditions allow.

We distinguish three layers: \( \text{geodesy} \) (WGS84 frames, coordinate conversion), \( \text{GNSS estimation} \) (from raw measurements to a pose fix), and \( \text{robot fusion} \) (integrating the fix with wheel–IMU motion models).

A key engineering goal is integrity: the system must detect when GNSS/RTK is unreliable and reduce its influence, rather than blindly fusing it. This is achieved via innovation gating, robust loss functions, and quality flags (e.g., RTK fixed/float).

Let a receiver at ECEF position \( \mathbf{p}\in\mathbb{R}^3 \) observe satellite \( s \) at known position \( \mathbf{p}_s \). The geometric range is \( \rho_s(\mathbf{p}) = \|\mathbf{p}-\mathbf{p}_s\| \). A simplified pseudorange model is

\[ P_s = \rho_s(\mathbf{p}) + c(\delta t_r - \delta t_s) + T_s + I_s + \epsilon_{P,s}, \]

where \( c \) is the speed of light, \( \delta t_r \) receiver clock bias, \( \delta t_s \) satellite clock bias, \( T_s \) tropospheric delay, \( I_s \) ionospheric delay, and \( \epsilon_{P,s} \) aggregates measurement noise and multipath.

The carrier-phase (in meters) is much more precise but introduces an integer ambiguity:

\[ \Phi_s = \rho_s(\mathbf{p}) + c(\delta t_r - \delta t_s) + T_s - I_s + \lambda N_s + \epsilon_{\Phi,s}, \]

where \( \lambda \) is the carrier wavelength and \( N_s\in\mathbb{Z} \) is an unknown integer. RTK is fundamentally about estimating \( \mathbf{p} \) and resolving the integer vector \( \mathbf{N} \).

Linearization (used by EKF / Gauss–Newton). Around a nominal \( \mathbf{p}_0 \), define \( \delta\mathbf{p}=\mathbf{p}-\mathbf{p}_0 \). Using \( \rho_s(\mathbf{p}) \approx \rho_s(\mathbf{p}_0) + \mathbf{u}_s^\top \delta\mathbf{p} \), where the line-of-sight unit vector is \( \mathbf{u}_s = \frac{\mathbf{p}_0 - \mathbf{p}_s}{\|\mathbf{p}_0 - \mathbf{p}_s\|} \). Thus the Jacobian row w.r.t. \( \mathbf{p} \) is \( \mathbf{u}_s^\top \).

RTK uses a base receiver at a known position and a rover (the robot). Let \( \Phi_s^r \) be rover carrier-phase to satellite \( s \), and \( \Phi_s^b \) the base measurement. Consider the single difference:

\[ \Delta \Phi_s^{rb} \equiv \Phi_s^r - \Phi_s^b = \big(\rho_s(\mathbf{p}^r) - \rho_s(\mathbf{p}^b)\big) + c(\delta t_r^r - \delta t_r^b) + \lambda (N_s^r - N_s^b) + \Delta\epsilon_s. \]

Satellite clock bias \( \delta t_s \) cancels in the difference. Next, choose a reference satellite \( s_0 \) and form the double difference:

\[ \nabla\Delta \Phi_{s,s_0}^{rb} \equiv \Delta \Phi_s^{rb} - \Delta \Phi_{s_0}^{rb} = \big(\rho_s(\mathbf{p}^r) - \rho_s(\mathbf{p}^b)\big) - \big(\rho_{s_0}(\mathbf{p}^r) - \rho_{s_0}(\mathbf{p}^b)\big) + \lambda \nabla\Delta N_{s,s_0}^{rb} + \nabla\Delta\epsilon. \]

Now the receiver clock terms also cancel, dramatically reducing common-mode biases. Under short baselines, atmospheric differences are small, enabling cm-level positioning after integer ambiguity resolution.

The integer least squares (ILS) formulation (schematic) is:

\[ \hat{\mathbf{N} } = \arg\min_{\mathbf{N}\in\mathbb{Z}^m} \big(\mathbf{N}-\hat{\mathbf{N} }_{float}\big)^\top \mathbf{Q}_{NN}^{-1} \big(\mathbf{N}-\hat{\mathbf{N} }_{float}\big), \]

where \( \hat{\mathbf{N} }_{float} \) and \( \mathbf{Q}_{NN} \) come from a real-valued estimation step (often a Kalman filter). The LAMBDA method is a classic approach to decorrelate \( \mathbf{Q}_{NN} \) and enable efficient integer search.

A navigation stack usually operates in a local metric frame (map/odom), while GNSS reports geodetic coordinates. A common pipeline is: \( (\varphi,\lambda,h) \) (latitude, longitude, height) \( → \) ECEF \( \mathbf{p} \) \( → \) local ENU \( \mathbf{p}^{enu} \).

For WGS84, with semi-major axis \( a \), flattening \( f \), and eccentricity squared \( e^2 = f(2-f) \), define the prime vertical radius \( N(\varphi) = \frac{a}{\sqrt{1-e^2\sin^2\varphi} } \). The ECEF conversion is:

\[ \begin{aligned} x &= (N(\varphi)+h)\cos\varphi\cos\lambda, \\ y &= (N(\varphi)+h)\cos\varphi\sin\lambda, \\ z &= (N(\varphi)(1-e^2)+h)\sin\varphi. \end{aligned} \]

Given a reference origin \( (\varphi_0,\lambda_0,h_0) \) with ECEF \( \mathbf{p}_0 \), the ENU coordinates are \( \mathbf{p}^{enu} = \mathbf{R}(\varphi_0,\lambda_0)(\mathbf{p}-\mathbf{p}_0) \), with rotation

\[ \mathbf{R}(\varphi_0,\lambda_0)= \begin{bmatrix} -\sin\lambda_0 & \cos\lambda_0 & 0 \\ -\sin\varphi_0\cos\lambda_0 & -\sin\varphi_0\sin\lambda_0 & \cos\varphi_0 \\ \cos\varphi_0\cos\lambda_0 & \cos\varphi_0\sin\lambda_0 & \sin\varphi_0 \end{bmatrix}. \]

In practice, this ENU frame is aligned with the robot’s map frame (possibly with a constant yaw rotation) after an initialization step.

Let the robot planar state be \( \mathbf{x} = [p_x, p_y, \psi, v, b_g]^\top \) where \( \psi \) is yaw and \( b_g \) is gyro bias. With wheel speed measurement \( v_m \) and gyro measurement \( \omega_m \), a simple continuous-time kinematic model is:

\[ \dot p_x = v\cos\psi,\quad \dot p_y = v\sin\psi,\quad \dot\psi = \omega_m - b_g,\quad \dot v = w_v,\quad \dot b_g = w_{bg}, \]

where \( w_v \) and \( w_{bg} \) are process noises (random walks). A loosely coupled approach uses the GNSS/RTK position fix as a measurement: \( \mathbf{z}_{gnss} = [p_x, p_y]^\top + \mathbf{v} \). A tightly coupled approach fuses raw pseudoranges / carrier phases directly; this improves robustness under partial satellite visibility but is more complex.

For the loosely coupled EKF measurement model \( \mathbf{h}(\mathbf{x}) = [p_x, p_y]^\top \), the Jacobian is constant: \( \mathbf{H} = \begin{bmatrix}1&0&0&0&0\\0&1&0&0&0\end{bmatrix} \). The EKF update is:

\[ \begin{aligned} \mathbf{K} &= \mathbf{P} \mathbf{H}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \mathbf{R})^{-1},\\ \mathbf{x} ← \mathbf{x} + \mathbf{K}\big(\mathbf{z}-\mathbf{h}(\mathbf{x})\big),\\ \mathbf{P} ← (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}. \end{aligned} \]

flowchart TD
  A["Sensors: wheel + IMU + GNSS"] --> B["Pre-processing: time sync, calibration"]
  B --> C["RTK engine: base corrections + ambiguity handling"]
  C --> D["Quality flags: fix/float, DOP, residuals"]
  D --> E["Fusion filter: EKF/UKF on planar state"]
  E --> F["Navigation stack: global plan + local control"]
  D --> G["Integrity logic: gate / downweight / reject"]
  G --> E
        

Design note. The measurement covariance \( \mathbf{R} \) should depend on RTK solution status. For example, RTK-fixed may justify centimeter-level \( \sigma \), while float or autonomous GNSS should be inflated to decimeter/meter scale.

A standard consistency check uses the normalized innovation squared (NIS): \( \nu = \mathbf{z}-\mathbf{h}(\mathbf{x}) \), \( \mathbf{S}=\mathbf{H}\mathbf{P}\mathbf{H}^\top+\mathbf{R} \). The statistic \( d^2 = \nu^\top \mathbf{S}^{-1}\nu \) follows a chi-square distribution with degrees of freedom equal to the measurement dimension under the filter’s assumptions.

\[ d^2 \sim \chi^2_{m}\quad \text{(approximately, if the linear-Gaussian assumptions hold)}. \]

Thus a gate at confidence \( 1-\alpha \) rejects measurements when \( d^2 > \chi^2_{m}(1-\alpha) \). For \( m=2 \) and \( 1-\alpha=0.99 \), the threshold is about \( 9.21 \).

Cycle slips. In carrier-phase, a cycle slip changes the integer ambiguity \( N \) abruptly. RTK engines detect slips using time-differenced phase, Doppler consistency, or residual tests, and then re-initialize ambiguities.

Multipath/NLOS. Reflections distort pseudorange and can bias position. Practical defenses include: (i) antenna placement and ground planes, (ii) elevation masks (discard low-elevation satellites), (iii) robust costs (Huber/Cauchy) on GNSS residuals, (iv) switching logic that downweights GNSS when local sensors indicate slip or poor traction.

In typical robotics middleware, the GNSS receiver publishes geodetic position while the navigation stack expects local metric pose. A robust integration workflow is:

• Convert GNSS \( (\varphi,\lambda,h) \) to local ENU using a fixed origin. Keep the origin stable per mission to avoid frame jumps.
• Time-align GNSS with IMU/odometry. If GNSS arrives at \( t_k \), but IMU is at higher rate, propagate the filter to \( t_k \) before updating.
• Use quality flags to set \( \mathbf{R} \) (RTK fixed < float < GNSS).
• Monitor NIS and residuals; on repeated rejections, trigger recovery (slow down, re-initialize, or rely on local odometry).

Implementation note. A common approach is to publish a fused planar pose for the navigation stack, while storing the geodetic fix for logging and geo-fencing. The examples below implement a didactic EKF fusion loop.

The following codes implement a compact loosely-coupled EKF for planar motion and GNSS/RTK position updates, including chi-square gating. They are designed for teaching and can be adapted into ROS2 nodes or other middleware.

Chapter18_Lesson1.py

"""
Chapter18_Lesson1.py
Autonomous Mobile Robots (Control Engineering) — Chapter 18, Lesson 1
GPS/RTK Integration in Navigation (didactic EKF fusion example)

This script implements a lightweight EKF that fuses:
  - wheel speed (as an input / pseudo-measurement)
  - gyro yaw-rate (as an input with bias state)
  - GNSS/RTK position in a local ENU frame (as a measurement)

It is intentionally dependency-light (numpy only) and designed for clarity.
"""

from __future__ import annotations
import math
import numpy as np
from dataclasses import dataclass


def wrap_to_pi(angle: float) -> float:
    """Wrap angle to (-pi, pi]."""
    a = (angle + math.pi) % (2.0 * math.pi) - math.pi
    # map -pi to +pi for consistency
    return a if a > -math.pi else a + 2.0 * math.pi


@dataclass
class EkfConfig:
    # Process noise (continuous-time-ish, scaled by dt in discretization)
    q_xy: float = 0.05          # m^2/s (unmodeled planar drift)
    q_yaw: float = 0.01         # rad^2/s (yaw random walk beyond gyro)
    q_v: float = 0.5            # (m/s)^2/s (speed random walk)
    q_bg: float = 1e-4          # (rad/s)^2/s (gyro-bias random walk)

    # Wheel speed "measurement" noise (if you choose to update v)
    r_v: float = 0.2**2         # (m/s)^2

    # Default GNSS measurement covariance (overridden by RTK status)
    r_gps_fix: float = 1.5**2   # m^2 (single-point)
    r_gps_rtk: float = 0.02**2  # m^2 (RTK-fixed)

    # Innovation gate: chi-square threshold for 2D position (dof=2)
    # 0.99-quantile for chi2(2) ≈ 9.2103
    gate_chi2_dof2: float = 9.2103


class EkfGpsFusion:
    """
    State: x = [px, py, yaw, v, b_g]^T
      px, py  : position in ENU (meters)
      yaw     : heading in ENU frame (rad)
      v       : forward speed (m/s)
      b_g     : gyro z-bias (rad/s)

    Inputs:
      v_meas      : wheel speed estimate (m/s)
      omega_meas  : gyro yaw-rate (rad/s)

    Measurements:
      z_gnss = [px, py]^T
      (optionally) z_v = v_meas as pseudo-measurement for v
    """

    def __init__(self, cfg: EkfConfig = EkfConfig()):
        self.cfg = cfg
        self.x = np.zeros((5, 1), dtype=float)
        self.P = np.diag([10.0, 10.0, (20.0*math.pi/180.0)**2, 2.0**2, (5.0*math.pi/180.0)**2])

    def set_state(self, px: float, py: float, yaw: float, v: float, bg: float) -> None:
        self.x[:, 0] = [px, py, yaw, v, bg]

    def predict(self, v_meas: float, omega_meas: float, dt: float) -> None:
        if dt <= 0.0:
            return

        px, py, yaw, v, bg = self.x[:, 0]
        omega = omega_meas - bg

        # Simple speed blending: treat wheel speed as input that nudges v state
        # (You can instead do a separate measurement update for v.)
        v_pred = v  # random walk
        yaw_pred = wrap_to_pi(yaw + omega * dt)

        px_pred = px + v_pred * math.cos(yaw_pred) * dt
        py_pred = py + v_pred * math.sin(yaw_pred) * dt

        # State transition Jacobian F = df/dx
        F = np.eye(5)
        F[0, 2] = -v_pred * math.sin(yaw_pred) * dt
        F[0, 3] =  math.cos(yaw_pred) * dt
        F[1, 2] =  v_pred * math.cos(yaw_pred) * dt
        F[1, 3] =  math.sin(yaw_pred) * dt
        F[2, 4] = -dt  # yaw depends on -bg

        # Process noise covariance Q (discretized)
        q_xy = self.cfg.q_xy
        q_yaw = self.cfg.q_yaw
        q_v = self.cfg.q_v
        q_bg = self.cfg.q_bg
        Q = np.diag([q_xy*dt, q_xy*dt, q_yaw*dt, q_v*dt, q_bg*dt])

        # Commit prediction
        self.x[:, 0] = [px_pred, py_pred, yaw_pred, v_pred, bg]
        self.P = F @ self.P @ F.T + Q

        # Optional: incorporate wheel speed as a pseudo-measurement for v
        self.update_speed(v_meas)

    def update_speed(self, v_meas: float) -> None:
        # z = v + noise
        H = np.zeros((1, 5))
        H[0, 3] = 1.0
        z = np.array([[v_meas]], dtype=float)
        h = np.array([[self.x[3, 0]]], dtype=float)
        R = np.array([[self.cfg.r_v]], dtype=float)

        y = z - h
        S = H @ self.P @ H.T + R
        K = self.P @ H.T @ np.linalg.inv(S)

        self.x = self.x + K @ y
        self.P = (np.eye(5) - K @ H) @ self.P

    def update_gnss(self, px_meas: float, py_meas: float, r_pos: float) -> bool:
        """
        GNSS/RTK position update with chi-square gating.
        Returns True if accepted, False if rejected by gate.
        """
        H = np.zeros((2, 5))
        H[0, 0] = 1.0
        H[1, 1] = 1.0

        z = np.array([[px_meas], [py_meas]], dtype=float)
        h = self.x[0:2, :]
        R = np.diag([r_pos, r_pos])

        y = z - h
        S = H @ self.P @ H.T + R
        d2 = float(y.T @ np.linalg.inv(S) @ y)  # Mahalanobis distance squared

        if d2 > self.cfg.gate_chi2_dof2:
            return False

        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.x[2, 0] = wrap_to_pi(float(self.x[2, 0]))
        self.P = (np.eye(5) - K @ H) @ self.P
        return True


# -----------------------------
# Minimal GNSS helpers (NMEA + local frame)
# -----------------------------
def nmea_degmin_to_deg(dm: float) -> float:
    """Convert ddmm.mmmm (or dddmm.mmmm) to decimal degrees."""
    deg = int(dm // 100)
    minutes = dm - 100 * deg
    return deg + minutes / 60.0


def lla_to_ecef(lat_deg: float, lon_deg: float, alt_m: float) -> np.ndarray:
    """WGS84 LLA -> ECEF (meters)."""
    a = 6378137.0
    f = 1.0 / 298.257223563
    e2 = f * (2.0 - f)

    lat = math.radians(lat_deg)
    lon = math.radians(lon_deg)
    s = math.sin(lat)
    N = a / math.sqrt(1.0 - e2 * s * s)

    x = (N + alt_m) * math.cos(lat) * math.cos(lon)
    y = (N + alt_m) * math.cos(lat) * math.sin(lon)
    z = (N * (1.0 - e2) + alt_m) * s
    return np.array([x, y, z], dtype=float)


def ecef_to_enu(ecef: np.ndarray, ref_lla: tuple[float, float, float]) -> np.ndarray:
    """ECEF -> ENU (meters) w.r.t. reference LLA."""
    lat0, lon0, alt0 = ref_lla
    ref = lla_to_ecef(lat0, lon0, alt0)
    dx = ecef - ref

    lat = math.radians(lat0)
    lon = math.radians(lon0)
    slat, clat = math.sin(lat), math.cos(lat)
    slon, clon = math.sin(lon), math.cos(lon)

    R = np.array([
        [-slon,        clon,       0.0],
        [-slat*clon,  -slat*slon,   clat],
        [ clat*clon,   clat*slon,   slat],
    ], dtype=float)
    return R @ dx


def parse_nmea_gga(line: str):
    """
    Parse $GPGGA/$GNGGA.
    Returns (lat_deg, lon_deg, alt_m, fix_quality) or None if parse fails.

    fix_quality: 0 invalid, 1 GPS, 2 DGPS, 4 RTK fixed, 5 RTK float, ...
    """
    if not line.startswith("$") or "GGA" not in line:
        return None
    parts = line.strip().split(",")
    if len(parts) < 10:
        return None

    try:
        lat_dm = float(parts[2]) if parts[2] else float("nan")
        lat_hemi = parts[3].strip()
        lon_dm = float(parts[4]) if parts[4] else float("nan")
        lon_hemi = parts[5].strip()
        fix_q = int(parts[6]) if parts[6] else 0
        alt = float(parts[9]) if parts[9] else 0.0
    except ValueError:
        return None

    lat = nmea_degmin_to_deg(lat_dm)
    lon = nmea_degmin_to_deg(lon_dm)
    if lat_hemi == "S":
        lat *= -1.0
    if lon_hemi == "W":
        lon *= -1.0
    return lat, lon, alt, fix_q


def rtk_quality_to_rpos(cfg: EkfConfig, fix_quality: int) -> float:
    """Map NMEA GGA fix quality to a position variance (m^2)."""
    # Common: 4=RTK fixed, 5=RTK float. Some receivers also report 2=DGPS.
    if fix_quality == 4:
        return cfg.r_gps_rtk
    if fix_quality == 5:
        return (0.2 ** 2)  # float: decimeter-ish
    if fix_quality == 2:
        return (0.8 ** 2)  # DGPS
    return cfg.r_gps_fix


# -----------------------------
# Demonstration (simulation)
# -----------------------------
def simulate():
    cfg = EkfConfig()
    ekf = EkfGpsFusion(cfg)
    ekf.set_state(px=0.0, py=0.0, yaw=0.0, v=1.0, bg=0.0)

    dt = 0.05
    T = 60.0
    n = int(T / dt)

    # True motion: mild turn
    v_true = 1.2
    omega_true = 0.07
    bg_true = 0.02  # rad/s gyro bias

    # Reference origin for ENU (choose any; here, fake small-area origin)
    ref_lla = (52.0, 4.0, 0.0)

    xs = []
    for k in range(n):
        t = k * dt

        # True state propagation
        if k == 0:
            px, py, yaw = 0.0, 0.0, 0.0
        else:
            px, py, yaw = xs[-1][1], xs[-1][2], xs[-1][3]
        yaw = wrap_to_pi(yaw + omega_true * dt)
        px = px + v_true * math.cos(yaw) * dt
        py = py + v_true * math.sin(yaw) * dt

        # Wheel and gyro measurements
        v_meas = v_true + np.random.normal(0.0, 0.05)
        omega_meas = omega_true + bg_true + np.random.normal(0.0, 0.01)

        ekf.predict(v_meas=v_meas, omega_meas=omega_meas, dt=dt)

        # GNSS update at 5 Hz
        accepted = None
        if k % int(0.2 / dt) == 0:
            # Simulate RTK "fixed" most of the time, occasionally degrade
            fix_q = 4 if (k % 200 != 0) else 5
            r_pos = rtk_quality_to_rpos(cfg, fix_q)

            # GNSS position measurement
            gnss_px = px + np.random.normal(0.0, math.sqrt(r_pos))
            gnss_py = py + np.random.normal(0.0, math.sqrt(r_pos))

            accepted = ekf.update_gnss(gnss_px, gnss_py, r_pos=r_pos)

        xs.append((t, px, py, yaw, ekf.x[0,0], ekf.x[1,0], ekf.x[2,0], accepted))

    # Print a small tail of results
    print("t, true_x, true_y, true_yaw, est_x, est_y, est_yaw, gnss_accepted")
    for row in xs[-10:]:
        print("{:6.2f} {:8.3f} {:8.3f} {:7.3f} {:8.3f} {:8.3f} {:7.3f} {}".format(*row))


if __name__ == "__main__":
    simulate()
      

Chapter18_Lesson1.cpp

// Chapter18_Lesson1.cpp
// Autonomous Mobile Robots (Control Engineering) — Chapter 18, Lesson 1
// GPS/RTK Integration in Navigation (didactic EKF fusion example)
//
// This C++17 example mirrors the Python EKF:
//   state x = [px, py, yaw, v, bg]^T
//   inputs: wheel speed v_meas, gyro yaw-rate omega_meas
//   meas  : GNSS/RTK position (px, py) in ENU
//
// Requires Eigen3. Build (example):
//   g++ -O2 -std=c++17 Chapter18_Lesson1.cpp -I /usr/include/eigen3 -o ekf_gnss

#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>

#include <Eigen/Dense>

static double wrap_to_pi(double a) {
  a = std::fmod(a + M_PI, 2.0 * M_PI);
  if (a < 0) a += 2.0 * M_PI;
  a -= M_PI;
  // map -pi to +pi
  if (a <= -M_PI) a += 2.0 * M_PI;
  return a;
}

struct EkfConfig {
  double q_xy  = 0.05;
  double q_yaw = 0.01;
  double q_v   = 0.5;
  double q_bg  = 1e-4;

  double r_v = 0.2 * 0.2;

  double r_gps_fix = 1.5 * 1.5;
  double r_gps_rtk = 0.02 * 0.02;

  // chi2(2) 0.99
  double gate_chi2_dof2 = 9.2103;
};

class EkfGpsFusion {
public:
  explicit EkfGpsFusion(EkfConfig cfg = EkfConfig()) : cfg_(cfg) {
    x_.setZero();
    P_.setZero();
    P_(0,0) = 10.0;
    P_(1,1) = 10.0;
    P_(2,2) = std::pow(20.0 * M_PI / 180.0, 2);
    P_(3,3) = 2.0 * 2.0;
    P_(4,4) = std::pow(5.0 * M_PI / 180.0, 2);
  }

  void set_state(double px, double py, double yaw, double v, double bg) {
    x_ << px, py, yaw, v, bg;
  }

  void predict(double v_meas, double omega_meas, double dt) {
    if (dt <= 0.0) return;
    const double px = x_(0), py = x_(1), yaw = x_(2), v = x_(3), bg = x_(4);
    const double omega = omega_meas - bg;

    const double v_pred = v; // random walk
    const double yaw_pred = wrap_to_pi(yaw + omega * dt);
    const double px_pred = px + v_pred * std::cos(yaw_pred) * dt;
    const double py_pred = py + v_pred * std::sin(yaw_pred) * dt;

    Eigen::Matrix<double,5,5> F = Eigen::Matrix<double,5,5>::Identity();
    F(0,2) = -v_pred * std::sin(yaw_pred) * dt;
    F(0,3) =  std::cos(yaw_pred) * dt;
    F(1,2) =  v_pred * std::cos(yaw_pred) * dt;
    F(1,3) =  std::sin(yaw_pred) * dt;
    F(2,4) = -dt;

    Eigen::Matrix<double,5,5> Q = Eigen::Matrix<double,5,5>::Zero();
    Q(0,0) = cfg_.q_xy  * dt;
    Q(1,1) = cfg_.q_xy  * dt;
    Q(2,2) = cfg_.q_yaw * dt;
    Q(3,3) = cfg_.q_v   * dt;
    Q(4,4) = cfg_.q_bg  * dt;

    x_ << px_pred, py_pred, yaw_pred, v_pred, bg;
    P_ = F * P_ * F.transpose() + Q;

    update_speed(v_meas);
  }

  void update_speed(double v_meas) {
    Eigen::Matrix<double,1,5> H; H.setZero();
    H(0,3) = 1.0;
    const double z = v_meas;
    const double h = x_(3);
    const double y = z - h;

    Eigen::Matrix<double,1,1> S = H * P_ * H.transpose();
    S(0,0) += cfg_.r_v;

    Eigen::Matrix<double,5,1> K = P_ * H.transpose() * S.inverse();
    x_ = x_ + K * y;
    P_ = (Eigen::Matrix<double,5,5>::Identity() - K * H) * P_;
  }

  bool update_gnss(double px_meas, double py_meas, double r_pos) {
    Eigen::Matrix<double,2,5> H; H.setZero();
    H(0,0) = 1.0;
    H(1,1) = 1.0;

    Eigen::Matrix<double,2,1> z;
    z << px_meas, py_meas;

    Eigen::Matrix<double,2,1> h;
    h << x_(0), x_(1);

    Eigen::Matrix<double,2,2> R = Eigen::Matrix<double,2,2>::Zero();
    R(0,0) = r_pos; R(1,1) = r_pos;

    Eigen::Matrix<double,2,1> innov = z - h;
    Eigen::Matrix<double,2,2> S = H * P_ * H.transpose() + R;

    const double d2 = (innov.transpose() * S.inverse() * innov)(0,0);
    if (d2 > cfg_.gate_chi2_dof2) return false;

    Eigen::Matrix<double,5,2> K = P_ * H.transpose() * S.inverse();
    x_ = x_ + K * innov;
    x_(2) = wrap_to_pi(x_(2));
    P_ = (Eigen::Matrix<double,5,5>::Identity() - K * H) * P_;
    return true;
  }

  const Eigen::Matrix<double,5,1>& x() const { return x_; }

private:
  EkfConfig cfg_;
  Eigen::Matrix<double,5,1> x_;
  Eigen::Matrix<double,5,5> P_;
};

static double rtk_quality_to_rpos(int fix_q, const EkfConfig& cfg) {
  if (fix_q == 4) return cfg.r_gps_rtk;     // RTK fixed
  if (fix_q == 5) return 0.2 * 0.2;         // RTK float
  if (fix_q == 2) return 0.8 * 0.8;         // DGPS
  return cfg.r_gps_fix;                     // autonomous GNSS
}

int main() {
  EkfConfig cfg;
  EkfGpsFusion ekf(cfg);
  ekf.set_state(0.0, 0.0, 0.0, 1.0, 0.0);

  const double dt = 0.05;
  const double T = 60.0;
  const int n = static_cast<int>(T / dt);

  // Truth
  const double v_true = 1.2;
  const double omega_true = 0.07;
  const double bg_true = 0.02;

  std::mt19937 rng(42);
  std::normal_distribution<double> n_v(0.0, 0.05);
  std::normal_distribution<double> n_omega(0.0, 0.01);

  double px = 0.0, py = 0.0, yaw = 0.0;

  std::cout << "t,true_x,true_y,true_yaw,est_x,est_y,est_yaw,gnss_ok\n";

  for (int k = 0; k < n; ++k) {
    const double t = k * dt;

    // Truth update
    yaw = wrap_to_pi(yaw + omega_true * dt);
    px += v_true * std::cos(yaw) * dt;
    py += v_true * std::sin(yaw) * dt;

    // Measurements
    const double v_meas = v_true + n_v(rng);
    const double omega_meas = omega_true + bg_true + n_omega(rng);

    ekf.predict(v_meas, omega_meas, dt);

    bool gnss_ok = false;
    if (k % static_cast<int>(0.2 / dt) == 0) {
      const int fix_q = (k % 200 != 0) ? 4 : 5;
      const double r_pos = rtk_quality_to_rpos(fix_q, cfg);

      std::normal_distribution<double> n_g(0.0, std::sqrt(r_pos));
      const double gnss_px = px + n_g(rng);
      const double gnss_py = py + n_g(rng);

      gnss_ok = ekf.update_gnss(gnss_px, gnss_py, r_pos);
    }

    const auto& xhat = ekf.x();
    std::cout << t << "," << px << "," << py << "," << yaw << ","
              << xhat(0) << "," << xhat(1) << "," << xhat(2) << ","
              << (gnss_ok ? 1 : 0) << "\n";
  }
  return 0;
}
      

Chapter18_Lesson1.java

// Chapter18_Lesson1.java
// Autonomous Mobile Robots (Control Engineering) — Chapter 18, Lesson 1
// GPS/RTK Integration in Navigation (didactic EKF fusion example)
//
// This Java example implements a compact EKF with state:
//   x = [px, py, yaw, bg]^T
// where bg is gyro bias. Wheel speed is treated as a known input.
//
// GNSS/RTK provides (px, py) in ENU.
//
// Build:
//   javac Chapter18_Lesson1.java
// Run:
//   java Chapter18_Lesson1

import java.util.Random;

public class Chapter18_Lesson1 {

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

  static class Mat {
    // Small utilities for fixed sizes in this example (4x4, 4x2, 2x2).
    static double[][] eye(int n) {
      double[][] I = new double[n][n];
      for (int i = 0; i < n; i++) I[i][i] = 1.0;
      return I;
    }

    static double[][] add(double[][] A, double[][] B) {
      int r = A.length, c = A[0].length;
      double[][] C = new double[r][c];
      for (int i = 0; i < r; i++)
        for (int j = 0; j < c; j++)
          C[i][j] = A[i][j] + B[i][j];
      return C;
    }

    static double[][] sub(double[][] A, double[][] B) {
      int r = A.length, c = A[0].length;
      double[][] C = new double[r][c];
      for (int i = 0; i < r; i++)
        for (int j = 0; j < c; j++)
          C[i][j] = A[i][j] - B[i][j];
      return C;
    }

    static double[][] mul(double[][] A, double[][] B) {
      int r = A.length, k = A[0].length, c = B[0].length;
      double[][] C = new double[r][c];
      for (int i = 0; i < r; i++) {
        for (int j = 0; j < c; j++) {
          double s = 0.0;
          for (int t = 0; t < k; t++) s += A[i][t] * B[t][j];
          C[i][j] = s;
        }
      }
      return C;
    }

    static double[][] transpose(double[][] A) {
      int r = A.length, c = A[0].length;
      double[][] T = new double[c][r];
      for (int i = 0; i < r; i++)
        for (int j = 0; j < c; j++)
          T[j][i] = A[i][j];
      return T;
    }

    static double[][] inv2(double[][] A) {
      // Inverse of 2x2
      double a = A[0][0], b = A[0][1], c = A[1][0], d = A[1][1];
      double det = a*d - b*c;
      double[][] X = new double[2][2];
      X[0][0] =  d / det;
      X[0][1] = -b / det;
      X[1][0] = -c / det;
      X[1][1] =  a / det;
      return X;
    }

    static double dot(double[] a, double[] b) {
      double s = 0.0;
      for (int i = 0; i < a.length; i++) s += a[i]*b[i];
      return s;
    }

    static double[] mv(double[][] A, double[] x) {
      int r = A.length, c = A[0].length;
      double[] y = new double[r];
      for (int i = 0; i < r; i++) {
        double s = 0.0;
        for (int j = 0; j < c; j++) s += A[i][j]*x[j];
        y[i] = s;
      }
      return y;
    }
  }

  static class EkfConfig {
    double q_xy  = 0.05;
    double q_yaw = 0.01;
    double q_bg  = 1e-4;

    double r_gps_fix = 1.5*1.5;
    double r_gps_rtk = 0.02*0.02;

    // chi2(2) 0.99
    double gateChi2Dof2 = 9.2103;
  }

  static class EkfGps {
    // x = [px, py, yaw, bg]
    double[] x = new double[4];
    double[][] P = new double[4][4];
    EkfConfig cfg;

    EkfGps(EkfConfig cfg) {
      this.cfg = cfg;
      // init covariance
      P[0][0] = 10.0;
      P[1][1] = 10.0;
      P[2][2] = Math.pow(20.0*Math.PI/180.0, 2);
      P[3][3] = Math.pow(5.0*Math.PI/180.0, 2);
    }

    void setState(double px, double py, double yaw, double bg) {
      x[0] = px; x[1] = py; x[2] = yaw; x[3] = bg;
    }

    void predict(double v_meas, double omega_meas, double dt) {
      double px = x[0], py = x[1], yaw = x[2], bg = x[3];
      double omega = omega_meas - bg;

      double yaw_p = wrapToPi(yaw + omega * dt);
      double px_p = px + v_meas * Math.cos(yaw_p) * dt;
      double py_p = py + v_meas * Math.sin(yaw_p) * dt;

      // F = df/dx (4x4)
      double[][] F = Mat.eye(4);
      F[0][2] = -v_meas * Math.sin(yaw_p) * dt;
      F[1][2] =  v_meas * Math.cos(yaw_p) * dt;
      F[2][3] = -dt;

      // Q (4x4)
      double[][] Q = new double[4][4];
      Q[0][0] = cfg.q_xy * dt;
      Q[1][1] = cfg.q_xy * dt;
      Q[2][2] = cfg.q_yaw * dt;
      Q[3][3] = cfg.q_bg * dt;

      x[0] = px_p; x[1] = py_p; x[2] = yaw_p; x[3] = bg;
      P = Mat.add(Mat.mul(Mat.mul(F, P), Mat.transpose(F)), Q);
    }

    boolean updateGnss(double px_meas, double py_meas, double r_pos) {
      // z = [px, py]
      double[] z = new double[] {px_meas, py_meas};
      double[] h = new double[] {x[0], x[1]};
      double[] innov = new double[] {z[0]-h[0], z[1]-h[1]};

      // H (2x4)
      double[][] H = new double[2][4];
      H[0][0] = 1.0;
      H[1][1] = 1.0;

      // S = H P H' + R
      double[][] R = new double[][] { {r_pos, 0.0},{0.0, r_pos} };
      double[][] S = Mat.add(Mat.mul(Mat.mul(H, P), Mat.transpose(H)), R);
      double[][] Sinv = Mat.inv2(S);

      // d2 = innov' Sinv innov
      double[] Sinv_innov = Mat.mv(Sinv, innov);
      double d2 = Mat.dot(innov, Sinv_innov);
      if (d2 > cfg.gateChi2Dof2) return false;

      // K = P H' S^-1  => (4x2)
      double[][] K = Mat.mul(Mat.mul(P, Mat.transpose(H)), Sinv);

      // x = x + K innov
      x[0] += K[0][0]*innov[0] + K[0][1]*innov[1];
      x[1] += K[1][0]*innov[0] + K[1][1]*innov[1];
      x[2] = wrapToPi(x[2] + K[2][0]*innov[0] + K[2][1]*innov[1]);
      x[3] += K[3][0]*innov[0] + K[3][1]*innov[1];

      // P = (I - K H) P
      double[][] I = Mat.eye(4);
      double[][] KH = Mat.mul(K, H);
      P = Mat.mul(Mat.sub(I, KH), P);
      return true;
    }
  }

  static double rtkQualityToRpos(EkfConfig cfg, int fixQ) {
    if (fixQ == 4) return cfg.r_gps_rtk;     // RTK fixed
    if (fixQ == 5) return 0.2*0.2;           // float
    if (fixQ == 2) return 0.8*0.8;           // DGPS
    return cfg.r_gps_fix;
  }

  public static void main(String[] args) {
    EkfConfig cfg = new EkfConfig();
    EkfGps ekf = new EkfGps(cfg);
    ekf.setState(0.0, 0.0, 0.0, 0.0);

    double dt = 0.05;
    double T = 60.0;
    int n = (int)(T/dt);

    double vTrue = 1.2;
    double omegaTrue = 0.07;
    double bgTrue = 0.02;

    Random rng = new Random(42);

    double px = 0.0, py = 0.0, yaw = 0.0;

    System.out.println("t,true_x,true_y,true_yaw,est_x,est_y,est_yaw,gnss_ok");
    for (int k = 0; k < n; k++) {
      double t = k*dt;

      yaw = wrapToPi(yaw + omegaTrue * dt);
      px += vTrue * Math.cos(yaw) * dt;
      py += vTrue * Math.sin(yaw) * dt;

      double vMeas = vTrue + rng.nextGaussian()*0.05;
      double omegaMeas = omegaTrue + bgTrue + rng.nextGaussian()*0.01;

      ekf.predict(vMeas, omegaMeas, dt);

      boolean ok = false;
      if (k % (int)(0.2/dt) == 0) {
        int fixQ = (k % 200 != 0) ? 4 : 5;
        double rPos = rtkQualityToRpos(cfg, fixQ);
        double gnssPx = px + rng.nextGaussian()*Math.sqrt(rPos);
        double gnssPy = py + rng.nextGaussian()*Math.sqrt(rPos);
        ok = ekf.updateGnss(gnssPx, gnssPy, rPos);
      }

      System.out.printf("%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d%n",
        t, px, py, yaw, ekf.x[0], ekf.x[1], ekf.x[2], ok ? 1 : 0);
    }
  }
}
      

Chapter18_Lesson1.m

% Chapter18_Lesson1.m
% Autonomous Mobile Robots (Control Engineering) — Chapter 18, Lesson 1
% GPS/RTK Integration in Navigation (didactic EKF fusion example)
%
% This MATLAB script implements an EKF that fuses wheel speed, gyro yaw-rate
% (with a bias state), and GNSS/RTK position in a local ENU frame.
%
% State: x = [px; py; yaw; v; bg]
% Inputs: v_meas, omega_meas
% Measurement: z_gnss = [px; py]
%
% NOTE: This is a teaching implementation; for production consider MATLAB
% Navigation Toolbox / Sensor Fusion and Tracking Toolbox.

clear; clc;

% -----------------------------
% Configuration
% -----------------------------
cfg.q_xy  = 0.05;
cfg.q_yaw = 0.01;
cfg.q_v   = 0.5;
cfg.q_bg  = 1e-4;

cfg.r_v = 0.2^2;

cfg.r_gps_fix = 1.5^2;
cfg.r_gps_rtk = 0.02^2;

cfg.gate_chi2_dof2 = 9.2103; % chi2inv(0.99,2)

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

% -----------------------------
% EKF initialization
% -----------------------------
x = [0; 0; 0; 1.0; 0]; % [px, py, yaw, v, bg]
P = diag([10, 10, (20*pi/180)^2, 2^2, (5*pi/180)^2]);

% -----------------------------
% Simulation setup (same as Python/C++)
% -----------------------------
dt = 0.05; T = 60; n = floor(T/dt);

v_true = 1.2;
omega_true = 0.07;
bg_true = 0.02;

px = 0; py = 0; yaw = 0;

rng(42);

log = zeros(n, 8); % t, true_x, true_y, true_yaw, est_x, est_y, est_yaw, gnss_ok

for k = 1:n
    t = (k-1)*dt;

    % Truth
    yaw = wrapToPi(yaw + omega_true*dt);
    px = px + v_true*cos(yaw)*dt;
    py = py + v_true*sin(yaw)*dt;

    % Measurements
    v_meas = v_true + 0.05*randn();
    omega_meas = omega_true + bg_true + 0.01*randn();

    % -----------------------------
    % Predict
    % -----------------------------
    omega = omega_meas - x(5);
    x_pred = x;
    x_pred(3) = wrapToPi(x(3) + omega*dt);
    x_pred(1) = x(1) + x(4)*cos(x_pred(3))*dt;
    x_pred(2) = x(2) + x(4)*sin(x_pred(3))*dt;
    % v random-walk, bg random-walk: unchanged

    F = eye(5);
    F(1,3) = -x(4)*sin(x_pred(3))*dt;
    F(1,4) =  cos(x_pred(3))*dt;
    F(2,3) =  x(4)*cos(x_pred(3))*dt;
    F(2,4) =  sin(x_pred(3))*dt;
    F(3,5) = -dt;

    Q = diag([cfg.q_xy*dt, cfg.q_xy*dt, cfg.q_yaw*dt, cfg.q_v*dt, cfg.q_bg*dt]);

    x = x_pred;
    P = F*P*F' + Q;

    % Optional: wheel speed pseudo-measurement update for v
    H_v = zeros(1,5); H_v(4) = 1;
    z_v = v_meas; h_v = x(4);
    y_v = z_v - h_v;
    S_v = H_v*P*H_v' + cfg.r_v;
    K_v = P*H_v'/S_v;
    x = x + K_v*y_v;
    P = (eye(5) - K_v*H_v)*P;

    % -----------------------------
    % GNSS update at 5 Hz
    % -----------------------------
    gnss_ok = 0;
    if mod(k-1, round(0.2/dt)) == 0
        fix_q = 4;
        if mod(k-1, 200) == 0
            fix_q = 5;
        end
        if fix_q == 4
            r_pos = cfg.r_gps_rtk;
        else
            r_pos = 0.2^2;
        end

        z = [px; py] + sqrt(r_pos)*randn(2,1);
        H = [1 0 0 0 0;
             0 1 0 0 0];
        h = x(1:2);
        R = diag([r_pos, r_pos]);

        innov = z - h;
        S = H*P*H' + R;
        d2 = innov'*(S\innov);
        if d2 <= cfg.gate_chi2_dof2
            K = P*H'/S;
            x = x + K*innov;
            x(3) = wrapToPi(x(3));
            P = (eye(5) - K*H)*P;
            gnss_ok = 1;
        end
    end

    log(k,:) = [t, px, py, yaw, x(1), x(2), x(3), gnss_ok];
end

disp('t true_x true_y true_yaw est_x est_y est_yaw gnss_ok (tail)');
disp(log(end-9:end,:));

% -----------------------------
% Simulink hint (programmatic skeleton)
% -----------------------------
% You can construct a simple fusion diagram with:
%  - "Discrete-Time Integrator" for state propagation
%  - "MATLAB Function" block for EKF step (predict+update)
%  - "From Workspace" for simulated wheel/gyro/GNSS streams
% Example (minimal):
%   mdl = 'Chapter18_Lesson1_EKF';
%   new_system(mdl); open_system(mdl);
%   add_block('simulink/Sources/From Workspace',[mdl '/wheel_v']);
%   add_block('simulink/Sources/From Workspace',[mdl '/gyro_w']);
%   add_block('simulink/Sources/From Workspace',[mdl '/gnss_xy']);
%   add_block('simulink/User-Defined Functions/MATLAB Function',[mdl '/EKF']);
% Then wire the blocks and implement the EKF equations inside the MATLAB Function.
      

Chapter18_Lesson1.nb

(* Chapter18_Lesson1.nb
   Autonomous Mobile Robots (Control Engineering) — Chapter 18, Lesson 1
   GPS/RTK Integration in Navigation

   This is a minimal Wolfram Notebook expression (plain-text .nb) containing
   Wolfram Language code cells. You can open it in Mathematica and evaluate.

   Topics:
     1) Weighted least squares (WLS) position update in 2D
     2) EKF-like update step (linearized)
     3) Tiny integer ambiguity toy: closest-integer search after decorrelation (conceptual)
*)

Notebook[{

 Cell["Chapter 18 — Lesson 1: GPS/RTK Integration in Navigation", "Title"],

 Cell["1) Weighted Least Squares (2D position) — linear measurement model", "Section"],

 Cell[BoxData@ToBoxes[
  Module[{H, z, R, x0, dx},
    (* Suppose z = H x + v, v~N(0,R), solve argmin ||R^{-1/2}(z - H x)||^2 *)
    H = { {1, 0}, {0, 1}, {1, 0}, {0, 1} };           (* four pseudo-measurements *)
    z = {10.2, -3.9, 9.8, -4.1};                    (* meters *)
    R = DiagonalMatrix[{0.04, 0.04, 0.25, 0.25}];   (* variances *)
    x0 = {0, 0};

    dx = Inverse[Transpose[H].Inverse[R].H].Transpose[H].Inverse[R].(z - H.x0);
    {xHat = x0 + dx, cov = Inverse[Transpose[H].Inverse[R].H]}
  ]
 ], "Input"],

 Cell["2) EKF measurement update for GNSS position z=[px,py]^T", "Section"],

 Cell[BoxData@ToBoxes[
  Module[{x, P, z, h, H, R, S, K, xNew, PNew},
    (* State x = [px, py, yaw, v, bg] *)
    x = {1.0, 2.0, 0.3, 1.2, 0.01};
    P = DiagonalMatrix[{1, 1, 0.05, 0.5, 1*^-4}];
    z = {0.95, 2.06};

    h = {x[[1]], x[[2]]};
    H = { {1, 0, 0, 0, 0},
         {0, 1, 0, 0, 0} };
    R = DiagonalMatrix[{0.02^2, 0.02^2}];

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

    xNew = x + K.(z - h);
    PNew = (IdentityMatrix[5] - K.H).P;

    {xNew, PNew}
  ]
 ], "Input"],

 Cell["3) Integer ambiguity toy (conceptual): nearest integer after decorrelation", "Section"],

 Cell[TextData[{
  "In RTK, carrier-phase double differences lead to an integer vector N. ",
  "A simplified ILS problem is: minimize (N - Nfloat)^T Q^{-1} (N - Nfloat) over integer N."
 }], "Text"],

 Cell[BoxData@ToBoxes[
  Module[{Nfloat, Q, L, D, Z, y, yInt, Nint},
    Nfloat = {12.37, -5.81, 3.22};
    Q = { {2.0, 0.6, 0.1},
         {0.6, 1.5, 0.2},
         {0.1, 0.2, 0.8} };

    (* Toy decorrelation via Cholesky; L.L^T = Q *)
    L = CholeskyDecomposition[Q];
    (* Transform: y = L^{-1} N so covariance ~ I (toy) *)
    y = LinearSolve[L, Nfloat];
    yInt = Round[y];
    Nint = L.yInt;

    {Nfloat, "decorrelated y" -> y, "rounded yInt" -> yInt, "mapped back Nint" -> Nint}
  ]
 ], "Input"],

 Cell["Note: Real RTK uses integer-preserving transforms (Z-transforms) and efficient search (e.g., LAMBDA).", "Text"]

}]
      

Problem 1 (Double-difference clock cancellation): Using the carrier-phase model \( \Phi_s = \rho_s(\mathbf{p}) + c(\delta t_r - \delta t_s) + T_s - I_s + \lambda N_s + \epsilon_s \), show that the double difference across receivers (rover/base) and satellites (s and s0) cancels both receiver and satellite clock biases.

Solution:

First take the single difference rover-base: \( \Delta \Phi_s^{rb} = \Phi_s^r - \Phi_s^b \). The term \( -c\delta t_s \) is common to both receivers, so it cancels: \( -c\delta t_s + c\delta t_s = 0 \). The remaining receiver clocks appear only as \( c(\delta t_r^r-\delta t_r^b) \). Now take a second difference across satellites s and s0: \( \nabla\Delta \Phi_{s,s_0}^{rb} = \Delta \Phi_s^{rb}-\Delta \Phi_{s_0}^{rb} \). Because \( c(\delta t_r^r-\delta t_r^b) \) does not depend on satellite index, it cancels too. Therefore clock biases vanish in the double-difference observable.

Problem 2 (EKF Jacobian for GNSS position update): For state \( \mathbf{x} = [p_x, p_y, \psi, v, b_g]^\top \) and measurement \( \mathbf{z} = [p_x, p_y]^\top + \mathbf{v} \), derive \( \mathbf{H} = \frac{\partial \mathbf{h} }{\partial \mathbf{x} } \) and write the innovation covariance \( \mathbf{S} \).

Solution:

Here \( \mathbf{h}(\mathbf{x}) = [p_x, p_y]^\top \), so \( \frac{\partial h_1}{\partial p_x}=1 \), \( \frac{\partial h_2}{\partial p_y}=1 \), and all other partial derivatives are zero. Hence:

\[ \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix}, \qquad \mathbf{S}=\mathbf{H}\mathbf{P}\mathbf{H}^\top + \mathbf{R}. \]

Problem 3 (Chi-square gating probability): Suppose the GNSS innovation is Gaussian: \( \nu \sim \mathcal{N}(\mathbf{0},\mathbf{S}) \) with \( m=2 \) dimensions. Show that \( d^2 = \nu^\top \mathbf{S}^{-1}\nu \) has a chi-square distribution with 2 DoF, and explain how to choose a gate threshold for a false-rejection rate \( \alpha \).

Solution:

Let \( \mathbf{S}=\mathbf{L}\mathbf{L}^\top \) be a Cholesky factorization and define \( \mathbf{w}=\mathbf{L}^{-1}\nu \). Then \( \mathbf{w} \sim \mathcal{N}(\mathbf{0},\mathbf{I}) \) and \( d^2 = \nu^\top \mathbf{S}^{-1}\nu = \mathbf{w}^\top \mathbf{w} = \sum_{i=1}^m w_i^2 \), which is by definition \( \chi^2_m \). Choose the gate \( \gamma = \chi^2_m(1-\alpha) \) so that \( \mathbb{P}(d^2 > \gamma)=\alpha \).

Problem 4 (Downweighting vs rejecting): You observe that GNSS updates are sometimes biased (multipath) rather than pure outliers. Propose a robust alternative to hard gating and write the corresponding weighted least-squares objective.

Solution:

Instead of rejecting when \( d^2 \) is large, use a robust loss \( \rho(\cdot) \) on residuals. In a batch view, solve:

\[ \hat{\mathbf{x} } = \arg\min_{\mathbf{x} }\; \rho\!\Big(\|\mathbf{R}^{-1/2}(\mathbf{z}-\mathbf{h}(\mathbf{x}))\|^2\Big). \]

For example, Huber loss behaves quadratically for small residuals and linearly for large residuals, effectively downweighting biased GNSS points while still extracting some information.

flowchart TD
  Z["New GNSS fix"] --> Q["Compute R from status (RTK fix/float/GNSS)"]
  Q --> I["Innovation nu = z - h(x)"]
  I --> D["Compute d2 = nu' S^-1 nu"]
  D --> G1["If d2 <= gate: accept update"]
  D --> G2["If d2 > gate: robust option"]
  G2 --> W["Downweight: inflate R or apply robust loss"]
  W --> U["Update state and covariance"]
  G1 --> U
        

We built a field-ready perspective on GNSS and RTK for outdoor AMR: (i) pseudorange and carrier-phase models, (ii) single- and double-difference observables that cancel clock biases, (iii) integer ambiguity resolution as an ILS problem, (iv) coordinate conversion into a local ENU frame, and (v) loosely coupled EKF fusion with integrity monitoring via chi-square innovation gating or robust downweighting. These tools provide a drift-bounding absolute reference that complements the local motion generation and SLAM methods developed earlier in the course.