Path Following Control System

Make a differential-drive wagon autonomously track a parametric reference trajectory within a fixed time budget, using only noisy GPS (1 Hz) and IMU (20 Hz) as sensors. The GPS alone is too low-rate and too noisy to drive a controller; the IMU alone drifts; and the reference path varies sharply in curvature, so any controller tuned for straight-line tracking falls apart on the tight sections.

Layered the problem into four clean stages: sensor fusion, path following, motor control, and kinematics. Each stage has a single responsibility and can be tuned, swapped or unit-tested on its own — which made it possible to characterise the system's failure modes precisely rather than just tweaking gains until it looked right.

State estimation uses an Extended Kalman Filter with a 5-state vector [pₓ, pᵧ, θ, v, gyro_bias], fusing the two sensor streams and estimating the gyro bias online. Path following is Pure Pursuit with a velocity-adaptive lookahead; motor control is a PI loop with anti-windup; kinematics is standard differential drive.

The EKF adapts its own trust in each sensor based on path curvature — scaling heading uncertainty up ~8× through high-curvature sections so the filter trusts GPS position fixes more than the drifting gyro estimate. Mahalanobis outlier rejection drops obviously-bad GPS updates, with its threshold also scaled by curvature (2.5× at κ = 1.0 rad/m) to stay tolerant during hard turns.

The Pure Pursuit lookahead is L = 0.9|v| + 0.3, clamped to [0.5 m, 2.0 m] — long lookahead at speed for smoothness, short lookahead at low speed for tight tracking. A time-parameterised reference trajectory (Lemniscate of Gerono) guarantees mission completion within the 20-second budget. The inner loop runs velocity and angular-velocity PI control with integral anti-windup clamped at ±0.5.

To validate the system I built a statistical test harness: 98 automated runs across randomised sensor noise, with full per-run metrics (tracking error, completion time, outlier counts) and a parameter sweep framework for gain tuning.

Mean tracking error of 6.53 m ± 2.2 m L2 over 98 runs, with 94.9% of runs under 10 m and 0% failure rate. Best single run was 3.69 m — close to the theoretical lower bound given GPS noise of ±0.5 m. Per-sample tracking error landed around 325 mm.

The layered structure and the test harness are also what make this the most interesting project to open source — each layer is swappable, which is exactly the playground the Milestone 5 interactive demo is built around (pick a controller, dial the noise, choose a path, watch the trajectory).