IMU–GNSS Fusion for Vehicle Localisation with an EKF

The problem

GNSS gives absolute position but at a low rate, with metre-level noise and gaps under flyovers, in tunnels and between tall buildings, all common in Indian cities. An IMU gives smooth, high-rate motion data but drifts quickly on its own. Lane-level ADAS functions and map-based features need a continuous, smooth position and heading, so production systems fuse the two. This project builds and tests that fusion.

Objectives

• Generate a realistic vehicle trajectory with matching IMU and GNSS measurements, including outages.
• Implement an EKF that predicts with IMU data and corrects with GNSS fixes.
• Use a vehicle non-holonomic constraint to limit drift during outages.
• Measure position and heading error against ground truth with and without outages.
• Study the effect of IMU grade and outage length on drift.

What you need

Key concepts

Dead reckoning

Integrating accelerations and angular rates to update position and heading. It is smooth and fast but errors grow quickly because sensor bias is integrated too.

Loosely coupled fusion

The filter uses GNSS position and velocity solutions as measurements, rather than raw satellite pseudoranges. It is simpler and adequate for a first study.

Non-holonomic constraint

A car on a road does not slide sideways or move vertically in normal driving, so lateral and vertical velocities in the vehicle frame are close to zero. Using this as a pseudo-measurement slows drift during outages.

IMU bias

A slowly varying offset in accelerometer and gyroscope readings. Estimating bias as part of the state is what lets the filter keep drift small after GNSS returns.

Step-by-step guide

1. 1

   State requirements and test routes

   Define what the localisation must achieve: for example, position error under 2 m with GNSS available and heading error under 2 degrees, plus a drift target after a 20 s outage. Describe the test routes: an urban loop with turns, a highway segment and a flyover or tunnel section with GNSS lost. State the sensor rates you will simulate, such as IMU at 100 Hz and GNSS at 5 Hz, and the IMU grade.

   Checkpoint: A requirement sheet with accuracy targets, sensor rates and the planned outage locations.

2. 2

   Generate ground-truth trajectories

   Use waypointTrajectory to create each route from waypoints and times of arrival, with orientation following the direction of travel. Check that speeds and accelerations are realistic for a car (for example lateral acceleration under 4 m/s² in turns). Output position, velocity, orientation, acceleration and angular velocity at 100 Hz. Save each trajectory so every filter variant is tested on identical truth.

   Checkpoint: Plots of each route, its speed profile and its heading, with physically plausible values.

3. 3

   Simulate IMU and GNSS

   Create an imuSensor with accelerometer and gyroscope noise, bias instability and random walk set to values from a consumer MEMS datasheet, and a second configuration for a better automotive-grade IMU. Create a gnssSensor at 5 Hz with horizontal position noise of a few metres. Remove GNSS fixes inside the outage windows to imitate the tunnel and flyover. Fix the random seeds for repeatability.

   Checkpoint: Simulated IMU and GNSS streams with outages visible as gaps on a time plot.

4. 4

   Run dead reckoning alone

   Integrate the IMU data alone, starting from the true initial state, to see how fast position and heading drift. Plot error against time for both IMU grades. This baseline makes the value of fusion concrete and gives a feel for the size of drift the filter has to control during outages. Record the error after 10, 20 and 60 s.

   Checkpoint: Drift curves for both IMU grades with errors at 10, 20 and 60 s tabulated.

5. 5

   Set up the fusion filter

   Configure insfilterNonholonomic with the IMU sample rate and the decimation factor at which the non-holonomic constraint is applied. Set initial state and covariance from the first GNSS fix and an initial heading. In a loop, call predict with each IMU sample and fusegps with each available GNSS fix and its covariance. Log state estimates and covariance for analysis. Check that the filter's reference frame and the GNSS reference location match the trajectory, because a frame mismatch produces errors that look like bad tuning.

   Checkpoint: The fused position follows the true route closely on the urban loop with GNSS always available.

6. 6

   Write a planar EKF yourself

   Implement a simpler planar EKF with state [x, y, heading, speed, gyro bias] and IMU yaw rate and longitudinal acceleration as inputs. Derive the Jacobians of the motion model by hand and correct with GNSS position. Compare it with the toolbox filter on the same data. Writing this smaller filter yourself is what lets you explain the toolbox filter in the viva.

   Checkpoint: The hand-written EKF tracks the urban loop with errors in the same range as the toolbox filter.

7. 7

   Tune the filter

   Adjust the process noise for accelerometer and gyroscope noise and bias, and the non-holonomic constraint noise, using a separate tuning route. Look at the innovations of the GNSS update: they should be zero-mean and roughly match the predicted innovation covariance. Avoid tuning on the test routes, so the reported results are fair. Change one noise group at a time and record its effect, so the final values can be explained parameter by parameter rather than presented as a lucky combination.

   Checkpoint: Innovation plots on the tuning route that look like zero-mean noise within predicted bounds.

8. 8

   Test through outages

   Run the fused filter on every route with outages of 5, 10, 20 and 40 s, for both IMU grades, over at least 30 Monte Carlo seeds. Record maximum position and heading error during each outage and the time to reconverge after GNSS returns. Repeat with the non-holonomic constraint disabled to show how much it helps. Place outages both on straight roads and in turns, because heading errors grow much faster when the vehicle is turning without GNSS.

   Checkpoint: A table of outage drift and reconvergence time across outage lengths, IMU grades and constraint settings.

9. 9

   Compare with GNSS alone

   Compute position and heading error for raw GNSS fixes, dead reckoning and the fused filter on every route. Note that raw GNSS gives no output during outages and a noisy heading at low speed, while the fused filter gives a continuous, smooth output. Plot error distributions as box plots across seeds for a clear comparison. Report results separately for the urban loop and the highway, because speed and turning change which approach looks best.

   Checkpoint: Box plots of position and heading error for the three approaches on each route.

10. 10

    Report

    Write up the sensor models, filter design, tuning procedure, outage tests and comparisons. Explain why bias estimation and the non-holonomic constraint matter, using your own plots. State the limitations: simulated sensors, flat roads, no wheel odometry and loosely coupled fusion only. Package the scripts so any result can be regenerated. Include a short table that maps each requirement from the first step to the result that shows whether it was met, and give the IMU datasheet values used so another student can repeat the study.

    Checkpoint: A report that reproduces every figure and explains drift behaviour in terms of the filter design.

Testing & validation

• Position and heading RMSE with GNSS available, against targets.
• Maximum drift during 5–40 s outages for two IMU grades.
• Effect of the non-holonomic constraint on outage drift.
• Hand-written EKF compared with the toolbox filter.
• Innovation statistics as a consistency check.

Expected results

A tuned IMU–GNSS fusion filter with measured accuracy on urban and highway routes and a clear quantification of drift through GNSS outages for two IMU grades.

Take it further

• Add wheel-speed odometry as a further measurement and measure outage drift again.
• Log real IMU data from the kit's MPU6050 and a USB GNSS receiver on a supervised campus loop and replay it offline.
• Compare loosely coupled fusion with an error-state filter.
• Use the localisation output to place the vehicle on a lane-level map.

Viva and review questions

Learn it at DIYguru

These DIYguru courses and programmes teach what this project needs.