By the end of this lab you should be able to:

• Formulate a discrete-time stochastic model for planar motion with IMU-driven propagation and wheel/GPS updates.
• Derive EKF Jacobians for the specific AMR model and implement a numerically safe covariance update (Joseph form).
• Implement a UKF with correct sigma-point construction, nonlinear propagation, and angle-aware averaging.
• Handle multi-rate sensor updates (IMU high-rate; wheel medium-rate; GPS low-rate) in a single filter loop.
• Evaluate estimation accuracy (RMSE) and basic statistical consistency (innovation gating, NIS/NEES concepts).

Recommended software stacks and robotics libraries (you will still implement the core filter “from scratch”):

• \( \text{Python} \): NumPy, Matplotlib; (optional) ROS2 rclpy; (reference) filterpy.
• \( \text{C++} \): Eigen; (reference) ROS2 robot_localization.
• \( \text{Java} \): EJML; (reference) ROSJava bindings.
• \( \text{MATLAB/Simulink} \): base MATLAB; (optional) Sensor Fusion and Tracking Toolbox; Simulink MATLAB Function blocks.
• \( \text{Wolfram Mathematica} \): Linear algebra primitives; custom EKF/UKF routines.

The lab implements a common real-world arrangement:

• IMU provides high-rate inertial increments (gyro yaw-rate and longitudinal acceleration).
• Wheel odometry provides medium-rate kinematic information (forward speed and yaw-rate from differential wheel speeds).
• GPS provides low-rate absolute position fixes (2D in a local planar frame).

flowchart TD
  A["Start: x0, P0"] --> B["IMU tick (high-rate)"]
  B --> C["Predict with IMU: propagate x, P"]
  C --> D{"Wheel msg available?"}
  D -->|yes| E["Update with wheel: z=[v_w, omega_w]"]
  D -->|no| F{"GPS msg available?"}
  E --> F
  F -->|yes| G["Update with GPS: z=[x_gps, y_gps]"]
  F -->|no| H["Log estimate"]
  G --> H
  H --> B
        

The key engineering requirement is that the update functions accept measurements whenever they arrive, while the filter state is always propagated to the correct timestamp using the IMU stream. In this lab we assume time alignment is already handled (single timeline), and focus on estimation math.

We use a planar state vector with IMU bias terms: \( \mathbf{x}_k = [x_k, y_k, \theta_k, v_k, b_{g,k}, b_{a,k}]^\top \), where \( (x,y) \) is position in a local world frame, \( \theta \) is heading, \( v \) is forward speed, and \( b_g, b_a \) are gyro and longitudinal accelerometer biases.

IMU measurements (treated as known inputs to the process model) are: \( \omega_{m,k} \) (yaw-rate) and \( a_{m,k} \) (longitudinal acceleration). Bias-corrected quantities are: \( \omega_k = \omega_{m,k} - b_{g,k} \), \( a_k = a_{m,k} - b_{a,k} \).

Using forward Euler discretization with timestep \( \Delta t \):

\[ \begin{aligned} x_{k+1} &= x_k + v_k \Delta t \cos(\theta_k) \\ y_{k+1} &= y_k + v_k \Delta t \sin(\theta_k) \\ \theta_{k+1} &= \mathrm{wrap}\!\left(\theta_k + (\omega_{m,k} - b_{g,k})\Delta t \right) \\ v_{k+1} &= v_k + (a_{m,k} - b_{a,k})\Delta t \\ b_{g,k+1} &= b_{g,k} + w_{bg,k} \\ b_{a,k+1} &= b_{a,k} + w_{ba,k} \end{aligned} \]

The noise terms implement the standard decomposition: white IMU noise contributes to \( \theta \) and \( v \), while bias states follow random walks. Define a noise vector \( \mathbf{w}_k = [n_{g,k}, n_{a,k}, w_{bg,k}, w_{ba,k}]^\top \) with covariance \( \mathbf{Q}_c = \mathrm{diag}(\sigma_g^2, \sigma_a^2, \sigma_{bg}^2, \sigma_{ba}^2) \).

A simple discrete-time approximation uses a noise injection matrix \( \mathbf{G}_k \) such that \( \mathbf{Q}_d \approx \mathbf{G}_k \mathbf{Q}_c \mathbf{G}_k^\top \):

\[ \mathbf{G}_k = \begin{bmatrix} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ \Delta t & 0 & 0 & 0\\ 0 & \Delta t & 0 & 0\\ 0 & 0 & \sqrt{\Delta t} & 0\\ 0 & 0 & 0 & \sqrt{\Delta t} \end{bmatrix}, \qquad \mathbf{Q}_d = \mathbf{G}_k \mathbf{Q}_c \mathbf{G}_k^\top. \]

Measurement models:

Wheel odometry update uses measurements \( \mathbf{z}^{(w)}_k = [v_{w,k}, \omega_{w,k}]^\top \). We model wheel speed as direct observation of \( v \), and wheel yaw-rate as observation of IMU yaw-rate with bias:

\[ \mathbf{z}^{(w)}_k = \underbrace{ \begin{bmatrix} v_k\\ \omega_{m,k} - b_{g,k} \end{bmatrix} }_{\mathbf{h}_w(\mathbf{x}_k,\omega_{m,k})} + \mathbf{n}^{(w)}_k, \qquad \mathbf{n}^{(w)}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_w). \]

GPS update uses measurements \( \mathbf{z}^{(g)}_k = [x_{gps,k}, y_{gps,k}]^\top \):

\[ \mathbf{z}^{(g)}_k = \underbrace{ \begin{bmatrix} x_k\\ y_k \end{bmatrix} }_{\mathbf{h}_g(\mathbf{x}_k)} + \mathbf{n}^{(g)}_k, \qquad \mathbf{n}^{(g)}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_g). \]

The EKF applies the Kalman filter equations to the first-order linearization of the nonlinear system around the current estimate. Define the nonlinear propagation function \( \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k,\mathbf{u}_k) + \boldsymbol{\eta}_k \), where \( \mathbf{u}_k = [\omega_{m,k}, a_{m,k}]^\top \).

The EKF prediction uses the Jacobian \( \mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_k} \):

\[ \mathbf{F}_k = \begin{bmatrix} 1 & 0 & -v_k\Delta t \sin(\theta_k) & \Delta t\cos(\theta_k) & 0 & 0\\ 0 & 1 & \phantom{-}v_k\Delta t \cos(\theta_k) & \Delta t\sin(\theta_k) & 0 & 0\\ 0 & 0 & 1 & 0 & -\Delta t & 0\\ 0 & 0 & 0 & 1 & 0 & -\Delta t\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}. \]

Prediction step:

\[ \hat{\mathbf{x}}_{k+1|k} = \mathbf{f}(\hat{\mathbf{x}}_{k|k},\mathbf{u}_k), \qquad \mathbf{P}_{k+1|k} = \mathbf{F}_k \mathbf{P}_{k|k}\mathbf{F}_k^\top + \mathbf{Q}_d. \]

For an update with measurement model \( \mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{n}_k \), define the measurement Jacobian \( \mathbf{H}_k = \frac{\partial \mathbf{h}}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_{k|k-1}} \). For wheel and GPS in this lab:

\[ \mathbf{H}_w = \begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & -1 & 0 \end{bmatrix}, \qquad \mathbf{H}_g = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix}. \]

EKF update (innovation form):

\[ \begin{aligned} \mathbf{y}_k &= \mathbf{z}_k - \mathbf{h}(\hat{\mathbf{x}}_{k|k-1})\\ \mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}\\ \mathbf{K}_k &= \mathbf{P}_{k|k-1}\mathbf{H}_k^\top \mathbf{S}_k^{-1}\\ \hat{\mathbf{x}}_{k|k} &\leftarrow \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k\mathbf{y}_k \end{aligned} \]

PSD-safe covariance update (Joseph form). To preserve symmetry and positive semidefiniteness under finite precision:

\[ \mathbf{P}_{k|k} \;=\; (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^\top + \mathbf{K}_k\mathbf{R}\mathbf{K}_k^\top. \]

Proof sketch (Joseph form is PSD). Let \( \mathbf{A} = (\mathbf{I}-\mathbf{K}\mathbf{H}) \). Then \( \mathbf{P}_{new} = \mathbf{A}\mathbf{P}\mathbf{A}^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \). If \( \mathbf{P}\succeq 0 \) and \( \mathbf{R}\succeq 0 \), both terms are PSD, hence their sum is PSD. This argument is algebraic and does not depend on the linearization details.

Interpretation as a local MAP step. For a (locally) linearized measurement model \( \mathbf{z} \approx \mathbf{h}(\hat{\mathbf{x}})+\mathbf{H}(\mathbf{x}-\hat{\mathbf{x}})+\mathbf{n} \) with Gaussian prior \( \mathbf{x} \sim \mathcal{N}(\hat{\mathbf{x}},\mathbf{P}) \), the posterior maximizer solves a weighted least-squares problem:

\[ \hat{\mathbf{x}}_{MAP} \;=\; \arg\min_{\mathbf{x}} \left\{ (\mathbf{x}-\hat{\mathbf{x}})^\top \mathbf{P}^{-1}(\mathbf{x}-\hat{\mathbf{x}}) + \left(\mathbf{z}-\mathbf{h}(\hat{\mathbf{x}})-\mathbf{H}(\mathbf{x}-\hat{\mathbf{x}})\right)^\top \mathbf{R}^{-1} \left(\mathbf{z}-\mathbf{h}(\hat{\mathbf{x}})- \\ \mathbf{H}(\mathbf{x}-\hat{\mathbf{x}})\right) \right\}, \]

whose normal equations yield the standard Kalman gain update. The EKF repeats this locally at each timestep.

The UKF replaces first-order linearization with the unscented transform: a deterministic set of sigma points is propagated through the nonlinear functions to match the posterior mean and covariance. For state dimension \( n \), define parameters \( \alpha, \beta, \kappa \) and \( \lambda = \alpha^2(n+\kappa)-n \).

Sigma points for \( \hat{\mathbf{x}} \) and \( \mathbf{P} \):

\[ \begin{aligned} \mathbf{X}^{(0)} &= \hat{\mathbf{x}}\\ \mathbf{X}^{(i)} &= \hat{\mathbf{x}} + \left[\sqrt{(n+\lambda)\mathbf{P}}\right]_i,\quad i=1,\dots,n\\ \mathbf{X}^{(i+n)} &= \hat{\mathbf{x}} - \left[\sqrt{(n+\lambda)\mathbf{P}}\right]_i,\quad i=1,\dots,n \end{aligned} \]

Weights:

\[ \begin{aligned} W^{(0)}_m &= \frac{\lambda}{n+\lambda}, \qquad W^{(0)}_c = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)\\ W^{(i)}_m &= W^{(i)}_c = \frac{1}{2(n+\lambda)}, \quad i=1,\dots,2n. \end{aligned} \]

Prediction (propagate sigma points through \( \mathbf{f} \)):

\[ \begin{aligned} \mathbf{X}^{(i)}_{k+1|k} &= \mathbf{f}(\mathbf{X}^{(i)}_{k|k},\mathbf{u}_k)\\ \hat{\mathbf{x}}_{k+1|k} &= \sum_{i=0}^{2n} W^{(i)}_m \mathbf{X}^{(i)}_{k+1|k}\\ \mathbf{P}_{k+1|k} &= \sum_{i=0}^{2n} W^{(i)}_c (\mathbf{X}^{(i)}_{k+1|k}-\hat{\mathbf{x}}_{k+1|k})(\cdot)^\top + \mathbf{Q}_d. \end{aligned} \]

Angle-aware mean and residuals. For heading \( \theta \), linear averaging fails near wrap boundaries. Use a circular mean:

\[ \bar{\theta} = \mathrm{atan2}\!\left(\sum_i W^{(i)}_m \sin(\theta^{(i)}),\; \sum_i W^{(i)}_m \cos(\theta^{(i)})\right), \quad \delta\theta^{(i)} = \mathrm{wrap}(\theta^{(i)} - \bar{\theta}). \]

Why UKF helps (formal approximation statement). For a nonlinear transform \( \mathbf{y}=\mathbf{g}(\mathbf{x}) \) with Gaussian \( \mathbf{x} \), the unscented transform matches mean/covariance to higher order than first-order linearization. A Taylor expansion argument shows the UKF captures second-order terms of \( \mathbf{g} \) in the moment approximation, whereas the EKF’s covariance propagation is fundamentally first-order in the nonlinearity (through \( \mathbf{H} \) / \( \mathbf{F} \)). Practically, this lab will show UKF advantage when turning dynamics are stronger and GPS updates are sparse.

flowchart TD
  S["Sigma points from (x,P)"] --> P1["Propagate through f(x,u)"]
  P1 --> M1["Mean/cov (angle-aware for theta)"]
  M1 --> U1{"Measurement arrives?"}
  U1 -->|wheel| W1["Transform sigma points via h_w"]
  U1 -->|gps| G1["Transform sigma points via h_g"]
  W1 --> K1["Compute S and Pxz, then K"]
  G1 --> K1
  K1 --> X1["Update x and P"]
  X1 --> S
        

The following items materially determine whether your filter is stable and meaningful:

• Initialization. Choose \( \hat{\mathbf{x}}_0 \) and \( \mathbf{P}_0 \) consistent with your real uncertainty. Underestimating \( \mathbf{P}_0 \) often yields inconsistency and divergence.
• Noise tuning. Ensure \( \mathbf{Q} \) reflects IMU noise and bias drift; ensure \( \mathbf{R}_g \) reflects GPS scatter in the chosen local frame; ensure \( \mathbf{R}_w \) reflects wheel slip and quantization.
• Angle normalization. After every prediction/update, wrap \( \theta \) via \( \mathrm{wrap}(\cdot) \).
• Innovation gating (optional). Use NIS to reject outliers (e.g., GPS multipath). For innovation \( \mathbf{y}_k \) and covariance \( \mathbf{S}_k \): \( \mathrm{NIS}_k = \mathbf{y}_k^\top \mathbf{S}_k^{-1}\mathbf{y}_k \). Accept if \( \mathrm{NIS}_k \le \chi^2_{m,1-\alpha} \) where \( m \) is measurement dimension.
• Logging. Log estimates, ground truth (if available), innovations, and covariances to diagnose inconsistency.

The Python implementation simulates a trajectory with biased IMU, noisy wheel odometry, and noisy GPS, then runs both EKF and UKF using the model in Sections 3–5.

Chapter7_Lesson5.py

# Chapter7_Lesson5.py
# EKF/UKF localization lab: Wheel + IMU + GPS (2D ground robot)
# Dependencies: numpy, matplotlib
# Optional: scipy (not required)

import math
import numpy as np
import matplotlib.pyplot as plt

np.random.seed(7)

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

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

def angle_residual(a: float, b: float) -> float:
    """Residual a - b for angles."""
    return wrap_angle(a - b)

# (Full file content is provided in the downloadable ZIP.)
      

Chapter7_Lesson5_Ex1.py

# Chapter7_Lesson5_Ex1.py
# Exercise: NIS gating for wheel/GPS updates + basic consistency checks
# Dependencies: numpy

import numpy as np
import math

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

# (Full file content is provided in the downloadable ZIP.)
      

Implementation notes:

• Wheel update uses \( \mathbf{h}_w(\mathbf{x},\omega_m)=[v,\omega_m-b_g]^\top \) to couple wheel and gyro for bias learning.
• UKF uses an angle-aware mean for \( \theta \) and wraps angular residuals in covariance accumulation.
• Plots include trajectory, heading, and speed comparisons; you can add innovation plots and gating statistics.

The C++ implementation mirrors the same simulation and filter structure, using Eigen for matrix operations. In production AMR stacks, you would typically integrate this logic via ROS2 and leverage robot_localization, but implementing it directly clarifies the mathematical mechanics.

Chapter7_Lesson5.cpp

// Chapter7_Lesson5.cpp
// EKF/UKF localization lab: Wheel + IMU + GPS (2D ground robot)
// Dependencies: Eigen (header-only for basic usage)
// Build (example):
//   g++ -O2 -std=c++17 Chapter7_Lesson5.cpp -I /usr/include/eigen3 -o ch7_l5

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

// (Full file content is provided in the downloadable ZIP.)
      

Engineering notes for C++:

• Prefer Cholesky (\( \mathbf{P} = \mathbf{L}\mathbf{L}^\top \)) for UKF sigma-point generation.
• Use the Joseph form in EKF updates to improve numerical stability.
• In ROS2, the same conceptual pipeline maps to callbacks that trigger predict/update operations based on message timestamps.

The Java implementation uses EJML for dense linear algebra. This is useful for Android/embedded JVM robotics prototypes or server-side simulation pipelines.

Chapter7_Lesson5.java

// Chapter7_Lesson5.java
// EKF/UKF localization lab: Wheel + IMU + GPS (2D ground robot)
// Dependencies: EJML (Efficient Java Matrix Library)

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

// (Full file content is provided in the downloadable ZIP.)
      

Practical notes:

• Angle wrapping must be applied after every state update.
• For UKF, ensure the Cholesky factorization is computed on \( (n+\lambda)\mathbf{P} \), not \( \mathbf{P} \).

MATLAB is convenient for rapid prototyping and comparison with toolbox implementations. The provided script runs both EKF and UKF and includes a minimal function to programmatically create a Simulink shell model (students can complete the block wiring using MATLAB Function blocks for predict/update).

Chapter7_Lesson5.m

% Chapter7_Lesson5.m
% EKF/UKF localization lab: Wheel + IMU + GPS (2D ground robot)
% Requires only base MATLAB. Optional: Sensor Fusion and Tracking Toolbox for comparison.

clear; clc; rng(7);

% (Full file content is provided in the downloadable ZIP.)
      

Suggested toolbox cross-check (optional): compare your EKF/UKF output against MATLAB’s filtering tools by constructing an equivalent nonlinear state-space model and feeding measurements at their sample times. Keep the “from scratch” implementation as the ground-truth learning artifact.

Mathematica is well-suited for symbolic Jacobian verification and rapid linear algebra experimentation. The provided notebook-style source defines the motion model, Jacobian, and EKF/UKF building blocks.

Chapter7_Lesson5.nb

(* Chapter7_Lesson5.nb
   Wolfram Mathematica notebook-style source (plain text).
   EKF/UKF localization lab: Wheel + IMU + GPS (2D ground robot)
*)

ClearAll["Global`*"];

(* (Full file content is provided in the downloadable ZIP.) *)
      

Recommended Mathematica workflow:

• Symbolically validate EKF Jacobians against finite-difference approximations.
• Numerically compare EKF and UKF outputs under increased turn rate and reduced GPS rate.

Problem 1 (EKF Jacobian Derivation): Starting from the discrete model \( x_{k+1}=x_k+v_k\Delta t\cos(\theta_k) \), \( y_{k+1}=y_k+v_k\Delta t\sin(\theta_k) \), derive the partial derivatives \( \frac{\partial x_{k+1}}{\partial \theta_k} \), \( \frac{\partial x_{k+1}}{\partial v_k} \), \( \frac{\partial y_{k+1}}{\partial \theta_k} \), \( \frac{\partial y_{k+1}}{\partial v_k} \) and recover the top-left block of \( \mathbf{F}_k \).

Solution:

\[ \begin{aligned} x_{k+1} &= x_k + v_k\Delta t\cos(\theta_k) \;\Rightarrow\; \frac{\partial x_{k+1}}{\partial \theta_k} = v_k\Delta t(-\sin\theta_k),\quad \frac{\partial x_{k+1}}{\partial v_k} = \Delta t\cos\theta_k\\ y_{k+1} &= y_k + v_k\Delta t\sin(\theta_k) \;\Rightarrow\; \frac{\partial y_{k+1}}{\partial \theta_k} = v_k\Delta t(\cos\theta_k),\quad \frac{\partial y_{k+1}}{\partial v_k} = \Delta t\sin\theta_k. \end{aligned} \]

Substituting these into the state ordering \( [x,y,\theta,v,b_g,b_a]^\top \) yields the \( (x,y) \)-rows of \( \mathbf{F}_k \) shown in Section 4.

Problem 2 (Joseph Form PSD Guarantee): Prove that if \( \mathbf{P}\succeq 0 \) and \( \mathbf{R}\succeq 0 \), then \( \mathbf{P}_{new} = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \succeq 0 \).

Solution:

Let \( \mathbf{A}=\mathbf{I}-\mathbf{K}\mathbf{H} \). For any vector \( \mathbf{v} \):

\[ \mathbf{v}^\top \mathbf{P}_{new}\mathbf{v} = (\mathbf{A}^\top\mathbf{v})^\top \mathbf{P}(\mathbf{A}^\top\mathbf{v}) + (\mathbf{K}^\top\mathbf{v})^\top \mathbf{R}(\mathbf{K}^\top\mathbf{v}). \]

Since \( \mathbf{P}\succeq 0 \) and \( \mathbf{R}\succeq 0 \), both terms are nonnegative for all \( \mathbf{v} \). Therefore \( \mathbf{P}_{new}\succeq 0 \).

Problem 3 (UKF Weight Normalization): Show that the mean weights satisfy \( \sum_{i=0}^{2n} W^{(i)}_m = 1 \).

Solution:

\[ \sum_{i=0}^{2n} W^{(i)}_m = \frac{\lambda}{n+\lambda} + 2n\cdot\frac{1}{2(n+\lambda)} = \frac{\lambda}{n+\lambda} + \frac{n}{n+\lambda} = \frac{n+\lambda}{n+\lambda} = 1. \]

Problem 4 (Innovation Gating Threshold): For a 2D GPS update, the innovation dimension is \( m=2 \). Write the acceptance test using NIS for a significance level \( \alpha \) and explain its meaning.

Solution:

Compute \( \mathrm{NIS}_k = \mathbf{y}_k^\top \mathbf{S}_k^{-1}\mathbf{y}_k \). Accept the measurement if:

\[ \mathrm{NIS}_k \le \chi^2_{2,1-\alpha}. \]

Under correct modeling (Gaussian assumptions) the NIS is chi-square distributed with \( m \) degrees of freedom. Rejecting when the statistic is too large implements a principled outlier detector (e.g., GPS multipath spikes).

Problem 5 (Bias Observability Intuition in This Lab): Explain why the gyro bias \( b_g \) becomes estimable when you include both wheel yaw-rate measurements and IMU yaw-rate in the model, but is poorly constrained if GPS is the only correction source.

Solution:

In this lab, wheel yaw-rate \( \omega_w \) provides an independent noisy observation of turning rate, while IMU yaw-rate \( \omega_m \) enters the process model as \( \omega_m - b_g \). The wheel update equation \( \omega_w \approx \omega_m - b_g \) directly couples the residual to \( b_g \), producing a correction direction in state space. If GPS were the only update, gyro bias would influence heading through integration, and heading would only be indirectly constrained via position fixes, which is weaker and can be intermittent—especially during near-stationary periods or low curvature motion where position provides limited heading information.

You implemented a complete wheel–IMU–GPS localization pipeline with both EKF and UKF variants. The EKF required explicit Jacobians and benefited from PSD-safe covariance updates, while the UKF required careful sigma-point construction and angle-aware mean/residual handling. The multi-rate structure reflects practical AMR deployments and sets the stage for particle-filter localization (next chapter) where non-Gaussian posteriors and multi-modality become central.