From d795a7d840d7d543fead998f675ca557a7d63356 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 2 Oct 2026 21:31:03 -0700
Subject: [PATCH 1/4] Add planar GPS and IMU fusion example with bias
 estimation

---
 Localization/gps_imu_fusion/gps_imu_fusion.py | 216 ++++++++++++++++++
 README.md                                     |  12 +
 .../gps_imu_fusion/gps_imu_fusion_main.rst    | 122 ++++++++++
 .../2_localization/localization_main.rst      |   1 +
 tests/test_gps_imu_fusion.py                  | 125 ++++++++++
 5 files changed, 476 insertions(+)
 create mode 100644 Localization/gps_imu_fusion/gps_imu_fusion.py
 create mode 100644 docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
 create mode 100644 tests/test_gps_imu_fusion.py

diff --git a/Localization/gps_imu_fusion/gps_imu_fusion.py b/Localization/gps_imu_fusion/gps_imu_fusion.py
new file mode 100644
index 0000000000..15358e3b6e
--- /dev/null
+++ b/Localization/gps_imu_fusion/gps_imu_fusion.py
@@ -0,0 +1,216 @@
+"""Planar GPS/IMU fusion with an extended Kalman filter.
+
+State: [x, y, vx, vy, yaw, accelerometer_bias_x, accelerometer_bias_y,
+        gyroscope_bias]. Positions and velocities are in a local metric world
+frame. IMU acceleration is in the body frame, with gravity already removed.
+Only horizontal motion is modeled; roll, pitch, altitude, magnetometers and
+barometers are outside this example. GPS and IMU are assumed time-aligned and
+co-located. Initial heading and velocity are assumed approximately known.
+
+Run this file to compare fusion against IMU-only dead reckoning, including a
+GPS outage. No external data or dependencies beyond NumPy/Matplotlib are needed.
+
+Reference: Oliver J. Woodman, An introduction to inertial navigation, 2007.
+https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
+"""
+
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+import numpy as np
+
+
+DT = 0.05  # IMU sample period [s] (20 Hz)
+GPS_INTERVAL = 20  # one GPS position every 20 IMU samples (1 Hz)
+SIM_TIME = 50.0  # [s]
+IMU_STD = np.array([0.1, 0.1, np.deg2rad(0.3)])  # [m/s^2, m/s^2, rad/s]
+GPS_STD = 0.8  # per-axis position standard deviation [m]
+BIAS_RW_STD = np.array([0.002, 0.002, np.deg2rad(0.02)])  # per sqrt(second)
+TRUE_BIAS = np.array([0.04, -0.03, np.deg2rad(0.4)])
+show_animation = True
+
+
+def rotation(yaw):
+    """Rotate a 2D body-frame vector into the world frame."""
+    c, s = np.cos(yaw), np.sin(yaw)
+    return np.array([[c, -s], [s, c]])
+
+
+def wrap_angle(angle):
+    return (angle + np.pi) % (2.0 * np.pi) - np.pi
+
+
+def motion_model(state, imu, dt):
+    """Propagate one IMU sample, holding world acceleration constant for dt."""
+    acceleration = rotation(state[4]) @ (imu[:2] - state[5:7])
+    predicted = state.copy()
+    predicted[:2] += state[2:4] * dt + 0.5 * acceleration * dt**2
+    predicted[2:4] += acceleration * dt
+    predicted[4] = wrap_angle(state[4] + (imu[2] - state[7]) * dt)
+    return predicted
+
+
+def motion_jacobians(state, imu, dt):
+    """Return state and IMU-input Jacobians of the discrete motion model."""
+    rot = rotation(state[4])
+    acceleration = rot @ (imu[:2] - state[5:7])
+    yaw_derivative = np.array([-acceleration[1], acceleration[0]])
+    f = np.eye(8)
+    f[:2, 2:4] = np.eye(2) * dt
+    f[:2, 4] = 0.5 * yaw_derivative * dt**2
+    f[2:4, 4] = yaw_derivative * dt
+    f[:2, 5:7] = -0.5 * rot * dt**2
+    f[2:4, 5:7] = -rot * dt
+    f[4, 7] = -dt
+
+    g = np.zeros((8, 3))
+    g[:2, :2] = 0.5 * rot * dt**2
+    g[2:4, :2] = rot * dt
+    g[4, 2] = dt
+    return f, g
+
+
+def predict(state, covariance, imu, dt=DT):
+    """Predict at the IMU rate, including sensor noise and bias random walk.
+
+    IMU_STD describes independent noise on each discrete IMU sample, while
+    BIAS_RW_STD describes continuous bias random walk per sqrt(second).
+    """
+    f, g = motion_jacobians(state, imu, dt)
+    process_noise = g @ np.diag(IMU_STD**2) @ g.T
+    process_noise[5:8, 5:8] += np.diag(BIAS_RW_STD**2) * dt
+    predicted_covariance = f @ covariance @ f.T + process_noise
+    return motion_model(state, imu, dt), (
+        predicted_covariance + predicted_covariance.T) / 2.0
+
+
+def update_gps(state, covariance, position):
+    """Correct the predicted state when a new 2D GPS position is available.
+
+    GPS observes only position. Velocity, heading and biases are corrected
+    through cross-covariances accumulated during IMU prediction.
+    """
+    h = np.zeros((2, 8))
+    h[:, :2] = np.eye(2)
+    r = np.eye(2) * GPS_STD**2
+    innovation_covariance = h @ covariance @ h.T + r
+    gain = np.linalg.solve(innovation_covariance, h @ covariance).T
+    updated = state + gain @ (position - h @ state)
+    updated[4] = wrap_angle(updated[4])
+    # Joseph form preserves covariance symmetry and positive semidefiniteness.
+    residual = np.eye(8) - gain @ h
+    updated_covariance = residual @ covariance @ residual.T + gain @ r @ gain.T
+    return updated, (updated_covariance + updated_covariance.T) / 2.0
+
+
+def reference_motion(time):
+    """Analytic figure-eight truth and ideal body-frame IMU measurements."""
+    position = np.array([20.0 * np.sin(0.1 * time), 10.0 * np.sin(0.2 * time)])
+    velocity = np.array([2.0 * np.cos(0.1 * time), 2.0 * np.cos(0.2 * time)])
+    acceleration = np.array([-0.2 * np.sin(0.1 * time), -0.4 * np.sin(0.2 * time)])
+    yaw = np.arctan2(velocity[1], velocity[0])
+    yaw_rate = (velocity[0] * acceleration[1] - velocity[1] * acceleration[0]) / (
+        velocity @ velocity)
+    state = np.concatenate((position, velocity, [yaw], TRUE_BIAS))
+    imu = np.concatenate((rotation(yaw).T @ acceleration, [yaw_rate]))
+    return state, imu
+
+
+def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
+    """Run a deterministic simulation with multi-rate sensors.
+
+    GPS outages cover [start, end); pass None for uninterrupted GPS. Truth is
+    computed analytically, independently of the filter's discrete integrator.
+    Both estimates start with the known initial pose/velocity and zero bias.
+    """
+    rng = np.random.default_rng(seed)
+    times = np.arange(int(round(duration / DT)) + 1) * DT
+    truth = np.array([reference_motion(t)[0] for t in times])
+    estimates = np.zeros_like(truth)
+    dead_reckoning = np.zeros_like(truth)
+    covariances = np.zeros((len(times), 8, 8))
+    gps = np.full((len(times), 2), np.nan)
+    state = truth[0].copy()
+    state[5:] = 0.0
+    dead_state = state.copy()
+    covariance = np.diag([1.0, 1.0, 0.2, 0.2, np.deg2rad(5.0),
+                          0.1, 0.1, np.deg2rad(1.0)])**2
+    estimates[0], dead_reckoning[0], covariances[0] = state, dead_state, covariance
+    for i in range(1, len(times)):
+        _, ideal_imu = reference_motion(times[i - 1])
+        imu = ideal_imu + TRUE_BIAS + rng.normal(size=3) * IMU_STD
+        state, covariance = predict(state, covariance, imu)
+        dead_state = motion_model(dead_state, imu, DT)
+        # Draw every scheduled fix, even during outages, to keep sensor noise
+        # identical when comparing different outage schedules with the same seed.
+        if i % GPS_INTERVAL == 0:
+            measurement = truth[i, :2] + rng.normal(size=2) * GPS_STD
+            if gps_outage is None or not gps_outage[0] <= times[i] < gps_outage[1]:
+                gps[i] = measurement
+                state, covariance = update_gps(state, covariance, measurement)
+        estimates[i], dead_reckoning[i], covariances[i] = state, dead_state, covariance
+    return {"time": times, "truth": truth, "estimate": estimates,
+            "dead_reckoning": dead_reckoning, "covariance": covariances,
+            "gps": gps, "gps_outage": gps_outage}
+
+
+def create_animation(history):  # pragma: no cover
+    """Create the path/error animation; keep the returned object alive to play it."""
+    fig, (path_ax, error_ax) = plt.subplots(1, 2, figsize=(11, 4.8))
+    colors = {"truth": "black", "estimate": "tab:blue", "dead_reckoning": "tab:orange"}
+    labels = {"truth": "Ground truth", "estimate": "GPS + IMU EKF",
+              "dead_reckoning": "IMU only"}
+    lines = {key: path_ax.plot([], [], color=color, label=labels[key])[0]
+             for key, color in colors.items()}
+    gps_line, = path_ax.plot([], [], "+", color="tab:green", label="GPS fixes", alpha=0.7)
+    positions = np.vstack([history[key][:, :2] for key in colors])
+    path_ax.set(xlim=(positions[:, 0].min() - 3, positions[:, 0].max() + 3),
+                ylim=(positions[:, 1].min() - 3, positions[:, 1].max() + 3),
+                xlabel="x [m]", ylabel="y [m]")
+    path_ax.set_aspect("equal", adjustable="box")
+    path_ax.legend(loc="best", fontsize=8)
+    errors = {key: np.linalg.norm(history[key][:, :2] - history["truth"][:, :2], axis=1)
+              for key in ["estimate", "dead_reckoning"]}
+    error_lines = {key: error_ax.plot([], [], color=colors[key], label=labels[key])[0]
+                   for key in errors}
+    error_ax.set(xlim=(0, history["time"][-1]),
+                 ylim=(0, max(values.max() for values in errors.values()) * 1.1 + 0.1),
+                 xlabel="Time [s]", ylabel="Position error [m]")
+    outage = history["gps_outage"]
+    if outage is not None:
+        error_ax.axvspan(*outage, color="gray", alpha=0.2, label="GPS outage")
+    error_ax.legend(loc="upper left", fontsize=8)
+    path_ax.grid(True)
+    error_ax.grid(True)
+    title = fig.suptitle("GPS/IMU fusion")
+    fig.tight_layout()
+
+    def update(index):
+        for key, line in lines.items():
+            line.set_data(history[key][:index + 1, 0], history[key][:index + 1, 1])
+        gps_line.set_data(history["gps"][:index + 1, 0], history["gps"][:index + 1, 1])
+        for key, line in error_lines.items():
+            line.set_data(history["time"][:index + 1], errors[key][:index + 1])
+        time = history["time"][index]
+        status = "GPS unavailable" if outage is not None and outage[0] <= time < outage[1] else "GPS available (1 Hz)"
+        title.set_text(f"GPS/IMU fusion — {time:.1f} s — {status}")
+        return [*lines.values(), gps_line, *error_lines.values(), title]
+
+    frames = list(range(0, len(history["time"]), 5))
+    if frames[-1] != len(history["time"]) - 1:
+        frames.append(len(history["time"]) - 1)
+    return FuncAnimation(fig, update, frames=frames, interval=50, repeat=False)
+
+
+def main():
+    history = simulate(duration=SIM_TIME)
+    for key in ["estimate", "dead_reckoning"]:
+        error = history[key][:, :2] - history["truth"][:, :2]
+        print(f"{key} position RMSE: {np.sqrt(np.mean(np.sum(error**2, axis=1))):.2f} m")
+    animation = create_animation(history) if show_animation else None
+    if animation is not None:
+        plt.show()
+    return history
+
+
+if __name__ == "__main__":
+    main()
diff --git a/README.md b/README.md
index f828d90e1d..6bb8a2950c 100644
--- a/README.md
+++ b/README.md
@@ -16,6 +16,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.
    * [How to use](#how-to-use)
    * [Localization](#localization)
       * [Extended Kalman Filter localization](#extended-kalman-filter-localization)
+      * [GPS/IMU fusion](#gpsimu-fusion)
       * [Particle filter localization](#particle-filter-localization)
       * [Histogram filter localization](#histogram-filter-localization)
    * [Mapping](#mapping)
@@ -173,6 +174,17 @@ Reference
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
 
+## GPS/IMU fusion
+
+![GPS/IMU fusion](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/b762650770bc6f6c3f9686b4a778415a8332847a/Localization/gps_imu_fusion/animation.gif)
+
+An extended Kalman filter fuses body-frame accelerometer and gyroscope readings
+with lower-rate GPS positions, estimates sensor biases, and continues inertial
+prediction during a temporary GPS outage.
+
+- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/gps_imu_fusion/gps_imu_fusion.html)
+- [sample code](Localization/gps_imu_fusion/gps_imu_fusion.py)
+
 ## Particle filter localization
 
 ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/particle_filter/animation.gif)
diff --git a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
new file mode 100644
index 0000000000..c3eb831bf2
--- /dev/null
+++ b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
@@ -0,0 +1,122 @@
+GPS/IMU Fusion
+==============
+
+This example uses an extended Kalman filter (EKF) to combine body-frame
+accelerometer and gyroscope measurements with GPS positions in a local metric
+frame. IMU prediction runs at 20 Hz and GPS correction at 1 Hz. The simulation
+compares the fused estimate with IMU-only dead reckoning along a figure-eight
+trajectory, including a GPS outage from 20 to 30 seconds.
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/b762650770bc6f6c3f9686b4a778415a8332847a/Localization/gps_imu_fusion/animation.gif
+   :alt: GPS and IMU fusion compared with inertial dead reckoning during a GPS outage
+
+Assumptions
+-----------
+
+Motion is restricted to a horizontal plane. The IMU is aligned with the body
+frame and its acceleration has already been compensated for gravity. GPS
+positions are expressed in metres, not latitude and longitude. Sensors are
+time-aligned and co-located. Initial heading and velocity are approximately
+known. Roll, pitch, altitude, magnetometers and barometers are not modeled.
+
+The example estimates constant or slowly varying accelerometer and gyroscope
+biases. GPS observes position only, so heading and bias estimation depend on
+motion and the initial uncertainty; they cannot all be recovered from a
+stationary position measurement. No real sensor data, outlier rejection or
+hardware-specific calibration is included.
+
+State and inertial prediction
+-----------------------------
+
+The eight-dimensional state contains world position, world velocity, heading,
+two body-frame accelerometer biases and one yaw-rate bias:
+
+.. math::
+
+   \mathbf{x} = [p_x, p_y, v_x, v_y, \psi, b_{ax}, b_{ay}, b_\omega]^T.
+
+Let :math:`\mathbf{a}_m` and :math:`\omega_m` be the measured acceleration
+and yaw rate. With the body-to-world rotation
+
+.. math::
+
+   R(\psi)=\begin{bmatrix}\cos\psi&-\sin\psi\\\sin\psi&\cos\psi\end{bmatrix},
+   \qquad \mathbf{a}_w=R(\psi)(\mathbf{a}_m-\mathbf{b}_a),
+
+the discrete prediction holds world acceleration constant over one sample:
+
+.. math::
+
+   \mathbf{p}^{-} &= \mathbf{p}+\mathbf{v}\Delta t+\tfrac12\mathbf{a}_w\Delta t^2,\\
+   \mathbf{v}^{-} &= \mathbf{v}+\mathbf{a}_w\Delta t,\\
+   \psi^{-} &= \operatorname{wrap}(\psi+(\omega_m-b_\omega)\Delta t),\\
+   \mathbf{b}^{-} &= \mathbf{b}.
+
+Heading is wrapped to :math:`[-\pi,\pi)`. Define
+:math:`\mathbf{j}=[-a_{wy},a_{wx}]^T`. In block order
+:math:`(\mathbf{p},\mathbf{v},\psi,\mathbf{b}_a,b_\omega)`, the Jacobians are
+
+.. math::
+
+   F=\begin{bmatrix}
+   I&\Delta t I&\tfrac12\Delta t^2\mathbf{j}&-\tfrac12\Delta t^2R&0\\
+   0&I&\Delta t\mathbf{j}&-\Delta t R&0\\
+   0&0&1&0&-\Delta t\\
+   0&0&0&I&0\\
+   0&0&0&0&1
+   \end{bmatrix},\qquad
+   G=\begin{bmatrix}
+   \tfrac12\Delta t^2R&0\\ \Delta t R&0\\ 0&\Delta t\\ 0&0\\ 0&0
+   \end{bmatrix}.
+
+Covariance propagates as
+
+.. math::
+
+   P^{-}=FPF^T+GQ_{\mathrm{imu}}G^T+Q_b.
+
+:math:`Q_{\mathrm{imu}}` is the covariance of a discrete IMU sample.
+For bias random-walk standard deviations specified per square root second,
+the bias block of :math:`Q_b` is
+:math:`\operatorname{diag}(\sigma_{bax}^2,\sigma_{bay}^2,\sigma_{b\omega}^2)\Delta t`;
+its other entries are zero. The simulation adds independent Gaussian sensor
+noise and fixed biases, while the filter allows those biases to vary slowly.
+
+GPS correction and outages
+--------------------------
+
+GPS supplies :math:`\mathbf{z}=[p_x,p_y]^T` with measurement matrix
+:math:`H=[I_2\;0_{2\times6}]` and covariance
+:math:`R_{\mathrm{gps}}=\sigma_{\mathrm{gps}}^2 I_2`.
+
+.. math::
+
+   S &= HP^{-}H^T+R_{\mathrm{gps}},\\
+   K &= P^{-}H^T S^{-1},\\
+   \mathbf{x}^{+} &= \mathbf{x}^{-}+K(\mathbf{z}-H\mathbf{x}^{-}),\\
+   P^{+} &= (I-KH)P^{-}(I-KH)^T+KR_{\mathrm{gps}}K^T.
+
+The gain is computed with a linear solve, and the Joseph covariance update
+helps preserve numerical symmetry and positive semidefiniteness. Between GPS
+fixes, and throughout the outage, only IMU prediction is performed. Uncertainty
+can grow during the outage and decreases when GPS corrections resume.
+
+The analytic reference trajectory is independent of the filter's discrete
+integrator. A fixed random seed makes the example reproducible. The same IMU
+measurements drive both estimates, and the error plot shows their Euclidean
+position errors; these synthetic results are not a real-sensor accuracy claim.
+
+Code
+----
+
+.. autofunction:: Localization.gps_imu_fusion.gps_imu_fusion.predict
+
+.. autofunction:: Localization.gps_imu_fusion.gps_imu_fusion.update_gps
+
+.. autofunction:: Localization.gps_imu_fusion.gps_imu_fusion.simulate
+
+References
+----------
+
+- `Oliver J. Woodman, An introduction to inertial navigation, University of Cambridge, 2007 <https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf>`_
+- :doc:`../extended_kalman_filter_localization_files/extended_kalman_filter_localization`
diff --git a/docs/modules/2_localization/localization_main.rst b/docs/modules/2_localization/localization_main.rst
index 770a234b69..32b25862a4 100644
--- a/docs/modules/2_localization/localization_main.rst
+++ b/docs/modules/2_localization/localization_main.rst
@@ -9,6 +9,7 @@ Localization is the ability of a robot to know its position and orientation with
    :caption: Contents
 
    extended_kalman_filter_localization_files/extended_kalman_filter_localization
+   gps_imu_fusion/gps_imu_fusion
    ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization
    unscented_kalman_filter_localization/unscented_kalman_filter_localization
    histogram_filter_localization/histogram_filter_localization
diff --git a/tests/test_gps_imu_fusion.py b/tests/test_gps_imu_fusion.py
new file mode 100644
index 0000000000..4c13c4ee81
--- /dev/null
+++ b/tests/test_gps_imu_fusion.py
@@ -0,0 +1,125 @@
+import conftest
+import numpy as np
+import matplotlib.pyplot as plt
+import pytest
+
+from Localization.gps_imu_fusion import gps_imu_fusion as m
+
+
+def test_body_frame_acceleration_and_bias_compensation():
+    state = np.array([1.0, 2.0, 3.0, 4.0, np.pi / 2, 0.2, -0.1, 0.03])
+    imu = np.array([2.2, -0.1, 0.13])
+    original = state.copy()
+    result = m.motion_model(state, imu, 0.5)
+    # Body x points along world y, so only vy has acceleration.
+    expected = [2.5, 4.25, 3.0, 5.0, np.pi / 2 + 0.05, 0.2, -0.1, 0.03]
+    np.testing.assert_allclose(result, expected, atol=1e-12)
+    np.testing.assert_array_equal(state, original)
+
+
+def test_motion_jacobians_match_finite_differences():
+    state = np.array([1.0, 2.0, -0.5, 1.5, 0.7, 0.2, -0.1, 0.03])
+    imu = np.array([0.8, -0.3, 0.2])
+    dt, eps = 0.13, 1e-6
+    f, g = m.motion_jacobians(state, imu, dt)
+    for index in range(8):
+        delta = np.eye(8)[index] * eps
+        numeric = (m.motion_model(state + delta, imu, dt)
+                   - m.motion_model(state - delta, imu, dt)) / (2 * eps)
+        np.testing.assert_allclose(f[:, index], numeric, atol=1e-9)
+    for index in range(3):
+        delta = np.eye(3)[index] * eps
+        numeric = (m.motion_model(state, imu + delta, dt)
+                   - m.motion_model(state, imu - delta, dt)) / (2 * eps)
+        np.testing.assert_allclose(g[:, index], numeric, atol=1e-9)
+
+
+def test_prediction_noise_units_at_rest():
+    state = np.zeros(8)
+    _, covariance = m.predict(state, np.zeros((8, 8)), np.zeros(3), dt=0.2)
+    np.testing.assert_allclose(covariance[0, 0], (0.5 * 0.2**2 * m.IMU_STD[0])**2)
+    np.testing.assert_allclose(covariance[0, 2], 0.5 * 0.2**3 * m.IMU_STD[0]**2)
+    np.testing.assert_allclose(covariance[4, 4], (0.2 * m.IMU_STD[2])**2)
+    np.testing.assert_allclose(np.diag(covariance)[5:], m.BIAS_RW_STD**2 * 0.2)
+
+
+def test_gps_update_matches_linear_kalman_result():
+    state = np.zeros(8)
+    covariance = np.eye(8)
+    covariance[0, 2] = covariance[2, 0] = 0.4
+    measurement = np.array([2.0, -1.0])
+    updated, posterior = m.update_gps(state, covariance, measurement)
+    expected = np.zeros(8)
+    expected[:2] = measurement / (1.0 + m.GPS_STD**2)
+    expected[2] = 0.4 * measurement[0] / (1.0 + m.GPS_STD**2)
+    np.testing.assert_allclose(updated, expected)
+    expected_covariance = covariance - covariance[:, :2] @ covariance[:2, :] / (
+        1.0 + m.GPS_STD**2)
+    np.testing.assert_allclose(posterior, expected_covariance, atol=1e-12)
+    np.testing.assert_array_equal(state, np.zeros(8))
+    assert covariance[0, 0] == 1.0
+
+
+def test_heading_wraps_during_prediction_and_correction():
+    state = np.zeros(8)
+    state[4] = np.pi - 0.01
+    result, _ = m.predict(state, np.eye(8), np.array([0.0, 0.0, 0.3]), dt=0.1)
+    np.testing.assert_allclose(result[4], -np.pi + 0.02)
+    covariance = np.eye(8)
+    covariance[0, 4] = covariance[4, 0] = 0.5
+    result, _ = m.update_gps(state, covariance, np.array([1.0, 0.0]))
+    assert -np.pi <= result[4] < 0.0
+
+
+@pytest.mark.parametrize("seed", [0, 7, 42])
+def test_fusion_reduces_drift_with_gps_outage(seed):
+    history = m.simulate(seed=seed)
+    estimates = history["estimate"]
+    fused_error = estimates[:, :2] - history["truth"][:, :2]
+    dead_error = history["dead_reckoning"][:, :2] - history["truth"][:, :2]
+    fused_rmse = np.sqrt(np.mean(np.sum(fused_error**2, axis=1)))
+    dead_rmse = np.sqrt(np.mean(np.sum(dead_error**2, axis=1)))
+    assert fused_rmse < 2.0
+    assert fused_rmse < dead_rmse / 5.0
+    assert np.all(np.isfinite(estimates))
+    assert np.all(np.abs(estimates[:, 4]) <= np.pi)
+    covariance = history["covariance"]
+    np.testing.assert_allclose(covariance, covariance.transpose(0, 2, 1), atol=1e-12)
+    assert np.linalg.eigvalsh(covariance).min() >= -1e-12
+
+
+def test_gps_schedule_outage_and_recovery():
+    history = m.simulate()
+    steps = np.arange(len(history["time"]))
+    available = np.isfinite(history["gps"]).all(axis=1)
+    expected = (steps > 0) & (steps % m.GPS_INTERVAL == 0)
+    expected &= (history["time"] < 20.0) | (history["time"] >= 30.0)
+    np.testing.assert_array_equal(available, expected)
+    uncertainty = np.trace(history["covariance"][:, :2, :2], axis1=1, axis2=2)
+    assert uncertainty[599] > uncertainty[399]  # grows without GPS
+    assert uncertainty[600] < uncertainty[599]  # shrinks on GPS recovery
+    continuous = m.simulate(gps_outage=None)
+    # Removing GPS corrections must not change simulated IMU noise or truth.
+    np.testing.assert_array_equal(history["dead_reckoning"], continuous["dead_reckoning"])
+    np.testing.assert_array_equal(history["truth"], continuous["truth"])
+    np.testing.assert_array_equal(history["gps"][available], continuous["gps"][available])
+
+
+def test_main_without_animation(monkeypatch):
+    monkeypatch.setattr(m, "show_animation", False)
+    monkeypatch.setattr(m, "SIM_TIME", 2.0)
+    history = m.main()
+    assert len(history["time"]) == 41
+
+
+def test_animation_renders(tmp_path):
+    history = m.simulate(duration=1.0, gps_outage=(0.4, 0.8))
+    animation = m.create_animation(history)
+    try:
+        animation.save(tmp_path / "fusion.gif", writer="pillow", fps=5)
+    finally:
+        plt.close(plt.gcf())
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)

From cd2e99294250184bd9974791a2171044d71898eb Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 4 Oct 2026 08:19:24 -0700
Subject: [PATCH 2/4] Compare GPS/IMU fusion without bias estimation

---
 Localization/gps_imu_fusion/gps_imu_fusion.py | 62 ++++++++++------
 README.md                                     |  7 +-
 .../gps_imu_fusion/gps_imu_fusion_main.rst    | 31 ++++++--
 tests/test_gps_imu_fusion.py                  | 71 +++++++++++++------
 4 files changed, 120 insertions(+), 51 deletions(-)

diff --git a/Localization/gps_imu_fusion/gps_imu_fusion.py b/Localization/gps_imu_fusion/gps_imu_fusion.py
index 15358e3b6e..d388f53235 100644
--- a/Localization/gps_imu_fusion/gps_imu_fusion.py
+++ b/Localization/gps_imu_fusion/gps_imu_fusion.py
@@ -7,8 +7,10 @@
 barometers are outside this example. GPS and IMU are assumed time-aligned and
 co-located. Initial heading and velocity are assumed approximately known.
 
-Run this file to compare fusion against IMU-only dead reckoning, including a
-GPS outage. No external data or dependencies beyond NumPy/Matplotlib are needed.
+Run this file to compare fusion with and without bias estimation against
+IMU-only dead reckoning, including a GPS outage. Without bias estimation, the
+state contains only [x, y, vx, vy, yaw] and biases are assumed zero.
+No external data or dependencies beyond NumPy/Matplotlib are needed.
 
 Reference: Oliver J. Woodman, An introduction to inertial navigation, 2007.
 https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
@@ -41,28 +43,31 @@ def wrap_angle(angle):
 
 def motion_model(state, imu, dt):
     """Propagate one IMU sample, holding world acceleration constant for dt."""
-    acceleration = rotation(state[4]) @ (imu[:2] - state[5:7])
+    bias = state[5:] if len(state) == 8 else np.zeros(3)
+    acceleration = rotation(state[4]) @ (imu[:2] - bias[:2])
     predicted = state.copy()
     predicted[:2] += state[2:4] * dt + 0.5 * acceleration * dt**2
     predicted[2:4] += acceleration * dt
-    predicted[4] = wrap_angle(state[4] + (imu[2] - state[7]) * dt)
+    predicted[4] = wrap_angle(state[4] + (imu[2] - bias[2]) * dt)
     return predicted
 
 
 def motion_jacobians(state, imu, dt):
     """Return state and IMU-input Jacobians of the discrete motion model."""
     rot = rotation(state[4])
-    acceleration = rot @ (imu[:2] - state[5:7])
+    bias = state[5:7] if len(state) == 8 else np.zeros(2)
+    acceleration = rot @ (imu[:2] - bias)
     yaw_derivative = np.array([-acceleration[1], acceleration[0]])
-    f = np.eye(8)
+    f = np.eye(len(state))
     f[:2, 2:4] = np.eye(2) * dt
     f[:2, 4] = 0.5 * yaw_derivative * dt**2
     f[2:4, 4] = yaw_derivative * dt
-    f[:2, 5:7] = -0.5 * rot * dt**2
-    f[2:4, 5:7] = -rot * dt
-    f[4, 7] = -dt
+    if len(state) == 8:
+        f[:2, 5:7] = -0.5 * rot * dt**2
+        f[2:4, 5:7] = -rot * dt
+        f[4, 7] = -dt
 
-    g = np.zeros((8, 3))
+    g = np.zeros((len(state), 3))
     g[:2, :2] = 0.5 * rot * dt**2
     g[2:4, :2] = rot * dt
     g[4, 2] = dt
@@ -74,10 +79,12 @@ def predict(state, covariance, imu, dt=DT):
 
     IMU_STD describes independent noise on each discrete IMU sample, while
     BIAS_RW_STD describes continuous bias random walk per sqrt(second).
+    A five-state filter assumes zero bias and has no bias random walk.
     """
     f, g = motion_jacobians(state, imu, dt)
     process_noise = g @ np.diag(IMU_STD**2) @ g.T
-    process_noise[5:8, 5:8] += np.diag(BIAS_RW_STD**2) * dt
+    if len(state) == 8:
+        process_noise[5:8, 5:8] += np.diag(BIAS_RW_STD**2) * dt
     predicted_covariance = f @ covariance @ f.T + process_noise
     return motion_model(state, imu, dt), (
         predicted_covariance + predicted_covariance.T) / 2.0
@@ -89,7 +96,7 @@ def update_gps(state, covariance, position):
     GPS observes only position. Velocity, heading and biases are corrected
     through cross-covariances accumulated during IMU prediction.
     """
-    h = np.zeros((2, 8))
+    h = np.zeros((2, len(state)))
     h[:, :2] = np.eye(2)
     r = np.eye(2) * GPS_STD**2
     innovation_covariance = h @ covariance @ h.T + r
@@ -97,7 +104,7 @@ def update_gps(state, covariance, position):
     updated = state + gain @ (position - h @ state)
     updated[4] = wrap_angle(updated[4])
     # Joseph form preserves covariance symmetry and positive semidefiniteness.
-    residual = np.eye(8) - gain @ h
+    residual = np.eye(len(state)) - gain @ h
     updated_covariance = residual @ covariance @ residual.T + gain @ r @ gain.T
     return updated, (updated_covariance + updated_covariance.T) / 2.0
 
@@ -120,25 +127,33 @@ def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
 
     GPS outages cover [start, end); pass None for uninterrupted GPS. Truth is
     computed analytically, independently of the filter's discrete integrator.
-    Both estimates start with the known initial pose/velocity and zero bias.
+    All three estimates share the same biased, noisy measurements and start
+    with the known initial pose/velocity. The eight-state EKF starts with zero
+    estimated bias; the five-state EKF and dead reckoning assume zero bias.
     """
     rng = np.random.default_rng(seed)
     times = np.arange(int(round(duration / DT)) + 1) * DT
     truth = np.array([reference_motion(t)[0] for t in times])
     estimates = np.zeros_like(truth)
+    no_bias_estimates = np.zeros((len(times), 5))
     dead_reckoning = np.zeros_like(truth)
     covariances = np.zeros((len(times), 8, 8))
+    no_bias_covariances = np.zeros((len(times), 5, 5))
     gps = np.full((len(times), 2), np.nan)
     state = truth[0].copy()
     state[5:] = 0.0
     dead_state = state.copy()
     covariance = np.diag([1.0, 1.0, 0.2, 0.2, np.deg2rad(5.0),
                           0.1, 0.1, np.deg2rad(1.0)])**2
+    no_bias_state = state[:5].copy()
+    no_bias_covariance = covariance[:5, :5].copy()
     estimates[0], dead_reckoning[0], covariances[0] = state, dead_state, covariance
+    no_bias_estimates[0], no_bias_covariances[0] = no_bias_state, no_bias_covariance
     for i in range(1, len(times)):
         _, ideal_imu = reference_motion(times[i - 1])
         imu = ideal_imu + TRUE_BIAS + rng.normal(size=3) * IMU_STD
         state, covariance = predict(state, covariance, imu)
+        no_bias_state, no_bias_covariance = predict(no_bias_state, no_bias_covariance, imu)
         dead_state = motion_model(dead_state, imu, DT)
         # Draw every scheduled fix, even during outages, to keep sensor noise
         # identical when comparing different outage schedules with the same seed.
@@ -147,8 +162,12 @@ def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
             if gps_outage is None or not gps_outage[0] <= times[i] < gps_outage[1]:
                 gps[i] = measurement
                 state, covariance = update_gps(state, covariance, measurement)
+                no_bias_state, no_bias_covariance = update_gps(
+                    no_bias_state, no_bias_covariance, measurement)
         estimates[i], dead_reckoning[i], covariances[i] = state, dead_state, covariance
+        no_bias_estimates[i], no_bias_covariances[i] = no_bias_state, no_bias_covariance
     return {"time": times, "truth": truth, "estimate": estimates,
+            "no_bias_estimate": no_bias_estimates, "no_bias_covariance": no_bias_covariances,
             "dead_reckoning": dead_reckoning, "covariance": covariances,
             "gps": gps, "gps_outage": gps_outage}
 
@@ -156,10 +175,13 @@ def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
 def create_animation(history):  # pragma: no cover
     """Create the path/error animation; keep the returned object alive to play it."""
     fig, (path_ax, error_ax) = plt.subplots(1, 2, figsize=(11, 4.8))
-    colors = {"truth": "black", "estimate": "tab:blue", "dead_reckoning": "tab:orange"}
-    labels = {"truth": "Ground truth", "estimate": "GPS + IMU EKF",
+    colors = {"truth": "black", "estimate": "tab:blue",
+              "no_bias_estimate": "tab:red", "dead_reckoning": "tab:orange"}
+    labels = {"truth": "Ground truth", "estimate": "EKF (bias estimation)",
+              "no_bias_estimate": "EKF (no bias estimation)",
               "dead_reckoning": "IMU only"}
-    lines = {key: path_ax.plot([], [], color=color, label=labels[key])[0]
+    styles = {"truth": "-", "estimate": "-", "no_bias_estimate": "--", "dead_reckoning": ":"}
+    lines = {key: path_ax.plot([], [], color=color, linestyle=styles[key], label=labels[key])[0]
              for key, color in colors.items()}
     gps_line, = path_ax.plot([], [], "+", color="tab:green", label="GPS fixes", alpha=0.7)
     positions = np.vstack([history[key][:, :2] for key in colors])
@@ -169,8 +191,8 @@ def create_animation(history):  # pragma: no cover
     path_ax.set_aspect("equal", adjustable="box")
     path_ax.legend(loc="best", fontsize=8)
     errors = {key: np.linalg.norm(history[key][:, :2] - history["truth"][:, :2], axis=1)
-              for key in ["estimate", "dead_reckoning"]}
-    error_lines = {key: error_ax.plot([], [], color=colors[key], label=labels[key])[0]
+              for key in ["estimate", "no_bias_estimate", "dead_reckoning"]}
+    error_lines = {key: error_ax.plot([], [], color=colors[key], linestyle=styles[key], label=labels[key])[0]
                    for key in errors}
     error_ax.set(xlim=(0, history["time"][-1]),
                  ylim=(0, max(values.max() for values in errors.values()) * 1.1 + 0.1),
@@ -203,7 +225,7 @@ def update(index):
 
 def main():
     history = simulate(duration=SIM_TIME)
-    for key in ["estimate", "dead_reckoning"]:
+    for key in ["estimate", "no_bias_estimate", "dead_reckoning"]:
         error = history[key][:, :2] - history["truth"][:, :2]
         print(f"{key} position RMSE: {np.sqrt(np.mean(np.sum(error**2, axis=1))):.2f} m")
     animation = create_animation(history) if show_animation else None
diff --git a/README.md b/README.md
index 6bb8a2950c..ed10f31e1a 100644
--- a/README.md
+++ b/README.md
@@ -176,11 +176,13 @@ Reference
 
 ## GPS/IMU fusion
 
-![GPS/IMU fusion](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/b762650770bc6f6c3f9686b4a778415a8332847a/Localization/gps_imu_fusion/animation.gif)
+![GPS/IMU fusion](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5da61a2d63ee5ebfce23f67e514f67056c26cc04/Localization/gps_imu_fusion/animation.gif)
 
 An extended Kalman filter fuses body-frame accelerometer and gyroscope readings
 with lower-rate GPS positions, estimates sensor biases, and continues inertial
-prediction during a temporary GPS outage.
+prediction during a temporary GPS outage. The animation compares it with an
+EKF without bias estimation and IMU-only dead reckoning using the same sensor
+measurements.
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/gps_imu_fusion/gps_imu_fusion.html)
 - [sample code](Localization/gps_imu_fusion/gps_imu_fusion.py)
@@ -686,4 +688,3 @@ They are providing a free license of their 1Password team license for this OSS p
 # Authors
 
 - [Contributors to AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/graphs/contributors)
-
diff --git a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
index c3eb831bf2..51ba717107 100644
--- a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
+++ b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
@@ -4,11 +4,11 @@ GPS/IMU Fusion
 This example uses an extended Kalman filter (EKF) to combine body-frame
 accelerometer and gyroscope measurements with GPS positions in a local metric
 frame. IMU prediction runs at 20 Hz and GPS correction at 1 Hz. The simulation
-compares the fused estimate with IMU-only dead reckoning along a figure-eight
-trajectory, including a GPS outage from 20 to 30 seconds.
+compares fusion with and without bias estimation and IMU-only dead reckoning
+along a figure-eight trajectory, including a GPS outage from 20 to 30 seconds.
 
-.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/b762650770bc6f6c3f9686b4a778415a8332847a/Localization/gps_imu_fusion/animation.gif
-   :alt: GPS and IMU fusion compared with inertial dead reckoning during a GPS outage
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5da61a2d63ee5ebfce23f67e514f67056c26cc04/Localization/gps_imu_fusion/animation.gif
+   :alt: GPS and IMU fusion with and without bias estimation compared with inertial dead reckoning during a GPS outage
 
 Assumptions
 -----------
@@ -101,10 +101,27 @@ helps preserve numerical symmetry and positive semidefiniteness. Between GPS
 fixes, and throughout the outage, only IMU prediction is performed. Uncertainty
 can grow during the outage and decreases when GPS corrections resume.
 
+Comparison without bias estimation
+----------------------------------
+
+The baseline EKF estimates only :math:`[p_x,p_y,v_x,v_y,\psi]^T`. It assumes
+zero accelerometer and gyroscope bias, so it uses the raw IMU measurements
+without bias compensation. Its prediction uses the first five rows and
+columns of :math:`F` and the first five rows of :math:`G`, with no bias
+random-walk term. GPS still corrects position, velocity and heading through
+their cross-covariances. Thus it differs from IMU-only dead reckoning, which
+never receives GPS corrections.
+
+All three methods use exactly the same IMU samples, including the same nonzero
+sensor biases and noise. Both EKFs receive the same GPS fixes and outage
+schedule, and start with the same pose, velocity and corresponding covariance.
+Only the eight-state EKF estimates and compensates for the biases. The red
+dashed curves show the five-state EKF without bias estimation.
+
 The analytic reference trajectory is independent of the filter's discrete
-integrator. A fixed random seed makes the example reproducible. The same IMU
-measurements drive both estimates, and the error plot shows their Euclidean
-position errors; these synthetic results are not a real-sensor accuracy claim.
+integrator. A fixed random seed makes the example reproducible, and the error
+plot shows each method's Euclidean position error. These synthetic results
+are not a real-sensor accuracy claim.
 
 Code
 ----
diff --git a/tests/test_gps_imu_fusion.py b/tests/test_gps_imu_fusion.py
index 4c13c4ee81..9f201c797d 100644
--- a/tests/test_gps_imu_fusion.py
+++ b/tests/test_gps_imu_fusion.py
@@ -17,13 +17,21 @@ def test_body_frame_acceleration_and_bias_compensation():
     np.testing.assert_array_equal(state, original)
 
 
-def test_motion_jacobians_match_finite_differences():
-    state = np.array([1.0, 2.0, -0.5, 1.5, 0.7, 0.2, -0.1, 0.03])
+def test_motion_without_bias_estimation():
+    state = np.array([1.0, 2.0, 3.0, 4.0, np.pi / 2])
+    result = m.motion_model(state, np.array([2.2, -0.1, 0.13]), 0.5)
+    expected = [2.5125, 4.275, 3.05, 5.1, np.pi / 2 + 0.065]
+    np.testing.assert_allclose(result, expected, atol=1e-12)
+
+
+@pytest.mark.parametrize("state_size", [5, 8])
+def test_motion_jacobians_match_finite_differences(state_size):
+    state = np.array([1.0, 2.0, -0.5, 1.5, 0.7, 0.2, -0.1, 0.03])[:state_size]
     imu = np.array([0.8, -0.3, 0.2])
     dt, eps = 0.13, 1e-6
     f, g = m.motion_jacobians(state, imu, dt)
-    for index in range(8):
-        delta = np.eye(8)[index] * eps
+    for index in range(state_size):
+        delta = np.eye(state_size)[index] * eps
         numeric = (m.motion_model(state + delta, imu, dt)
                    - m.motion_model(state - delta, imu, dt)) / (2 * eps)
         np.testing.assert_allclose(f[:, index], numeric, atol=1e-9)
@@ -34,29 +42,32 @@ def test_motion_jacobians_match_finite_differences():
         np.testing.assert_allclose(g[:, index], numeric, atol=1e-9)
 
 
-def test_prediction_noise_units_at_rest():
-    state = np.zeros(8)
-    _, covariance = m.predict(state, np.zeros((8, 8)), np.zeros(3), dt=0.2)
+@pytest.mark.parametrize("state_size", [5, 8])
+def test_prediction_noise_units_at_rest(state_size):
+    state = np.zeros(state_size)
+    _, covariance = m.predict(state, np.zeros((state_size, state_size)), np.zeros(3), dt=0.2)
     np.testing.assert_allclose(covariance[0, 0], (0.5 * 0.2**2 * m.IMU_STD[0])**2)
     np.testing.assert_allclose(covariance[0, 2], 0.5 * 0.2**3 * m.IMU_STD[0]**2)
     np.testing.assert_allclose(covariance[4, 4], (0.2 * m.IMU_STD[2])**2)
-    np.testing.assert_allclose(np.diag(covariance)[5:], m.BIAS_RW_STD**2 * 0.2)
+    if state_size == 8:
+        np.testing.assert_allclose(np.diag(covariance)[5:], m.BIAS_RW_STD**2 * 0.2)
 
 
-def test_gps_update_matches_linear_kalman_result():
-    state = np.zeros(8)
-    covariance = np.eye(8)
+@pytest.mark.parametrize("state_size", [5, 8])
+def test_gps_update_matches_linear_kalman_result(state_size):
+    state = np.zeros(state_size)
+    covariance = np.eye(state_size)
     covariance[0, 2] = covariance[2, 0] = 0.4
     measurement = np.array([2.0, -1.0])
     updated, posterior = m.update_gps(state, covariance, measurement)
-    expected = np.zeros(8)
+    expected = np.zeros(state_size)
     expected[:2] = measurement / (1.0 + m.GPS_STD**2)
     expected[2] = 0.4 * measurement[0] / (1.0 + m.GPS_STD**2)
     np.testing.assert_allclose(updated, expected)
     expected_covariance = covariance - covariance[:, :2] @ covariance[:2, :] / (
         1.0 + m.GPS_STD**2)
     np.testing.assert_allclose(posterior, expected_covariance, atol=1e-12)
-    np.testing.assert_array_equal(state, np.zeros(8))
+    np.testing.assert_array_equal(state, np.zeros(state_size))
     assert covariance[0, 0] == 1.0
 
 
@@ -81,11 +92,28 @@ def test_fusion_reduces_drift_with_gps_outage(seed):
     dead_rmse = np.sqrt(np.mean(np.sum(dead_error**2, axis=1)))
     assert fused_rmse < 2.0
     assert fused_rmse < dead_rmse / 5.0
-    assert np.all(np.isfinite(estimates))
-    assert np.all(np.abs(estimates[:, 4]) <= np.pi)
-    covariance = history["covariance"]
-    np.testing.assert_allclose(covariance, covariance.transpose(0, 2, 1), atol=1e-12)
-    assert np.linalg.eigvalsh(covariance).min() >= -1e-12
+    for key in ["estimate", "no_bias_estimate"]:
+        assert np.all(np.isfinite(history[key]))
+        assert np.all(np.abs(history[key][:, 4]) <= np.pi)
+    for key in ["covariance", "no_bias_covariance"]:
+        covariance = history[key]
+        np.testing.assert_allclose(covariance, covariance.transpose(0, 2, 1), atol=1e-12)
+        assert np.linalg.eigvalsh(covariance).min() >= -1e-12
+
+
+def test_no_bias_filter_uses_imu_and_gps():
+    without_gps = m.simulate(duration=2.0, gps_outage=(0.0, 3.0))
+    # With no GPS and no bias compensation, this is IMU-only integration.
+    np.testing.assert_array_equal(without_gps["no_bias_estimate"],
+                                  without_gps["dead_reckoning"][:, :5])
+    with_gps = m.simulate(duration=2.0, gps_outage=None)
+    first_fix = m.GPS_INTERVAL
+    np.testing.assert_array_equal(with_gps["no_bias_estimate"][:first_fix],
+                                  without_gps["no_bias_estimate"][:first_fix])
+    measurement = with_gps["gps"][first_fix]
+    prior_error = np.linalg.norm(without_gps["no_bias_estimate"][first_fix, :2] - measurement)
+    posterior_error = np.linalg.norm(with_gps["no_bias_estimate"][first_fix, :2] - measurement)
+    assert posterior_error < prior_error
 
 
 def test_gps_schedule_outage_and_recovery():
@@ -95,9 +123,10 @@ def test_gps_schedule_outage_and_recovery():
     expected = (steps > 0) & (steps % m.GPS_INTERVAL == 0)
     expected &= (history["time"] < 20.0) | (history["time"] >= 30.0)
     np.testing.assert_array_equal(available, expected)
-    uncertainty = np.trace(history["covariance"][:, :2, :2], axis1=1, axis2=2)
-    assert uncertainty[599] > uncertainty[399]  # grows without GPS
-    assert uncertainty[600] < uncertainty[599]  # shrinks on GPS recovery
+    for key in ["covariance", "no_bias_covariance"]:
+        uncertainty = np.trace(history[key][:, :2, :2], axis1=1, axis2=2)
+        assert uncertainty[599] > uncertainty[399]  # grows without GPS
+        assert uncertainty[600] < uncertainty[599]  # shrinks on GPS recovery
     continuous = m.simulate(gps_outage=None)
     # Removing GPS corrections must not change simulated IMU noise or truth.
     np.testing.assert_array_equal(history["dead_reckoning"], continuous["dead_reckoning"])

From 7c857fc0fd64fb664b3d2f90d3c9f05fb9015dc1 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 4 Oct 2026 08:48:05 -0700
Subject: [PATCH 3/4] Show IMU bias estimates and align localization
 documentation title

---
 Localization/gps_imu_fusion/gps_imu_fusion.py | 39 ++++++++++++++++---
 README.md                                     |  9 +++--
 .../gps_imu_fusion/gps_imu_fusion_main.rst    | 13 +++++--
 3 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/Localization/gps_imu_fusion/gps_imu_fusion.py b/Localization/gps_imu_fusion/gps_imu_fusion.py
index d388f53235..83125d711e 100644
--- a/Localization/gps_imu_fusion/gps_imu_fusion.py
+++ b/Localization/gps_imu_fusion/gps_imu_fusion.py
@@ -173,8 +173,13 @@ def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
 
 
 def create_animation(history):  # pragma: no cover
-    """Create the path/error animation; keep the returned object alive to play it."""
-    fig, (path_ax, error_ax) = plt.subplots(1, 2, figsize=(11, 4.8))
+    """Animate paths, position errors and bias estimates against ground truth."""
+    fig = plt.figure(figsize=(12, 8))
+    grid = fig.add_gridspec(2, 6, height_ratios=[1.3, 1])
+    path_ax = fig.add_subplot(grid[0, :3])
+    error_ax = fig.add_subplot(grid[0, 3:])
+    bias_axes = [fig.add_subplot(grid[1, 2 * i:2 * i + 2], sharex=error_ax)
+                 for i in range(3)]
     colors = {"truth": "black", "estimate": "tab:blue",
               "no_bias_estimate": "tab:red", "dead_reckoning": "tab:orange"}
     labels = {"truth": "Ground truth", "estimate": "EKF (bias estimation)",
@@ -203,7 +208,29 @@ def create_animation(history):  # pragma: no cover
     error_ax.legend(loc="upper left", fontsize=8)
     path_ax.grid(True)
     error_ax.grid(True)
-    title = fig.suptitle("GPS/IMU fusion")
+
+    # Accelerometer biases stay in m/s^2; show gyroscope bias in deg/s.
+    bias_scale = np.array([1.0, 1.0, 180.0 / np.pi])
+    bias_estimates = history["estimate"][:, 5:8] * bias_scale
+    bias_truth = history["truth"][:, 5:8] * bias_scale
+    bias_titles = ["Accelerometer x bias", "Accelerometer y bias", "Gyroscope bias"]
+    bias_units = ["Bias [m/s²]", "Bias [m/s²]", "Bias [deg/s]"]
+    bias_lines = []
+    for i, ax in enumerate(bias_axes):
+        line, = ax.plot([], [], color="tab:blue", label="EKF estimate")
+        bias_lines.append(line)
+        ax.plot(history["time"], bias_truth[:, i], "k--", label="Ground truth")
+        values = np.concatenate((bias_estimates[:, i], bias_truth[:, i]))
+        padding = max(np.ptp(values) * 0.15, 0.01)
+        ax.set(ylim=(values.min() - padding, values.max() + padding),
+               xlabel="Time [s]", ylabel=bias_units[i], title=bias_titles[i])
+        if outage is not None:
+            ax.axvspan(*outage, color="gray", alpha=0.2)
+        ax.legend(loc="best", fontsize=8)
+        ax.grid(True)
+
+    animation_title = "GPS/IMU Fusion Localization with Bias Estimation"
+    title = fig.suptitle(animation_title + "\n", fontsize=12)
     fig.tight_layout()
 
     def update(index):
@@ -212,10 +239,12 @@ def update(index):
         gps_line.set_data(history["gps"][:index + 1, 0], history["gps"][:index + 1, 1])
         for key, line in error_lines.items():
             line.set_data(history["time"][:index + 1], errors[key][:index + 1])
+        for i, line in enumerate(bias_lines):
+            line.set_data(history["time"][:index + 1], bias_estimates[:index + 1, i])
         time = history["time"][index]
         status = "GPS unavailable" if outage is not None and outage[0] <= time < outage[1] else "GPS available (1 Hz)"
-        title.set_text(f"GPS/IMU fusion — {time:.1f} s — {status}")
-        return [*lines.values(), gps_line, *error_lines.values(), title]
+        title.set_text(f"{animation_title}\n{time:.1f} s — {status}")
+        return [*lines.values(), gps_line, *error_lines.values(), *bias_lines, title]
 
     frames = list(range(0, len(history["time"]), 5))
     if frames[-1] != len(history["time"]) - 1:
diff --git a/README.md b/README.md
index ed10f31e1a..ffb5851934 100644
--- a/README.md
+++ b/README.md
@@ -16,7 +16,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.
    * [How to use](#how-to-use)
    * [Localization](#localization)
       * [Extended Kalman Filter localization](#extended-kalman-filter-localization)
-      * [GPS/IMU fusion](#gpsimu-fusion)
+      * [GPS/IMU Fusion Localization with Bias Estimation](#gpsimu-fusion-localization-with-bias-estimation)
       * [Particle filter localization](#particle-filter-localization)
       * [Histogram filter localization](#histogram-filter-localization)
    * [Mapping](#mapping)
@@ -174,15 +174,16 @@ Reference
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
 
-## GPS/IMU fusion
+## GPS/IMU Fusion Localization with Bias Estimation
 
-![GPS/IMU fusion](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5da61a2d63ee5ebfce23f67e514f67056c26cc04/Localization/gps_imu_fusion/animation.gif)
+![GPS/IMU Fusion Localization with Bias Estimation](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5d1bb293772b94b19e7795d4265891017b5925f6/Localization/gps_imu_fusion/animation.gif)
 
 An extended Kalman filter fuses body-frame accelerometer and gyroscope readings
 with lower-rate GPS positions, estimates sensor biases, and continues inertial
 prediction during a temporary GPS outage. The animation compares it with an
 EKF without bias estimation and IMU-only dead reckoning using the same sensor
-measurements.
+measurements. It also plots the accelerometer and gyroscope bias estimates
+against their true values.
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/gps_imu_fusion/gps_imu_fusion.html)
 - [sample code](Localization/gps_imu_fusion/gps_imu_fusion.py)
diff --git a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
index 51ba717107..efd18e25b5 100644
--- a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
+++ b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
@@ -1,5 +1,5 @@
-GPS/IMU Fusion
-==============
+GPS/IMU Fusion Localization with Bias Estimation
+================================================
 
 This example uses an extended Kalman filter (EKF) to combine body-frame
 accelerometer and gyroscope measurements with GPS positions in a local metric
@@ -7,8 +7,13 @@ frame. IMU prediction runs at 20 Hz and GPS correction at 1 Hz. The simulation
 compares fusion with and without bias estimation and IMU-only dead reckoning
 along a figure-eight trajectory, including a GPS outage from 20 to 30 seconds.
 
-.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5da61a2d63ee5ebfce23f67e514f67056c26cc04/Localization/gps_imu_fusion/animation.gif
-   :alt: GPS and IMU fusion with and without bias estimation compared with inertial dead reckoning during a GPS outage
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5d1bb293772b94b19e7795d4265891017b5925f6/Localization/gps_imu_fusion/animation.gif
+   :alt: GPS and IMU fusion localization with path comparisons, position errors, and accelerometer and gyroscope bias estimates against ground truth
+
+The lower three panels show the EKF's estimated accelerometer x/y biases
+(m/s²) and gyroscope bias (deg/s) in blue, with ground truth as black dashed
+lines. The true biases are 0.04 m/s², -0.03 m/s² and 0.4 deg/s, respectively.
+Gray shading marks the GPS outage in the error and bias plots.
 
 Assumptions
 -----------

From e3797038d8e84947401d82f3b2119dfcc0430429 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 4 Oct 2026 09:01:58 -0700
Subject: [PATCH 4/4] Visualize covariance-based three-sigma localization
 uncertainty

---
 Localization/gps_imu_fusion/gps_imu_fusion.py | 54 +++++++++++++++++--
 README.md                                     |  5 +-
 .../gps_imu_fusion/gps_imu_fusion_main.rst    | 37 ++++++++++++-
 tests/test_gps_imu_fusion.py                  | 17 ++++++
 4 files changed, 104 insertions(+), 9 deletions(-)

diff --git a/Localization/gps_imu_fusion/gps_imu_fusion.py b/Localization/gps_imu_fusion/gps_imu_fusion.py
index 83125d711e..52d303e714 100644
--- a/Localization/gps_imu_fusion/gps_imu_fusion.py
+++ b/Localization/gps_imu_fusion/gps_imu_fusion.py
@@ -172,6 +172,16 @@ def simulate(duration=SIM_TIME, seed=0, gps_outage=(20.0, 30.0)):
             "gps": gps, "gps_outage": gps_outage}
 
 
+def position_covariance_ellipse(position, covariance):
+    """Return the 2D position ellipse with semi-axes of three standard deviations."""
+    eigenvalues, eigenvectors = np.linalg.eigh(covariance[:2, :2])
+    angles = np.linspace(0.0, 2.0 * np.pi, 61)
+    circle = np.array([np.cos(angles), np.sin(angles)])
+    # Clip roundoff at zero for positive semidefinite covariances.
+    radii = 3.0 * np.sqrt(np.maximum(eigenvalues, 0.0))
+    return position[:2, None] + eigenvectors @ (radii[:, None] * circle)
+
+
 def create_animation(history):  # pragma: no cover
     """Animate paths, position errors and bias estimates against ground truth."""
     fig = plt.figure(figsize=(12, 8))
@@ -188,19 +198,37 @@ def create_animation(history):  # pragma: no cover
     styles = {"truth": "-", "estimate": "-", "no_bias_estimate": "--", "dead_reckoning": ":"}
     lines = {key: path_ax.plot([], [], color=color, linestyle=styles[key], label=labels[key])[0]
              for key, color in colors.items()}
+    current_points = {key: path_ax.plot([], [], marker="x" if key == "truth" else "o",
+                                       color=colors[key], markersize=6, linestyle="none")[0]
+                      for key in ["truth", "estimate", "no_bias_estimate"]}
     gps_line, = path_ax.plot([], [], "+", color="tab:green", label="GPS fixes", alpha=0.7)
-    positions = np.vstack([history[key][:, :2] for key in colors])
+    covariance_keys = {"estimate": "covariance", "no_bias_estimate": "no_bias_covariance"}
+    ellipse_lines = {}
+    positions = [history[key][:, :2] for key in colors]
+    for key, covariance_key in covariance_keys.items():
+        ellipse_lines[key], = path_ax.plot([], [], color=colors[key], linestyle=styles[key],
+                                           linewidth=1.5, label=labels[key] + " 3σ")
+        position_width = 3.0 * np.sqrt(np.maximum(
+            np.diagonal(history[covariance_key][:, :2, :2], axis1=1, axis2=2), 0.0))
+        positions.extend([history[key][:, :2] - position_width,
+                          history[key][:, :2] + position_width])
+    positions = np.vstack(positions)
     path_ax.set(xlim=(positions[:, 0].min() - 3, positions[:, 0].max() + 3),
                 ylim=(positions[:, 1].min() - 3, positions[:, 1].max() + 3),
                 xlabel="x [m]", ylabel="y [m]")
     path_ax.set_aspect("equal", adjustable="box")
-    path_ax.legend(loc="best", fontsize=8)
+    path_ax.legend(loc="best", fontsize=7)
     errors = {key: np.linalg.norm(history[key][:, :2] - history["truth"][:, :2], axis=1)
               for key in ["estimate", "no_bias_estimate", "dead_reckoning"]}
     error_lines = {key: error_ax.plot([], [], color=colors[key], linestyle=styles[key], label=labels[key])[0]
                    for key in errors}
+    # This is the ellipse's enclosing radius, not a 1D error standard deviation.
+    position_radius = 3.0 * np.sqrt(np.maximum(
+        np.linalg.eigvalsh(history["covariance"][:, :2, :2])[:, -1], 0.0))
+    radius_line, = error_ax.plot([], [], "--", color="tab:blue", label="EKF 3σ major radius")
     error_ax.set(xlim=(0, history["time"][-1]),
-                 ylim=(0, max(values.max() for values in errors.values()) * 1.1 + 0.1),
+                 ylim=(0, max(position_radius.max(),
+                              max(values.max() for values in errors.values())) * 1.1 + 0.1),
                  xlabel="Time [s]", ylabel="Position error [m]")
     outage = history["gps_outage"]
     if outage is not None:
@@ -213,14 +241,20 @@ def create_animation(history):  # pragma: no cover
     bias_scale = np.array([1.0, 1.0, 180.0 / np.pi])
     bias_estimates = history["estimate"][:, 5:8] * bias_scale
     bias_truth = history["truth"][:, 5:8] * bias_scale
+    bias_width = 3.0 * np.sqrt(np.maximum(
+        np.diagonal(history["covariance"], axis1=1, axis2=2)[:, 5:8], 0.0)) * bias_scale
+    bias_lower, bias_upper = bias_estimates - bias_width, bias_estimates + bias_width
     bias_titles = ["Accelerometer x bias", "Accelerometer y bias", "Gyroscope bias"]
     bias_units = ["Bias [m/s²]", "Bias [m/s²]", "Bias [deg/s]"]
     bias_lines = []
+    bias_bands = []
     for i, ax in enumerate(bias_axes):
         line, = ax.plot([], [], color="tab:blue", label="EKF estimate")
         bias_lines.append(line)
         ax.plot(history["time"], bias_truth[:, i], "k--", label="Ground truth")
-        values = np.concatenate((bias_estimates[:, i], bias_truth[:, i]))
+        bias_bands.append(ax.fill_between([], [], [], color="tab:blue", alpha=0.2,
+                                           label="Estimate ±3σ"))
+        values = np.concatenate((bias_lower[:, i], bias_upper[:, i], bias_truth[:, i]))
         padding = max(np.ptp(values) * 0.15, 0.01)
         ax.set(ylim=(values.min() - padding, values.max() + padding),
                xlabel="Time [s]", ylabel=bias_units[i], title=bias_titles[i])
@@ -236,15 +270,25 @@ def create_animation(history):  # pragma: no cover
     def update(index):
         for key, line in lines.items():
             line.set_data(history[key][:index + 1, 0], history[key][:index + 1, 1])
+        for key, point in current_points.items():
+            point.set_data([history[key][index, 0]], [history[key][index, 1]])
+        for key, line in ellipse_lines.items():
+            ellipse = position_covariance_ellipse(
+                history[key][index], history[covariance_keys[key]][index])
+            line.set_data(ellipse[0], ellipse[1])
         gps_line.set_data(history["gps"][:index + 1, 0], history["gps"][:index + 1, 1])
         for key, line in error_lines.items():
             line.set_data(history["time"][:index + 1], errors[key][:index + 1])
+        radius_line.set_data(history["time"][:index + 1], position_radius[:index + 1])
         for i, line in enumerate(bias_lines):
             line.set_data(history["time"][:index + 1], bias_estimates[:index + 1, i])
+            bias_bands[i].set_data(history["time"][:index + 1],
+                                   bias_lower[:index + 1, i], bias_upper[:index + 1, i])
         time = history["time"][index]
         status = "GPS unavailable" if outage is not None and outage[0] <= time < outage[1] else "GPS available (1 Hz)"
         title.set_text(f"{animation_title}\n{time:.1f} s — {status}")
-        return [*lines.values(), gps_line, *error_lines.values(), *bias_lines, title]
+        return [*lines.values(), *current_points.values(), *ellipse_lines.values(),
+                gps_line, *error_lines.values(), radius_line, *bias_lines, *bias_bands, title]
 
     frames = list(range(0, len(history["time"]), 5))
     if frames[-1] != len(history["time"]) - 1:
diff --git a/README.md b/README.md
index ffb5851934..1a5c07f290 100644
--- a/README.md
+++ b/README.md
@@ -176,14 +176,15 @@ Reference
 
 ## GPS/IMU Fusion Localization with Bias Estimation
 
-![GPS/IMU Fusion Localization with Bias Estimation](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5d1bb293772b94b19e7795d4265891017b5925f6/Localization/gps_imu_fusion/animation.gif)
+![GPS/IMU Fusion Localization with Bias Estimation](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5abf69412ae056a2f30ba562a1c04600b6062fe4/Localization/gps_imu_fusion/animation.gif)
 
 An extended Kalman filter fuses body-frame accelerometer and gyroscope readings
 with lower-rate GPS positions, estimates sensor biases, and continues inertial
 prediction during a temporary GPS outage. The animation compares it with an
 EKF without bias estimation and IMU-only dead reckoning using the same sensor
 measurements. It also plots the accelerometer and gyroscope bias estimates
-against their true values.
+against their true values, with covariance-derived 3σ position ellipses and
+bias uncertainty bands.
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/gps_imu_fusion/gps_imu_fusion.html)
 - [sample code](Localization/gps_imu_fusion/gps_imu_fusion.py)
diff --git a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
index efd18e25b5..e383da4ed9 100644
--- a/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
+++ b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
@@ -7,14 +7,47 @@ frame. IMU prediction runs at 20 Hz and GPS correction at 1 Hz. The simulation
 compares fusion with and without bias estimation and IMU-only dead reckoning
 along a figure-eight trajectory, including a GPS outage from 20 to 30 seconds.
 
-.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5d1bb293772b94b19e7795d4265891017b5925f6/Localization/gps_imu_fusion/animation.gif
-   :alt: GPS and IMU fusion localization with path comparisons, position errors, and accelerometer and gyroscope bias estimates against ground truth
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/5abf69412ae056a2f30ba562a1c04600b6062fe4/Localization/gps_imu_fusion/animation.gif
+   :alt: GPS and IMU fusion localization with position covariance ellipses and bias estimates with three-sigma uncertainty bands against ground truth
 
 The lower three panels show the EKF's estimated accelerometer x/y biases
 (m/s²) and gyroscope bias (deg/s) in blue, with ground truth as black dashed
 lines. The true biases are 0.04 m/s², -0.03 m/s² and 0.4 deg/s, respectively.
 Gray shading marks the GPS outage in the error and bias plots.
 
+Covariance-based uncertainty
+----------------------------
+
+The path plot shows a 3σ position ellipse centered on each EKF estimate, with
+markers for the current estimates and ground truth. If :math:`\lambda_i` are
+the eigenvalues of the position covariance :math:`P_{xy}`, the semi-axis
+lengths are :math:`3\sqrt{\lambda_i}` and the eigenvectors give their directions.
+This includes the x/y cross-covariance. The ellipse boundary satisfies
+:math:`(\mathbf{p}-\hat{\mathbf{p}})^T P_{xy}^{-1}
+(\mathbf{p}-\hat{\mathbf{p}})=9`.
+
+The dashed blue line in the position-error plot tracks the bias-estimating
+EKF's ellipse semi-major axis, :math:`3\sqrt{\lambda_{\max}(P_{xy})}`.
+It is the ellipse's enclosing radius, not the standard deviation of the
+Euclidean position error. An error below this line alone does not guarantee
+that the true position lies inside the ellipse; direction also matters.
+
+Each bias panel shades :math:`\hat b_i \pm 3\sqrt{P_{ii}}`, using the matching
+diagonal element of the estimated state covariance. Gyroscope estimates and
+standard deviations are both converted from rad/s to deg/s. The estimate is
+at the center of its band; the true value provides an independent comparison.
+The plots use the filter covariance directly, so they show uncertainty
+contracting with GPS corrections and growing during the outage.
+
+For the default seed-0 simulation, ground truth stays within the
+bias-estimating EKF's position ellipse and all three bias bands at every
+sample. The EKF without bias estimation has position errors outside its
+ellipse during parts of the run. These are results for this synthetic run,
+not a guarantee of coverage for other data. A 3σ ellipse encloses about
+98.9% of an ideal 2D Gaussian, whereas a scalar ±3σ interval encloses about
+99.7%; see `Matplotlib's confidence-ellipse explanation
+<https://matplotlib.org/stable/gallery/statistics/confidence_ellipse.html>`_.
+
 Assumptions
 -----------
 
diff --git a/tests/test_gps_imu_fusion.py b/tests/test_gps_imu_fusion.py
index 9f201c797d..78d6e858b8 100644
--- a/tests/test_gps_imu_fusion.py
+++ b/tests/test_gps_imu_fusion.py
@@ -141,6 +141,23 @@ def test_main_without_animation(monkeypatch):
     assert len(history["time"]) == 41
 
 
+@pytest.mark.parametrize("covariance", [np.diag([4.0, 1.0]),
+                                       np.array([[3.0, 2.0], [2.0, 3.0]])])
+def test_position_ellipse_has_three_sigma_mahalanobis_radius(covariance):
+    position = np.array([2.0, -1.0])
+    points = m.position_covariance_ellipse(position, covariance)
+    errors = points - position[:, None]
+    squared_distance = np.sum(errors * np.linalg.solve(covariance, errors), axis=0)
+    np.testing.assert_allclose(squared_distance, 9.0, atol=1e-12)
+    np.testing.assert_allclose(points[:, 0], points[:, -1], atol=1e-12)
+
+
+def test_position_ellipse_with_zero_variance_axis():
+    points = m.position_covariance_ellipse(np.array([2.0, -1.0]), np.diag([4.0, 0.0]))
+    np.testing.assert_allclose(points[1], -1.0)
+    np.testing.assert_allclose([points[0].min(), points[0].max()], [-4.0, 8.0])
+
+
 def test_animation_renders(tmp_path):
     history = m.simulate(duration=1.0, gps_outage=(0.4, 0.8))
     animation = m.create_animation(history)