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..52d303e714
--- /dev/null
+++ b/Localization/gps_imu_fusion/gps_imu_fusion.py
@@ -0,0 +1,311 @@
+"""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 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
+"""
+
+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."""
+    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] - 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])
+    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(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
+    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((len(state), 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).
+    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
+    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
+
+
+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, len(state)))
+    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(len(state)) - 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.
+    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.
+        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)
+                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}
+
+
+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))
+    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)",
+              "no_bias_estimate": "EKF (no bias estimation)",
+              "dead_reckoning": "IMU only"}
+    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)
+    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=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(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:
+        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)
+
+    # 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_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")
+        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])
+        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):
+        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(), *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:
+        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", "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
+    if animation is not None:
+        plt.show()
+    return history
+
+
+if __name__ == "__main__":
+    main()
diff --git a/README.md b/README.md
index f828d90e1d..1a5c07f290 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 Localization with Bias Estimation](#gpsimu-fusion-localization-with-bias-estimation)
       * [Particle filter localization](#particle-filter-localization)
       * [Histogram filter localization](#histogram-filter-localization)
    * [Mapping](#mapping)
@@ -173,6 +174,21 @@ Reference
 
 - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)
 
+## GPS/IMU Fusion Localization with Bias Estimation
+
+![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, 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)
+
 ## Particle filter localization
 
 ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/particle_filter/animation.gif)
@@ -674,4 +690,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
new file mode 100644
index 0000000000..e383da4ed9
--- /dev/null
+++ b/docs/modules/2_localization/gps_imu_fusion/gps_imu_fusion_main.rst
@@ -0,0 +1,177 @@
+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
+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/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
+-----------
+
+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.
+
+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, and the error
+plot shows each method's Euclidean position error. 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..78d6e858b8
--- /dev/null
+++ b/tests/test_gps_imu_fusion.py
@@ -0,0 +1,171 @@
+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_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(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)
+    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)
+
+
+@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)
+    if state_size == 8:
+        np.testing.assert_allclose(np.diag(covariance)[5:], m.BIAS_RW_STD**2 * 0.2)
+
+
+@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(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(state_size))
+    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
+    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():
+    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)
+    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"])
+    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
+
+
+@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)
+    try:
+        animation.save(tmp_path / "fusion.gif", writer="pillow", fps=5)
+    finally:
+        plt.close(plt.gcf())
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)