A while back, I was working on localization with GPs and had a thought: could we encode vehicle dynamics directly into the GP kernel?
I know GPs are used to model parameters in physical models. But my idea was that a car’s trajectory resembles a smooth GP sample. A faster car takes smoother paths, just like longer length scales produce smoother GPs. Instead of modeling `y(x)` directly, I used cumulative distance `s` as the input, and trained two separate GPs:
* `x(s)`
* `y(s)`
Both use an RBF kernel. So we are basically maximizing the probability function:
https://preview.redd.it/ksoisiw9r9ef1.png?width=430&format=png&auto=webp&s=e01f1827f3c74550f596de2ee02fe4b7d2e93178
Which translates to something like
*“Given a speed, how probable is it that these data points came from this vehicle?”*
**The algorithm goes like this:**
1. Collect data
2. Optimize the kernel
3. Construct the `l(v)` function
4. Optimize the lap
I fitted the kernel’s length scale `l` as a function of speed: `l(v)`. To do this, I recorded driving data in batches at different constant speeds, optimized the GP on each batch, then fit a simple `l(v)` relation, which turned out to be very linear.
With the optimized kernel in hand, you can ask questions like:
*“Given this raceline and a speed, can my car follow it?”*
As the GP is a probabilistic model, it doesn’t give a binary answer that we requested. We could optimize for “the most likely speed” the same way we optimized the length scales. However, this would be more like asking, “What is the most likely speed this raceline can be achieved?”, which is okay for keeping your Tesla on the road, but not optimal for racing. My approach was to define an acceptable tolerance for the deviation from the raceline. With these constraints in hand, I run a heuristic window-based optimization for a given raceline:
**Results?**
Simulator executed lap plan times were close to human-driven laps. The model didn’t account for acceleration limits, so actual performance fell slightly short of the predicted plan, but I think it proved the concept.
There are a lot of things that could be improved in the model. One of the biggest limitations is the independent models for x and y coordinates. Some of the things I also tried:
1. Absolute angle and cumulative distance model – This one considers the dynamics in terms of the absolute heading angle with respect to cumulative distance. This solves the problem of intercorrelation between X and Y coordinates, but introduces two more problems. First, to go back from the angle-domain, you need to integrate. This will lead to drifting errors. And even if you don’t want to go back to trajectory space, you still lose the direct link between the error definition of the two domains. And second, this function is not entirely smooth, so you need a fancier Kernel to capture the features. A Matérn at least.
2. “Unfolding the trajectory” – This was one of my favorites, since it is the closest to the analogy of modeling y relation to x directly, wiggly road style. In the original domain, you would face the multivalued problem, where for a single x-value, there can be multiple y-values. One can “unfold” the lap (loop) by reducing the corner angles until you have unfolded the points to a single-valued function. This, however, also destroys the link to the original domain error values.
Here is the code and the data if you want to make it better:
[https://github.com/Miikkasna/gpdynalgo](https://github.com/Miikkasna/gpdynalgo)
Trend Engine: AI-Powered News & Trends 
