The most important openpilot release yet!
First of all, we need to talk about how we use the term GPS when we actually mean GNSS. Since 1995, 24 GPS satellites have been launched by the US, but GPS is just the name for the US constellation of satellites and there are many other Global Navigation Satellite System (GNSS) and local systems launched by other countries:
All these satellites have one-way communication to a GNSS receiver on the ground like your phone or a comma three. How do we turn the received information into a position? Each satellite carries a very accurate atomic clock. The satellite continually broadcasts a signal that includes the transmission time and a synchronization marker. When your comma three receives this signal it can figure out the time it took to travel in space by comparing it to its own clock. The difference between transmit and received time divided by the speed of light gets us the distance to the satellite. For example the travel time of GPS is at least 0.067 seconds: height of GPS (20.000km) / speed of light (~300.000km/s).
Now that we have the distance to a satellite (which is called a pseudorange), we need to figure out where the satellite is located in space. Using the position of the satellite and the distance to it our position can be derived. Figuring out the satellite’s orbit can be done in two ways: receive it from the satellite which sends its ephemeris (aka trajectory) every 30 seconds or download the data from the internet. NASA publishes past and predicted orbits multiple times per day on an ftp server. These predicted orbits are more accurate than what’s broadcasted by the satellites themselves. laikad can use both, and prefers the one from the internet when it’s available.
By using trilateration with pseudoranges of different satellites, we can calculate our position. Each position and distance to a satellite allows us to draw a sphere where we should be on. Three satellites should provide us the exact position on earth as can be seen in the image below. However there is one variable that we assumed to be correct: the clock of the satellite and our comma three doesn’t tick at the same speed which causes minor differences in time and large difference in distance. We need a minimum of four satellites to estimate this clock error variable.
All these steps provide us with a decent position on earth within ~50 meters. However, for navigation we require higher accuracy such that we for example know which lane we are driving in. The signal is affected when it travels through the ionosphere and troposphere (aka atmosphere) of our earth where the signal is damped, refracted and delayed. This delay is specific to the comma three’s position and the position of the satellite (e.g. standing on a mountain results in less travel distance through the atmosphere. While a satellite close to the horizon would increase this distance), by using our previous estimated position and correction data from the internet we can correct the delays for each satellite which result in a final position estimate with an error of less than a meter.
Find out more and every little detail on GNSS in the book “GNSS Springer Handbook of Global Navigation Satellite Systems”.
Now you know all the basic steps to calculate your position anywhere on earth! And fortunately, we provide an open-source Python library Laika to do all these steps for you. Checkout the Walkthrough notebook for the code and more visualisations of the steps described previously.
Laika has already been used in production for quite some time in the training pipeline of our models. It is used to calculate the trajectories our users have driven, which we use to train driving models.
Now we are going to use the same code to compute the position in realtime while driving. Over the last few months, the library has been improved to handle realtime processing on the device, by reducing data usage and speedups which also lead to reduced power usage. And in general better usability of the library after many refactors.
Locationd uses a Kalman filter to combine different measurements to get a good estimate of the current state (e.g. position and velocities). A Kalman filter can take into account uncertainties of the measurements when they are processed. E.g. a noisy or uncertain measurement should have less influence on the state then a good measurement.
In our current setup two separate Kalman filters are used. One is run on the U-blox module, and uses just GNSS measurements to estimate the position and velocity of the receiver. The position and velocity outputs from the filter in the U-blox module are then used as inputs to a second Kalman filter in locationd to be fused with other measurements from the IMU and camera. This is called a loosely coupled setup.
However in this setup a lot of information about the uncertainty of individual GNSS measurements is lost. It’s better to integrate everything into a single tightly coupled Kalman filter. Laikad outputs all the raw GNSS measurements in corrected form, ready for use by the Kalman filter in locationd.
Written by: Gijs Koning
This work is the result of a 3 month Research internship at comma.ai in Rotterdam, The Netherlands. An amazing experience which I can definitely recommend.