Smooth GPS data

Viewed 122362

I'm working with GPS data, getting values every second and displaying current position on a map. The problem is that sometimes (specially when accuracy is low) the values vary a lot, making the current position to "jump" between distant points in the map.

I was wondering about some easy enough method to avoid this. As a first idea, I thought about discarding values with accuracy beyond certain threshold, but I guess there are some other better ways to do. What's the usual way programs perform this?

12 Answers

What you are looking for is called a Kalman Filter. It's frequently used to smooth navigational data. It is not necessarily trivial, and there is a lot of tuning you can do, but it is a very standard approach and works well. There is a KFilter library available which is a C++ implementation.

My next fallback would be least squares fit. A Kalman filter will smooth the data taking velocities into account, whereas a least squares fit approach will just use positional information. Still, it is definitely simpler to implement and understand. It looks like the GNU Scientific Library may have an implementation of this.

One method that uses less math/theory is to sample 2, 5, 7, or 10 data points at a time and determine those which are outliers. A less accurate measure of an outlier than a Kalman Filter is to to use the following algorithm to take all pair wise distances between points and throw out the one that is furthest from the the others. Typically those values are replaced with the value closest to the outlying value you are replacing

For example

Smoothing at five sample points A, B, C, D, E

ATOTAL = SUM of distances AB AC AD AE

BTOTAL = SUM of distances AB BC BD BE

CTOTAL = SUM of distances AC BC CD CE

DTOTAL = SUM of distances DA DB DC DE

ETOTAL = SUM of distances EA EB EC DE

If BTOTAL is largest you would replace point B with D if BD = min { AB, BC, BD, BE }

This smoothing determines outliers and can be augmented by using the midpoint of BD instead of point D to smooth the positional line. Your mileage may vary and more mathematically rigorous solutions exist.

As for least squares fit, here are a couple other things to experiment with:

  1. Just because it's least squares fit doesn't mean that it has to be linear. You can least-squares-fit a quadratic curve to the data, then this would fit a scenario in which the user is accelerating. (Note that by least squares fit I mean using the coordinates as the dependent variable and time as the independent variable.)

  2. You could also try weighting the data points based on reported accuracy. When the accuracy is low weight those data points lower.

  3. Another thing you might want to try is rather than display a single point, if the accuracy is low display a circle or something indicating the range in which the user could be based on the reported accuracy. (This is what the iPhone's built-in Google Maps application does.)

You can also use a spline. Feed in the values you have and interpolate points between your known points. Linking this with a least-squares fit, moving average or kalman filter (as mentioned in other answers) gives you the ability to calculate the points inbetween your "known" points.

Being able to interpolate the values between your knowns gives you a nice smooth transition and a /reasonable/ approximation of what data would be present if you had a higher-fidelity. http://en.wikipedia.org/wiki/Spline_interpolation

Different splines have different characteristics. The one's I've seen most commonly used are Akima and Cubic splines.

Another algorithm to consider is the Ramer-Douglas-Peucker line simplification algorithm, it is quite commonly used in the simplification of GPS data. (http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm)

Here's a Javascript implementation of @Stochastically's Java implementation for anyone needing it:

class GPSKalmanFilter {
  constructor (decay = 3) {
    this.decay = decay
    this.variance = -1
    this.minAccuracy = 1
  }

  process (lat, lng, accuracy, timestampInMs) {
    if (accuracy < this.minAccuracy) accuracy = this.minAccuracy

    if (this.variance < 0) {
      this.timestampInMs = timestampInMs
      this.lat = lat
      this.lng = lng
      this.variance = accuracy * accuracy
    } else {
      const timeIncMs = timestampInMs - this.timestampInMs

      if (timeIncMs > 0) {
        this.variance += (timeIncMs * this.decay * this.decay) / 1000
        this.timestampInMs = timestampInMs
      }

      const _k = this.variance / (this.variance + (accuracy * accuracy))
      this.lat += _k * (lat - this.lat)
      this.lng += _k * (lng - this.lng)

      this.variance = (1 - _k) * this.variance
    }

    return [this.lng, this.lat]
  }
}

Usage example:

   const kalmanFilter = new GPSKalmanFilter()
   const updatedCoords = []

    for (let index = 0; index < coords.length; index++) {
      const { lat, lng, accuracy, timestampInMs } = coords[index]
      updatedCoords[index] = kalmanFilter.process(lat, lng, accuracy, timestampInMs)
    }

I have transformed the Java code from @Stochastically to Kotlin

class KalmanLatLong
{
    private val MinAccuracy: Float = 1f

    private var Q_metres_per_second: Float = 0f
    private var TimeStamp_milliseconds: Long = 0
    private var lat: Double = 0.toDouble()
    private var lng: Double = 0.toDouble()
    private var variance: Float =
        0.toFloat() // P matrix.  Negative means object uninitialised.  NB: units irrelevant, as long as same units used throughout

    fun KalmanLatLong(Q_metres_per_second: Float)
    {
        this.Q_metres_per_second = Q_metres_per_second
        variance = -1f
    }

    fun get_TimeStamp(): Long { return TimeStamp_milliseconds }
    fun get_lat(): Double { return lat }
    fun get_lng(): Double { return lng }
    fun get_accuracy(): Float { return Math.sqrt(variance.toDouble()).toFloat() }

    fun SetState(lat: Double, lng: Double, accuracy: Float, TimeStamp_milliseconds: Long)
    {
        this.lat = lat
        this.lng = lng
        variance = accuracy * accuracy
        this.TimeStamp_milliseconds = TimeStamp_milliseconds
    }

    /// <summary>
    /// Kalman filter processing for lattitude and longitude
    /// https://stackoverflow.com/questions/1134579/smooth-gps-data/15657798#15657798
    /// </summary>
    /// <param name="lat_measurement_degrees">new measurement of lattidude</param>
    /// <param name="lng_measurement">new measurement of longitude</param>
    /// <param name="accuracy">measurement of 1 standard deviation error in metres</param>
    /// <param name="TimeStamp_milliseconds">time of measurement</param>
    /// <returns>new state</returns>
    fun Process(lat_measurement: Double, lng_measurement: Double, accuracy: Float, TimeStamp_milliseconds: Long)
    {
        var accuracy = accuracy
        if (accuracy < MinAccuracy) accuracy = MinAccuracy

        if (variance < 0)
        {
            // if variance < 0, object is unitialised, so initialise with current values
            this.TimeStamp_milliseconds = TimeStamp_milliseconds
            lat = lat_measurement
            lng = lng_measurement
            variance = accuracy * accuracy
        }
        else
        {
            // else apply Kalman filter methodology

            val TimeInc_milliseconds = TimeStamp_milliseconds - this.TimeStamp_milliseconds

            if (TimeInc_milliseconds > 0)
            {
                // time has moved on, so the uncertainty in the current position increases
                variance += TimeInc_milliseconds.toFloat() * Q_metres_per_second * Q_metres_per_second / 1000
                this.TimeStamp_milliseconds = TimeStamp_milliseconds
                // TO DO: USE VELOCITY INFORMATION HERE TO GET A BETTER ESTIMATE OF CURRENT POSITION
            }

            // Kalman gain matrix K = Covarariance * Inverse(Covariance + MeasurementVariance)
            // NB: because K is dimensionless, it doesn't matter that variance has different units to lat and lng
            val K = variance / (variance + accuracy * accuracy)
            // apply K
            lat += K * (lat_measurement - lat)
            lng += K * (lng_measurement - lng)
            // new Covarariance  matrix is (IdentityMatrix - K) * Covarariance
            variance = (1 - K) * variance
        }
    }
}
Related