State Estimation · Embedded Systems · Extended Kalman Filter
Overview
Every autonomous platform, whether in the air, on the ground, or underwater, has to answer the same question when GPS drops out: where am I? This project builds a working answer from scratch, starting with hardware selection and ending with a quantified, reproducible result on real driving data.
An Extended Kalman Filter was implemented entirely in Python/NumPy, without any Kalman filter library, and run against data collected from a custom ESP32-based navigation rig during an actual driving test. The filter went through three versions of increasing complexity, each one teaching something concrete about how inertial navigation fails and how to fix it.
Key result: The final 3D filter with tilt correction achieved 74.3m of drift over a 60-second GPS-denied window at highway speeds, reducing unaided drift by 98% compared to an uncorrected 3D filter (4,346m).
Hardware
ESP32 navigation rig with MPU6050 IMU (center) and NEO-6M GPS module (bottom)
6-DOF: 3-axis accelerometer and 3-axis gyroscope, no onboard sensor fusion. A chip like the BNO055 does fusion in hardware and just hands you a quaternion. The MPU6050 gives raw data only, so all the estimation has to be built and understood. Header pins were soldered on by hand.
Wired to the ESP32's hardware Serial2 rather than SoftwareSerial, which is what most example code uses. Hardware UART is a real peripheral, not bit-banged, and produces cleaner, more reliable data at higher baud rates. Getting it working required sorting out a mismatch between physical pin positions and the GPIO numbers on the board's silkscreen.
The narrow variant was chosen specifically because it fits a standard breadboard without blocking either row of header pins, which matters when you need to probe signals during debugging. A microSD module was also wired for standalone logging, but the final pipeline streamed CSV over USB serial to a laptop, which turned out to be simpler and more than fast enough.
Firmware and Data Collection
The firmware runs a non-blocking loop keyed off millis(), not delay().
GPS bytes are parsed on every pass through the loop so nothing is missed between
IMU samples. IMU readings go out at a fixed 20Hz. Everything streams as CSV over
USB serial; a Python/pyserial script on the laptop captures each row tagged with
both the ESP32's clock and the laptop's wall-clock time.
Each subsystem was tested in isolation before anything was combined. The stationary desk test found the first real problem immediately: the accelerometer reads about 31% high at rest (12.85 m/s² instead of 9.81 m/s²), a manufacturing calibration offset common on budget MPU6050 clones. A scale factor computed from that stationary window corrects it before any filter sees the data.
The first driving attempt produced corrupted data, every row truncated, values frozen across the whole drive. Comparing engine-on vs. engine-off while parked pointed to automotive electrical noise coupling into the exposed breadboard wiring. Repositioning the rig away from the main noise sources cleared it. The final collection run was 4.2 minutes, 4,987 samples at 20Hz, covering roughly 800-950m of real driving with GPS lock held throughout.
Filter Version 1: 2D Heading
The first filter tracked a 6-state 2D model: position (x, y), velocity (vx, vy), heading, and gyro bias, using a constant-turn-rate-and-acceleration model standard in ground-vehicle tracking. It was implemented from scratch because the point of the project is being able to explain the predict/update math, not just call a library function that does it.
The sanity check ran the filter with GPS fused continuously for the full drive. The estimate tracked ground truth essentially exactly, confirming the filter mechanics were right before moving to the harder case.
The real test withheld GPS for a 60-second window mid-drive covering roughly 650m at 40-54 km/h, forcing dead-reckoning on IMU alone. Unaided drift was 388m. A zero-velocity update (ZUPT) was added: when the vehicle appears stopped, the filter is told velocity is exactly zero, cutting drift to 150m (61% reduction).
Building the stillness detector exposed a non-obvious problem. A car cruising at steady speed and a car sitting still look almost identical to an accelerometer alone, since both produce close to zero net acceleration. The detector had to be rebuilt around gyroscope variance instead, with thresholds set empirically from known-stopped vs. known-cruising segments of the actual collected data.
Filter Versions 2 and 3: Full 3D Orientation
The 2D filter assumes the sensor's tilt relative to the car doesn't change. Version 2 removed that assumption by rebuilding around full 3D orientation tracking via quaternion integration, using all three gyro axes. Initial tilt came from the stationary gravity vector; initial heading from early GPS-observed motion. The Jacobian was computed numerically rather than derived analytically, a deliberate choice that trades a small amount of computation speed for correctness you can actually verify.
The first result was a genuine failure: unaided drift jumped to over 4,300 meters, far worse than the 2D version. The filter's internal roll estimate drifted past 30 degrees within about a minute. This is textbook unaided attitude divergence: with no continuous "which way is down" reference, small gyro bias errors on the roll and pitch axes integrate unchecked, and the resulting tilt error leaks into position through incorrect gravity cancellation.
Version 3 fixed this with a continuous accelerometer-based tilt correction. Whenever the accelerometer reads close to 1g, meaning the vehicle isn't under significant external acceleration, its direction is treated as a live "down" reference and used to correct roll and pitch without needing GPS. This is standard in production inertial navigation systems. Unaided drift dropped to 74.3 meters, better than the original 2D filter.
Adding ZUPT back on top of the tilt-corrected filter made things worse (365m, not better). The stillness detector only caught the stop in the final second of the 60-second window. By then, the filter's velocity estimate had wandered, and the abrupt correction interrupted a trajectory that was already converging back toward truth on its own via the tilt correction. This is a systems-engineering lesson worth knowing: correction techniques don't always stack. A technique that rescues a weaker model can actively hurt a stronger one.
Drift growth over the 60-second GPS-denied window, all filter versions
Results
| Filter Version | Unaided drift (60s GPS-denied) | With correction |
|---|---|---|
| 2D heading-only | 388 m | 150 m (ZUPT, -61%) |
| 3D orientation, no tilt correction | 4,346 m | 174 m (ZUPT) |
| 3D orientation, with tilt correction | 74.3 m | 365 m (ZUPT, net negative) |
Full-drive trajectory comparison: GPS ground truth vs. EKF estimate with GPS-denied window
Notes
The filter mechanics, EKF predict/update equations, quaternion attitude kinematics, numerical Jacobians, were all derived and debugged from first principles. Every calibration number came from the project's own collected data, not a datasheet. The pipeline runs end-to-end on real logged data, not a simulation.
The debugging steps were just as instructive as the math: tracing the automotive EMI problem from corrupted rows to engine noise, figuring out that gyroscope variance is the right signal for stillness detection rather than accelerometer variance, and correctly diagnosing why ZUPT hurt the tilt-corrected filter instead of helping it. That last result is easy to hide. Reporting it is more useful.