Project #2 — 2D Bias-Aware Kalman Filter (IMU + GPS)

This project implements a bias-aware 2D state estimator that fuses IMU acceleration and GPS position to address two fundamental problems in robotics localization: - IMU drift caused by accelerometer bias and double integration - GPS noise and low update rate that make it unreliable for real-time motion tracking The estimator demonstrates how sensor fusion and innovation-driven bias estimation produce a stable, bounded state estimate where either sensor alone fails.

System Overview

The system is intentionally modular:

1. Plant — Simulated ground-truth motion with hidden disturbances
2. IMU Sensor — Measures acceleration with noise and constant bias
3. GPS Sensor — Measures absolute position with noise and lower frequency
4. Estimator — Bias-aware Kalman-style filter
5. Simulation Loop — Orchestrates timing, prediction, and correction

This separation mirrors real robotics software stacks and allows components to be modified or replaced independently.

    Plant
    │
    ▼
    IMU Sensor ───────┐
                      ▼
    GPS Sensor ───► Estimator
                        │
                        ▼
                    Controller
                        │
                        ▼
                      Plant

Engineering Motivation

Robotic systems often rely on multiple sensors that have complementary strengths and weaknesses.

This project was built to explore how sensor fusion can combine fast but drifting IMU measurements with slow but absolute GPS measurements, while simultaneously estimating unknown accelerometer bias.

How to Run

Prerequisites

• This project requires:

  • Python 3
  • Python packages listed in requirements.txt

• Install dependencies:

  pip install -r requirements.txt

• Run the simulation:

  • Fast GPS:

    py simulate_fastGPS.py

    or

    python simulate_fastGPS.py

  • Slow GPS:

    py simulate_slowGPS.py

    or

    python simulate_slowGPS.py

  • Very Slow GPS:

    py simulate_verySlowGPS.py

    or

    python simulate_verySlowGPS.py

Project Outcome

• Implemented a 2D bias-aware state estimator combining IMU and GPS measurements.
• Demonstrated how innovation-driven bias estimation suppresses long-term drift.
• Compared estimator behavior under multiple GPS update rates to study sensor timing effects on observability and localization accuracy.

Estimation-Control Interaction

A feedback controller uses the Kalman-filtered state estimate to compute control inputs.

As a result:

• The true state depends on estimation quality
• Estimation errors propagate into control actions
• Sensor fusion stability directly affects closed-loop behavior

This coupling reflects real robotic systems, where:

• Controllers never see ground truth
• Estimation errors directly affect motion

The system demonstrates that robust control is impossible without robust state estimation.

Key Insight

• Accelerometer bias is not directly measurable from GPS.
• It becomes observable only through persistent innovation over time.
  • Consistent positive innovation → prediction lagging → bias too negative
  • Consistent negative innovation → prediction overshooting → bias too positive
• The estimator uses this information to:
  • Correct position using a Kalman gain
  • Slowly adjust the bias estimate to suppress long-term drift

Results

The simulation demonstrates IMU-only prediction, GPS-only correction, and fused estimation under closed-loop control:

• IMU-only: unbounded drift
• GPS-only: noisy, jittery position
• Fused estimate:
  • bounded position
  • suppressed drift
  • stable behavior despite noise and bias

Bias estimates converge gradually without instability, validating innovation-driven correction.

Plots

All plots show true state, IMU dead-reckoning, GPS measurements, and the fused Kalman estimate

• Fast GPS (0.5s update)

Observation:
Frequent GPS corrections rapidly suppress IMU drift. Bias converges smoothly and position closely tracks the true state.

• Slow GPS (2.5s update)

Observation:
Longer prediction intervals allow drift to accumulate. Corrections appear as discrete jumps, but stability is maintained.

• Very Slow GPS (5.0s update)

Observation:
Sparse GPS updates significantly reduce bias observability. Estimator relies heavily on IMU prediction, leading to larger correction steps.

Design Decisions & Simplifications

• Scalar (decoupled) updates are used instead of a full covariance matrix.
• Focus is on observability, bias behavior, and estimator intuition.
• Cross-axis coupling and full matrix algebra are intentionally deferred.

Implementation Details

• Bias-aware state estimator
• IMU prediction step
• GPS correction step
• Innovation-driven bias adaptation
• Multi-rate sensor simulation

Limitations

These are conscious tradeoffs, not omissions.

• No full covariance matrix (F, P, Q, H)
• No delayed measurement handling
• Simple feedback controller (no optimal control)
• Simplified sensor models

Planned Extensions

• Full matrix Kalman filter implementation in C++
• Explicit cross-covariance handling
• Multithreaded sensor update loops
• Integration with control
• Full matrix Kalman filter (F, P, Q, H)
• Out-of-sequence measurement handling

Key Takeaway

This project demonstrates that: - Reliable localization requires both fast prediction and absolute correction, and that sensor bias can only be inferred indirectly through persistent innovation.

Technologies Used

• Python
• Kalman Filtering
• Sensor Fusion
• State Estimation
• Inertial Navigation
• GPS Localization
• Robotics Simulation
• Matplotlib