Smooth GPS data

问题:

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?

回答1:

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.

回答2:

Here\'s a simple Kalman filter that could be used for exactly this situation. It came from some work I did on Android devices.

General Kalman filter theory is all about estimates for vectors, with the accuracy of the estimates represented by covariance matrices. However, for estimating location on Android devices the general theory reduces to a very simple case. Android location providers give the location as a latitude and longitude, together with an accuracy which is specified as a single number measured in metres. This means that instead of a covariance matrix, the accuracy in the Kalman filter can be measured by a single number, even though the location in the Kalman filter is a measured by two numbers. Also the fact that the latitude, longitude and metres are effectively all different units can be ignored, because if you put scaling factors into the Kalman filter to convert them all into the same units, then those scaling factors end up cancelling out when converting the results back into the original units.

The code could be improved, because it assumes that the best estimate of current location is the last known location, and if someone is moving it should be possible to use Android\'s sensors to produce a better estimate. The code has a single free parameter Q, expressed in metres per second, which describes how quickly the accuracy decays in the absence of any new location estimates. A higher Q parameter means that the accuracy decays faster. Kalman filters generally work better when the accuracy decays a bit quicker than one might expect, so for walking around with an Android phone I find that Q=3 metres per second works fine, even though I generally walk slower than that. But if travelling in a fast car a much larger number should obviously be used.

public class KalmanLatLong {
    private final float MinAccuracy = 1;

    private float Q_metres_per_second;    
    private long TimeStamp_milliseconds;
    private double lat;
    private double lng;
    private float variance; // P matrix.  Negative means object uninitialised.  NB: units irrelevant, as long as same units used throughout

    public KalmanLatLong(float Q_metres_per_second) { this.Q_metres_per_second = Q_metres_per_second; variance = -1; }

    public long get_TimeStamp() { return TimeStamp_milliseconds; }
    public double get_lat() { return lat; }
    public double get_lng() { return lng; }
    public float get_accuracy() { return (float)Math.sqrt(variance); }

    public void SetState(double lat, double lng, float accuracy, long TimeStamp_milliseconds) {
        this.lat=lat; this.lng=lng; variance = accuracy * accuracy; this.TimeStamp_milliseconds=TimeStamp_milliseconds;
    }

    /// <summary>
    /// Kalman filter processing for lattitude and longitude
    /// </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>
    public void Process(double lat_measurement, double lng_measurement, float accuracy, long TimeStamp_milliseconds) {
        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

            long 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 * 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
            float 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;
        }
    }
}

回答3:

This might come a little late...

I wrote this KalmanLocationManager for Android, which wraps the two most common location providers, Network and GPS, kalman-filters the data, and delivers updates to a LocationListener (like the two \'real\' providers).

I use it mostly to \"interpolate\" between readings - to receive updates (position predictions) every 100 millis for instance (instead of the maximum gps rate of one second), which gives me a better frame rate when animating my position.

Actually, it uses three kalman filters, on for each dimension: latitude, longitude and altitude. They\'re independent, anyway.

This makes the matrix math much easier: instead of using one 6x6 state transition matrix, I use 3 different 2x2 matrices. Actually in the code, I don\'t use matrices at all. Solved all equations and all values are primitives (double).

The source code is working, and there\'s a demo activity. Sorry for the lack of javadoc in some places, I\'ll catch up.

回答4:

You should not calculate speed from position change per time. GPS may have inaccurate positions, but it has accurate speed (above 5km/h). So use the speed from GPS location stamp. And further you should not do that with course, although it works most of the times.

GPS positions, as delivered, are already Kalman filtered, you probably cannot improve, in postprocessing usually you have not the same information like the GPS chip.

You can smooth it, but this also introduces errors.

Just make sure that your remove the positions when the device stands still, this removes jumping positions, that some devices/Configurations do not remove.

回答5:

I usually use the accelerometers. A sudden change of position in a short period implies high acceleration. If this is not reflected in accelerometer telemetry it is almost certainly due to a change in the \"best three\" satellites used to compute position (to which I refer as GPS teleporting).

When an asset is at rest and hopping about due to GPS teleporting, if you progressively compute the centroid you are effectively intersecting a larger and larger set of shells, improving precision.

To do this when the asset is not at rest you must estimate its likely next position and orientation based on speed, heading and linear and rotational (if you have gyros) acceleration data. This is more or less what the famous K filter does. You can get the whole thing in hardware for about $150 on an AHRS containing everything but the GPS module, and with a jack to connect one. It has its own CPU and Kalman filtering on board; the results are stable and quite good. Inertial guidance is highly resistant to jitter but drifts with time. GPS is prone to jitter but does not drift with time, they were practically made to compensate each other.

回答6:

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.

回答7:

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)

回答8:

Going back to the Kalman Filters ... I found a C implementation for a Kalman filter for GPS data here: http://github.com/lacker/ikalman I haven\'t tried it out yet, but it seems promising.

回答9:

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.)

回答10:

Mapped to CoffeeScript if anyones interested. **edit -> sorry using backbone too, but you get the idea.

Modified slightly to accept a beacon with attribs

{latitude: item.lat,longitude: item.lng,date: new Date(item.effective_at),accuracy: item.gps_accuracy}

MIN_ACCURACY = 1

# mapped from http://stackoverflow.com/questions/1134579/smooth-gps-data

class v.Map.BeaconFilter

  constructor: ->
    _.extend(this, Backbone.Events)

  process: (decay,beacon) ->

    accuracy     = Math.max beacon.accuracy, MIN_ACCURACY

    unless @variance?
      # if variance nil, inititalise some values
      @variance     = accuracy * accuracy
      @timestamp_ms = beacon.date.getTime();
      @lat          = beacon.latitude
      @lng          = beacon.longitude

    else

      @timestamp_ms = beacon.date.getTime() - @timestamp_ms

      if @timestamp_ms > 0
        # time has moved on, so the uncertainty in the current position increases
        @variance += @timestamp_ms * decay * decay / 1000;
        @timestamp_ms = beacon.date.getTime();

      # 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
      _k  = @variance / (@variance + accuracy * accuracy)
      @lat = _k * (beacon.latitude  - @lat)
      @lng = _k * (beacon.longitude - @lng)

      @variance = (1 - _k) * @variance

    [@lat,@lng]