Smooth GPS data

Question

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?

Solution 1

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;
        }
    }
}

Solution 2

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.

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

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

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

View more solutions

Comments

[heltonbiker]

I feel the bad effects of the "GPS noise" when trying to calculate associated (derivative) values like speed and slope, which are very discontinuous specially for high sample rate tracklogs (since time has integer [one second] resolution).

[heltonbiker]

(also, if you are navigating through main roads, you can use "snap to roads" algorithm provided you have a good [correct, precise] roadmap dataset. Just a thought)

[ViruMax]

I am facing this issue for best accuracy also.

[Al.]

Thanks Chris. Yes, I read about Kalman while doing some search, but it's certainly a bit beyond my math knowledge. Are you aware of any sample code easy to read (and understand!), or better yet, some implementation available? (C / C++ / Java)

[Efren Narvaez]

@Al Unfortunately my only exposure with Kalman filters is through work, so I have some wonderfully elegant code I can't show you.

[Al.]

No problem :-) I tried looking but for some reason it seems this Kalman thing is black magic. Lots of theory pages but little to none code.. Thanks, will try the other methods.

[Al.]

Thanks, I'll give it a shot too. Note that I want to smooth the current position, as it's the one being displayed and the one used to retrieve some data. I'm not interested in past points. My original idea was using weighted means, but I still have to see what's best.

[Karl]

Al, this appears to be a form of weighted means. You will need to use "past" points if you want to do any smoothing, because the system needs to have more than the current position in order to know where to smooth too. If your GPS is taking datapoints once per second and your user looks at the screen once per five seconds, you can use 5 datapoints without him noticing! A moving average would only be delayed by one dp also.

[Horacio]

Shouldn't the variance calculation be: variance += TimeInc_milliseconds * TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000000

[Stochastically]

@Horacio, I know why you think that, but no! Mathematically, the uncertainty here is being modelled by a Wiener process (see en.wikipedia.org/wiki/Wiener_process ) and with a Wiener process the variance grows linearly with time. The variable Q_metres_per_second corresponds to the variable sigma in the section "Related processes" in that Wikipedia article. Q_metres_per_second is a standard deviation and it's measured in metres, so metres and not metres/seconds are its units. It corresponds to the standard deviation of the distribution after 1 second has elapsed.

[Horacio]

Thanks for the explanation. Can you elaborate on how to account velocity information to get a better estimate? can I simply set Q_meters_per_second equal to the velocity of the user (in meters per 1 sec) multiplied by some factor? say 1.5?

[Stochastically]

@Horacio what you suggest won't work at all in terms of incorporating velocity to get a better estimate. I was trying to answer that question myself, and ended up asking a couple of questions elsewhere, see dsp.stackexchange.com/questions/8860/… . In fact Kalman filters are quite sophisticated, and at the moment I don't think there's a simple answer to your question. The code that I posted here is a bit of an anomaly in the sense that for this special case, everything works out very nicely and it looks quite simple!

[Rostyslav Druzhchenko]

kalman.sourceforge.net/index.php here is C++ implementation of Kalman filter.

[Efren Narvaez]

@Rost Thanks, that's good to know. I added the link to the main body of the answer to make it more obvious. I'll have to play with that library now...

[Rostyslav Druzhchenko]

@ChrisArguin You are welcome. Let me know if the result is good please.

[Andreas Rudolph]

I tried this approach and the code, but it ended up shortening the total distance too much. Made it too imprecise.

[user2999943]

How to use your class? Should I process (with your code) already received coordinates in onLocationChanged() method?

[Stochastically]

@user2999943 yes, use the code to process the coordinates that you get from onLocationChanged().

[umesh]

I tried to use your lib code, I got some undesired results, I am not sure if I'm doing something wrong...(Below is image url, blue is filtered locations' path, orange are raw locations) app.box.com/s/w3uvaz007glp2utvgznmh8vlggvaiifk

[villoren]

The spikes you're seeing 'growing' from the mean (orange line) look like network provider updates. Can you try plotting both raw network and gps updates? Perhaps you would be better off with no network updates, depending on what you're trying to achieve. Btw, where are you getting those raw orange updates from?

[umesh]

orange points are from gps provider, and the blue are from Kalman, I plotted logs on map

[AlexWien]

Why do you think that this will improve the positions? The GPS chip internally heavilly uses the kalman filter, the output is already kalman filtered.

[Stochastically]

@AlexWien the above code is a simple Kalman filter :-). Whether it improves anything or not is subjective, and will depend on the objectives. Also, whether GPS readings are already Kalman filtered (and to what extent) is presumably hardware dependent. Finally, I haven't looked into the detailed mathematical theory but I wouldn't be surprised if someone told me that a Kalman filter applied to a Kalman filter is just a Kalman filter which is less sensitive to the inputs. If so, even if GPS readings are already filtered, if one wants less noisy readings another filter may be useful.

[Matt Upson]

Could you provide some references for this please?

[AlexWien]

There is many info and much professional experience in that sentences, Which sentence exactly you want a reference for? for speed: search for doppler effect and GPS. internal Kalman? This is basic GPS knowlege, every paper or book describing how a GPS chip internaly works. smootig-errors: ever smoothing introduce erros. stand still? try it out.

[hgoebl]

The "jumping around" when standing still is not the only source of error. There are also signal reflections (e.g. from mountains) where the position jumps around. My GPS chips (e.g. Garmin Dakota 20, SonyEricsson Neo) haven't filtered this out... And what is really a joke is the elevation value of the GPS signals when not combined with barometric pressure. These values aren't filtered or I don't want to see the unfiltered values.

[villoren]

Could you send me that data in some text format? Each location update has the Location.getProvider() field set. Just one file with all Location.toString().

[mauron85]

Is there any recommendation when some method is more suitable than the other? Like using in real time on device (with very few samples from the past) vs on server processing in background (many samples).

[Peter Wone]

@Al. and anyone else who thinks K filters are black magic: accelerometers and gyros (inertial guidance systems) are jitter resistant but very prone to drift. GPS does not drift but is jittery. K filters apply statistical noise rejection math to a sliding window of data to integrate the GPS and inertial data into results that are neither drifty nor jittery.

[Peter Wone]

@AlexWien GPS computes distance from a point at a time to a tolerance giving you a sphere with thickness, a shell centred around a satellite. You are somewhere in this shell volume. The intersection of three of these shell volumes gives you a position volume, the centroid of which is your computed position. If you have a set of reported positions and you know the sensor is at rest, computing the centroid effectively intersects a lot more shells, improving precision. Error in this case is reduced.

[pickaxe]

@Stochastically, thanks for this. It works really well for my use case. I'm still unclear on why you say adding the noisy velocity measurements (from the GPS points) to the model won't work. Maybe you mean calculating it is "baked in"? (I've been following dsp.stackexchange.com/questions/8860/… but don't have the rep. to comment there). I'm on iOS, but I'm sure it's quite similar to Android. They provide a speed and course, seemingly without notion of accuracy. Could those measurements be fused with the position measurements? Thanks again!

[pickaxe]

@Stochastically, one more question: using the Weiner process here means that the variance will just increase to infinitum for streaming data, K --> 1, and eventually the filtered location will just be the input. does this poses a problem for your use case? Or do you reset variance at some point?

[Stochastically]

@pickaxe I'm not sure you understand what a Kalman filter is meant to do. If you you have perfect accuracy of your measurements then K=1 and the position is set to exactly the measurement. For completely imperfect measurements the accuracy value is huge so K=0 and the position is unaffected.

[pickaxe]

@Stochastically, yes, I understand. I don't think I said anything to the contrary regarding the behavior of K. But... I overlooked the re-calc of the covariance when trying to understand how it behaves over time. What I learned so far is that this is a really nice approach for when GPS data is sparse and using a physical model of motion produces terrible results. Thanks again.

[Naikrovek]

@Stochastically - Thank you for this code. This is excellent. Do you have usage code (an example of how to use this?) I'm just ramping up on Kalman filters.

[Stochastically]

@Naikrovek I used this an an android App which needed to know the location of a phone, but I haven't worked with this since 2013 so I don't have any more code to add. Good luck!

[sobri]

"GPS positions, as delivered, are already Kalman filtered, you probably cannot improve". If you can point to a source that confirms this for modern smartphones (for example), that would be very useful. I cannot see evidence of it myself. Even simple Kalman filtering of a device's raw locations strongly suggests it is not true. The raw locations dance around erratically, while the filtered locations most often hold close to the real (known) location.

[Glebka]

@Naikrovek: something like this: filter.Process(geo.getLatitude(), geo.getLongitude(), (float) point.getLocation().getAccuracy(), point.getTimestamp().getMillis()); ` ` AddPoint(filter.get_lat(), filter.get_lng(), point.getTimestamp();

[Peter Wone]

@sobri - you are correct. It is possible to compute the centroid of a dataset of n+1 positions from the centroid of the first n positions and the n+1 position, and this is essentially what your solution will do. More related info in my 5yo answer below.

[jjmontes]

For reference, this is a paper mentioning the doppler effect technique for very accurate speed and acceleration measurements. I don't know, however, if this is implemented by GPS vendors.

[Koray]

@GLeBaTi what if we don't have the accuracy info?

[Stochastically]

@Koray if you don't have accuracy info then you can't use a Kalman filter. It's completely fundamental to what the Kalman filter is trying to do.

[Koray]

@Stochastically I've used your code with a static accuracy value and modified a little (ie. calculate acceleration and eliminate the points that creates more than a threshold) and the results look good. I was thinking to use a Douglas Peucker or a spline smoothing, but my results with your code were just fine. Do you think its ok to use your code like that?

[Stochastically]

@Koray a Kalman Filter has a current position estimate with some variance, and then a measurement comes along for a different position, also with some variance (N.B. variance = accuracy parameter). The formulas in the filter are derived so that the old position is combined with the new position estimate so as to produce an estimate with the lowest possible variance. So if there's no real accuracy parameter, the variances are wrong and the results will end up being a bit arbitrary. It will work if your variances are constant(ish) and you use that constant, but unreliable otherwise.

[Christian Lindig]

The way I understand it, the code is dampening the changes in lat/lon by taking only a part (controlled by K) of the difference to the previous lat/lon pair. This is a form of exponential averaging. Does it matter that this happening in the lat/lon domain and not in the euclidian domain?

[Stochastically]

@ChristianLindig I suggest you look at the mathematical basis for the Kalman filter. The idea is that the existing best guess location has an error, and the new measurement has an error too. The choice of K is calculated so as to combine the existing best guess location with the new measurement so as to minimize the error of the new best guess location :-) .

[Baglay Vyacheslav]

1. Does anyone has examples of what parameters Q, accuracy for a car gps track?

[jdixon04]

Tried to edit this, but there's a typo in the last lines where @lat and @lng are set. Should be += rather than =

[Matt]

How does your implementation handle crossing the international date line? Seems like the discontinuity in longitude might not be properly handled. eg 179.99999W -> 179.99999E

[Stochastically]

Without doubt, @Matt, this implementation does not handle the international date line.

[AlexWien]

@PeterWone your comment refers to how a GPS chip calculates its position. Speed is calculaetd differently, it is not only position change over time.