GPS-IMU Fusion

Problem Statement

IMU integration is high-rate but drifts; GPS is globally referenced but low-rate and noisy. GPS-IMU fusion combines both into a single estimate that remains smooth, drift-limited, and real-time.

Model and Formulation

The fused estimator uses IMU-driven prediction and GPS correction:

$$ \hat{x}{k|k-1} = f(\hat{x}, u^{imu}_k) $$

$$ \hat{x}{k|k} = \hat{x} + K_k\left(z^{gps}k - h(\hat{x})\right) $$

The asynchronous rate structure is critical: f runs at IMU rate, update only when GPS messages arrive.

Algorithm Procedure

1. Integrate IMU acceleration and angular rate at high frequency.
2. Propagate covariance with process model and bias noise.
3. On each GPS arrival, compute innovation and perform correction.
4. Publish fused state for path tracking and planning.

Tuning Guidance

• Keep GPS R realistic; over-trusting GPS causes noisy position estimates.
• Include IMU bias states whenever drift is non-negligible.
• Use timestamp-consistent interpolation/extrapolation for sensor alignment.

Failure Modes and Diagnostics

• Time synchronization error can appear as oscillatory position correction.
• GPS dropouts cause rapid uncertainty growth if process noise is under-modeled.
• Bias unobservability yields slowly diverging velocity/attitude estimates.

Implementation and Execution

python -m uav_sim.simulations.estimation.gps_imu_fusion

Evidence

References

• Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems
• Maybeck, Stochastic Models, Estimation, and Control

Related Algorithms

• Extended Kalman Filter
• Complementary Filter