Add localization GPS/IMU fusion with bias estimation and outage simulation

[AtsushiSakai]

Reference issue

Addresses #603 with a planar GPS/IMU fusion example.

What does this implement/fix?

Add an eight-state extended Kalman filter that predicts from body-frame accelerometer/gyroscope readings at 20 Hz and corrects with GPS positions at 1 Hz. It estimates position, velocity, heading and three IMU biases, continuing prediction during a simulated 10-second GPS outage.

An independently generated analytic figure-eight trajectory lets the animation compare GPS/IMU fusion with bias estimation, a five-state GPS/IMU EKF without bias estimation, and IMU-only dead reckoning. All three use the same biased, noisy IMU samples; both EKFs receive the same GPS fixes and outage schedule. The five-state EKF assumes zero bias while still correcting position, velocity and heading with GPS. The sample includes analytic motion Jacobians, sample-noise propagation, bias random walk and a Joseph-form GPS covariance update.

The documentation and README use the title GPS/IMU Fusion Localization with Bias Estimation. The animation includes three additional panels comparing estimated accelerometer x/y biases (m/s²) and gyroscope bias (deg/s) against ground truth, with covariance-derived ±3σ bands and GPS outages shaded. The path plot shows current 3σ position ellipses for both EKFs, including x/y cross-covariance; the error plot also tracks the bias-estimating EKF's ellipse semi-major axis.

The documentation states the planar, gravity-compensated, local-metric-frame assumptions and the need for approximately known initial heading/velocity. It includes the equations and code links. README and localization contents link to the example.

Companion animation: PythonRoboticsGifs#17. The documentation uses its immutable commit URL so the image is available before merging.

Additional information

Validated on Python 3.13.2 with the repository's pinned runtime and documentation dependencies:

• MPLBACKEND=Agg bash runtests.sh: 156 passed, including 19 tests for the new example, Ruff and mypy.
• cd docs && make html: passed with warnings treated as errors.
• Tests cover rotated acceleration/bias compensation, finite-difference Jacobians, noise units and known Kalman corrections for both five- and eight-state filters, angle wrapping, covariance symmetry/PSD, GPS cadence/outage/recovery, repeatable sensor streams, the no-bias filter's IMU-only behavior without GPS and correction when GPS returns, 3σ ellipse scaling/rotation/degenerate covariance, and animation rendering.
• Default 50-second simulation (seed 0): position RMSE 0.66 m for fusion with bias estimation, 2.58 m for fusion without bias estimation, and 15.74 m for IMU-only integration. These are synthetic demonstration results.
• For seed 0, the true position and all three true biases stay inside the bias-estimating EKF's displayed 3σ regions at all 1,001 samples. This is a result for this synthetic run; the documentation distinguishes scalar intervals from 2D ellipses and explains their coverage.
• Generated and visually checked the 201-frame, 1080 × 720 animation with all three localization estimates and all three estimated/true bias traces; no new dependencies.

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example?
• All CIs are green? (Local checks passed; remote checks pending.)

[chatgpt-codex-connector]

Codex Review Summary

This comment shows the latest Codex review activity on this pull request.

Review	Status	Commit	Review trigger
📝 Code Review	✅ Completed 2026-10-04T15:02:04.926755Z	d795a7d	Draft marked ready

ℹ️ About Codex in GitHub

Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you

• Open a pull request for review
• Mark a draft as ready
• Comment "@codex review" or "@codex security review".

Codex reacts with 👀 while any review is running, comments if it has suggestions, and reacts with 👍 once all reviews finish with no findings.

[Copilot]

Copilot review overview

🟢 Approval recommended

The implementation, documentation, links, and comprehensive tests are consistent and no blocking issues were identified.

Review effort: Balanced
Findings: None

What changed in this PR

Adds a documented planar GPS/IMU EKF example with bias estimation and simulated GPS outages.

Changes:

• Implements eight-state sensor fusion, simulation, and animation.
• Adds comprehensive numerical and rendering tests.
• Integrates the example into project documentation and README.

File	Description
Localization/​gps_imu_fusion/​gps_imu_fusion.py	Implements the EKF, simulation, and animation.
tests/​test_gps_imu_fusion.py	Tests models, covariance, outages, and rendering.
docs/​modules/​2_localization/​gps_imu_fusion/​gps_imu_fusion_main.rst	Documents assumptions and equations.
docs/​modules/​2_localization/​localization_main.rst	Adds the documentation entry.
README.md	Adds the example overview and links.

---

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

[AtsushiSakai]

GPS/IMU Fusion Localization with Bias Estimation

The upper panels compare GPS/IMU fusion with bias estimation (blue), fusion without bias estimation (red dashed), and IMU-only dead reckoning (orange dotted), using the same sensor measurements.

• Position: each EKF has a current 3σ covariance ellipse centered on its estimate, with semi-axes 3√λᵢ and orientation from the position covariance eigenvectors. Dots mark the current estimates and a black cross marks the true position. The dashed blue error-plot curve is the bias-estimating EKF's ellipse semi-major axis, not a scalar position-error standard deviation.
• Biases: blue estimates and shaded estimate ± 3√Pᵢᵢ bands are shown against black dashed ground truth. True accelerometer x/y biases are 0.04 m/s² and −0.03 m/s²; true gyroscope bias is 0.4 deg/s. The band and estimate use the same units.
• Gray shading marks the 20–30 s GPS outage. Uncertainty grows during the outage and contracts with GPS corrections.

For the default seed 0, ground truth lies inside the bias-estimating EKF's position ellipse and all three bias bands at all 1,001 samples. The no-bias-estimation EKF's true position falls outside its ellipse during parts of the run. These are synthetic demonstration results, not a guarantee of coverage for other data.

The bias 3σ half-widths shrink from 0.300 → 0.0339 m/s² (x), 0.300 → 0.0296 m/s² (y), and 3.000 → 0.458 deg/s (gyro).

Position RMSE: 0.66 m with bias estimation, 2.58 m without bias estimation, and 15.74 m for IMU-only dead reckoning. Updated animation: PythonRoboticsGifs#17.

[Copilot]

Copilot review overview

🟢 Approval recommended

The implementation, documentation, tests, and successful CI checks consistently support the stated functionality.

Review effort: Balanced
Findings: None

[AtsushiSakai]

LGTM

[Copilot]

Copilot review overview

🟢 Approval recommended

The implementation, documentation, tests, and companion animation are consistent and no unresolved correctness issues were found.

Review effort: Balanced
Findings: None