What it did
Given (a) a pre-built map of known landmark positions and (b) a stream of noisy LIDAR observations plus IMU/wheel-odometry, estimate where the vehicle actually is. Implementation is a 2D particle filter in C++ talking to Udacity’s term-2 simulator.
The pipeline
For each timestep:
- Motion update. Propagate every particle forward by the
commanded
(velocity, yaw_rate)plus Gaussian noise on each. - Sensor update. For each particle’s hypothesized pose:
- Transform observed landmarks from car frame → world frame.
- Data-associate observed → nearest map landmark.
- Compute a multivariate Gaussian likelihood across the matched pairs.
- Assign the particle a weight ∝ likelihood.
- Resample. Draw N new particles from the discrete distribution defined by the weights. Particles in low-likelihood regions die; high-likelihood ones split.
After ~10 steps the cloud collapses around the true pose.
What was actually tricky
- Coordinate frames. Three of them — car body, sensor, world. Forgetting to apply the sensor mount offset rotates the entire estimate by a few degrees, which compounds.
- Particle starvation. Too few particles + a sharp likelihood function = the filter collapses to one wrong hypothesis and never recovers. The fix is either more particles or a fat-tailed likelihood (don’t square the residuals too aggressively).
- Nearest-neighbor data association breaks at clutter. If two landmarks are close to one observation, the wrong association pulls particles toward the wrong cluster. Robust gating helps.
What I’d do differently with hindsight
- MCL with adaptive sample size (KLD-sampling). Use more particles when uncertainty is high, fewer when localized. Pays for itself within a few seconds of runtime.
- Use the map’s structure, not just landmarks. If you have a grid-occupancy map, scan-matching (correlative or ICP) is more data-efficient than nearest-neighbor landmark association.
- Initialize from GPS prior. The Udacity project starts the filter from a known initial pose, which is unrealistic. A real cold-start needs a wide-prior reinitialization mode.
What it taught me
Probabilistic robotics finally clicked here. Particles are not estimates of the pose; they’re samples from a posterior distribution. The filter doesn’t “find” the pose, it converges the distribution. That distinction is the difference between thinking deterministically and thinking like a roboticist.