Algorithms

Midline Generation

Given cone detections, we want to compute the centerline that represents the desired driving path and then pass this midline to Controls. A centerline maximizes distance from left and right cones and naturally follows the track, making it very safe compared to more optimal racelines.

Geometric Methods

• Examples:
  • Delaunay triangulation
  • Voronoi diagrams
  • Nearest-neighbor pairing
  • Midpoint generation

Advantages	Disadvantages
Extremely fast Easy to debug Deterministic No training data	Sensitive to missing cones Requires reasonably accurate cone positions

SVM-Based Midline

We can treat left/right cone assignment as a classification problem and train a Support Vector Machine (SVM) separating blue and yellow cones. Since we don’t have enough data to train a good SVM model, we augment more points by creating new cones around each blue and yellow cone. We do that for every point in a grid in front of the car (a grid with adjustable length and width) and if we see that at a certain point the classification flips, we assume that that point is part of the midline. To optimize this, we perform binary search across the grid for a starting point on the midline, and then do flood fill starting from that point, which we found cut runtime by a factor of 5.

Advantages	Disadvantages
Robust to cone dropout Robust to occasional misclassification Produces smooth centerlines	Slightly more computationally expensive Still assumes sufficient cone observations

We implement the SVM using libSVM, which provides the C-SVC classifier and different kernel functions. The final implementation uses an RBF (Radial Basis Function) kernel, allowing the model to learn a nonlinear decision boundary between blue and yellow cones.

Why We Chose Geometric Methods

We primarily use geometric methods because of their deterministic behavior, low latency, simplicity, and easy debugging. SVM-based approaches remain a useful alternative when robustness to perceptions failures becomes more important. The geometric method we use is nearest-neighbor pairing where for each blue cone we take the nearest yellow and append their midpoint to the midline, and vice versa for the yellow cones. If cones come in uncolored, we use a Delaunay triangulation-based recoloring algorithm that constructs a graph connecting neighboring cones, identifies likely track-width edges using geometric heuristics such as expected track width, heading consistency, and proximity, then assigns cones to the left (blue) or right (yellow) side based on the estimated track direction. The midline then is created from the selected-edge midpoints.

Localization (Known Map)

Localization for events where a prior map exists (eg. Skidpad) is a lot easier because we can assume what the track will look like up to a certain amount of accuracy. Using IMU data, cone observations, and the prior track map, we calculate an accurate estimation of our vehicle position and heading within the known map. Without localization in Skidpad for example, the planner cannot create an accurate trajectory for Controls to follow, as well as determine the current lap, current circle, transition points, and finish condition.

Filter Comparison

A filter maintains a running estimate of the vehicle’s state (position, heading) by cycling between two steps: predict, using the motion model (odometry/IMU) to propagate the previous estimate forward, and update, correcting that prediction against new sensor observations (cone detections). The Bayes filter is the general probabilistic formulation of this predict-update cycle — EKF, UKF, and Particle Filter are practical approximations of it, differing in how they represent and propagate uncertainty.

Filter	State Representation	Handles Nonlinearity	Multimodal Support	Compute Cost	Notes
Bayes Filter	Full probability distribution	Yes (exact, in theory)	Yes	Intractable for continuous state	Foundational framework; not directly implementable, EKF/UKF/PF approximate it
EKF	Single Gaussian	Linearizes via first-order Taylor expansion	No	Low	Fast and simple, but can diverge under strong nonlinearity
UKF	Single Gaussian	Captures nonlinearity via sigma points (no linearization)	No	Moderate	More accurate than EKF for nonlinear motion, no Jacobians needed
Particle Filter	Set of weighted samples	Yes (arbitrary)	Yes	High	Best for ambiguous/multimodal cases, needs many particles for accuracy

EKF Localization

Extended Kalman Filter (EKF) localization estimates the vehicle’s pose by first predicting its position using odometry and IMU data, then associating observed cones with cones in the known track map. The difference between the expected and observed cone positions is used to update the vehicle’s pose, with the correction weighted according to the uncertainty in both the prediction and the measurements.

Advantages	Disadvantages
Fast Simple Well understood	Can drift Accuracy depends on noise tuning

There are several ways to associate observed cones with the map, summarized below:

Method	Description	Speed	Robustness	Average Pose Error* (m)	Average Yaw Error* (rad)
Nearest Neighbor (Greedy)	Greedily matches each observed cone to the mapped cone with the closest predicted range/bearing	Extremely fast	Susceptible to incorrect matches when pose estimate is inaccurate	0.105	0.0197
JCBB (Joint Compatibility Branch and Bound)	Evaluates candidate associations jointly, selecting a statistically compatible set given pose uncertainty	Slower	More robust in ambiguous cone layouts	0.122	0.0203
ICP (Iterative Closest Point)	Aligns observed cones to mapped cones in the horizontal plane, then does one-to-one nearest-neighbor matching post-alignment	Moderate	Works best when initial pose estimate is already close	0.095	0.0156

*Measured by running each association method on a simulated Skidpad track with injected map and sensor noise.

Particle Filter

Particle Filter localization estimates the vehicle’s pose by maintaining a set of possible vehicle poses (particles) rather than a single estimate. Each particle represents a hypothesis of the vehicle’s location and orientation. As the vehicle moves, particles are propagated using the motion model, then weighted based on how well the expected cone observations from that pose match the actual sensor measurements. Particles with higher weights are resampled, causing the estimate to converge toward the true vehicle pose.

Advantages	Disadvantages
Better with nonlinear distributions Handles multimodal uncertainty Handles ambiguous environments well	Computationally expensive Requires a large number of particles for good accuracy Performance depends on careful tuning of the motion and measurement models

Why We Chose EKF Localization

For the Skidpad event, the vehicle begins from a known location on a predefined map, making the localization uncertainty relatively small. Under these conditions, an Extended Kalman Filter provides comparable accuracy with significantly lower computational cost and implementation complexity. As a result, EKF was selected as the primary localization method, while Particle Filters remain a viable alternative for scenarios with greater pose uncertainty or more complex environments. For the cone association, we found that ICP gave the most accurate and robust results during testing, so that is what we use.