Moving vehicle localization with Gaussian Process
In the previous post, I presented a simple autonomous vehicle project. I left the localization part open and have been since searching for different methods to finish it. Time of Flight and Received Signal Strength Indication are both promising methods for this scale, but neither works without uncertainty. And whenever there is uncertainty, there is a slot for Gaussian Process.
In this story, we will test the performance of the GP for localization problems on 2D plane. Python and Sci-Kit Learn are used for the implementation. A link to the code and used data is at the end.
Gaussian Process
Everything you need to know (and a little more) about GP is in the book Gaussian Processes for Machine Learning written by C. E. Rasmussen. Here comes a practical way to see it.
I like to think the GP as an infinitely big box of random functions. However, the functions are not completely random, because they have some constraints (priors) which make functions behaving in a certain way more common. Somewhere in that box lays a function that explains our system completely. When we give data to the GP we add more constraints. Functions that won’t fit the given data are discarded, resulting in a posterior distribution of functions.
A kernel for the GP is given as prior information. There are many different kernels to choose from. The kernel and its parameters define how the functions are behaving.
Predictions with GP are made by taking the mean of the function distribution. The uncertainty of the predictions is easily available as it is built in the GP. This comes in handy for localization problems, as we can directly observe how certain we are about the location.
Data simulation
OpenAI Gym has pretty decent 2D racing simulator. I did one lap on the simulator and saved the xy-coordinates and the speed with respect to time. The pixel coordinates were scaled and converted to meters to match 1000m² training plane.
Time of Flight is a measuring method, which means the time that it takes for a wave to travel from a sensor to the object and back. Since we know how fast light travels, we can calculate the distance between the sensor and the object. Three sensors are enough to trilaterate 2D location of an object to a known reference plane. We are not going to simulate ToF itself, but add the estimated noise to the existing data. Referring to this publication we will use a standard deviation of ±15 cm for the ToF position error. The noise is considered normally distributed with zero mean.
Received Signal Strength Indication tells us the received signal strength from transmitter to receiver. This can be used to define the distance between the receiving sensor and the transmitting object, as being further away from the transmitter means virtually weaker signal. After that, defining the position in the 2D plane works the same as for ToF. Interpreting the accuracies of this paper, we might expect something like ±1.0 m for raw distance measurement errors. Let’s add this to our simulated location data to generate RSSI data points.
In reality, both of the methods can achieve a sample rate of several hertz with a sophisticated system. We will sample the simulation data with 1 Hz for ToF and 5 Hz for RSSI. Here is the data:
Route predictions
Let’s use GP to predict the route of the car from the simulated data. In the code below, X_train is time and Y_train is x and y coordinates. Both extracted from the sampled simulation data above. Alphas is an array of expected uncertainty (noise) for individual datapoints.
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBFkernel = 1.0 * RBF(length_scale=1.0, length_scale_bounds=(1e-3, 1000.0))
gpr = GaussianProcessRegressor(kernel=kernel, alpha=alphas, random_state=0, n_restarts_optimizer=100)
gpr.fit(X_train, Y_train)
pred = gpr.predict(real_pos[:, 0].reshape(-1, 1))
plt.plot(real_pos[:, 2], real_pos[:, 3])
plt.plot(pred[:, 0], pred[:, 1])
This gives us route estimates:
Now one can think, why not combine the data? If we do, we have to reconsider the expected noise for the points. We already know that not all data is evenly accurate. As we saw earlier, we can define the error rates for individual points for GP as an array. To see the effect of noise assumptions, let’s have it both ways. One for constant error and one for individually defined by the measurement source:
As can be seen from the results, it gives a great advantage to be able to weigh points using prior knowledge. So far we have predicted the route, but let’s move to a bit more dynamic predictions.
Real-time localization
The difference between route estimation and real-time localization is in the amount of data. Real-time localization can use only the data available at that specific time. We can simulate this by iterating through the simulation data and making the prediction of the current position at a specific time. We can also bring in the estimated uncertainty as a confidence interval. Below is a visualization and the results of the localization. There are also results of Nearest Neighbors algorithms for reference.
We can see that the localization accuracy is worse than in the route estimations. I wouldn’t blame Gaussian Process for that, since both nearest neighbor algorithms performed even worse. Let’s analyze that a bit more in the final part.
Making future predictions
One could say Nearest Neighbor is bad for extrapolation since it cannot see anything that is not in the data. Linear Regression is probably the opposite but is still just a guess if we don’t have data to confirm it. The same applies to GP, but in addition, there is a feature that causes certain behavior; When going further away from the data points, GP converges to the prior mean. This might sound like a dumb feature but compare it to linear regression where the point (your vehicle) maintains the course to the infinity or nearest neighbor where the point stays in place indefinitely. Extraploation is always case dependent and not trivial.
The animation shows that the GP is going toward zero when there is no data guiding it anywhere. However, the possibility to extrapolate can be useful in cases, where the sample rate is low. The cool thing is, that the GP takes into account the current speed and the course trend when it makes the predictions. By adjusting the kernel length scale one can change how long the course and the speed are affecting the extrapolation. What comes to the inaccuracy of real-time localization, like many other algorithms GP is just more comfortable in the middle of the data points than at the edges.
For all the examples here, the hyperparameters were learned during the fitting. One interesting approach would be to gather more training data from different cases and try to find fixed kernel parameters for modeling the car dynamics. Going deeper into the deterministic dynamics of the system, however, is more a place for a Kalman Filter.
These results are promising and I think this method can be useful in real localization tasks. The beauty of using the Gaussian Process for this purpose is its easy use. The dynamics of your vehicle is already in the system, completely estimated from the data.
The data is available in the repository below, so you can try your own methods to improve the results. Leave your thoughts in the comments!
Here is the code and the data:
