Smoothing of GPS data and removal of outliers

Question

I have real-time GPS data coming at 5 updates per second. On average 80% of the data is fairly accurate; but around 20% of the data is jerky. Plus occasionally we also get an outlier i.e. an erroneous data point far away from the actual trajectory.

I am looking for an algorithm that could me do achieve the following:

• Smooth out the data so that jerkiness is eliminated.
• Not to smooth out the outlier data but rather eliminate those erroneous data points and replace with some extrapolated value.

To give some context, I first searched stackoverflow.com site for some similar topic and found the following link:

Smooth GPS data

My software engineer implemented the KalmanLatLong routine that was provided in the above link; but we encountered the following issues:

• The algorithm is lagging behind meaning while the algo is generating extrapolated values, more GPS data points arrive (remember the data is coming in real-time).

• In case of an occasional outlier, the algo smooths it out is well. Whereas out goal is to eliminate such outliers because they are erroneous data.

I am looking for an algorithm that could work in real-time and handle GPS updates at 5 Hz and smoothen the data while eliminating outliers.

Your help would be highly appreciated.

Answer 1

You can incorporate observation rejection within the kalman filter itself. This is sometimes called data snooping.

I'll suppose that you have the simple case where you have just the GPS data and want either to incorporate an instant's measurments, or throw them all away (rather than throwing away just the latitude say).

The notation is conformant with the Wiki article

In the update step, compute

V = y'*inv(S)*y

(y is the residual vector, S the residual covariance) If your filter is correctly tuned, this has a a chi-square(2) distribution, if you are using just lat and lon, or chi-square(3) if you are using height too. If V exceeds say the upper 0.1% percentile of this distribution, reject the observations, ie do not compute the gain or update your state or state covariance.

Answer 2

A basic approach could be like this:

• Take the last x points and compute the average delta vector.
• Apply it to the last point to get the extrapolated new point (where you would expect the new point to be).
• Compare the extrapolated point to what you get from the GPS.
• If the distance between the extrapolated node and the one you get is less then some threshold then you can consider that this is a good data point and blend it with the extrapolated one (new point is P* GPS_point + (1-P) * extrapolated_point for some P between 0 and 1).
• If the distance exceeds the threshold then it's probably an outlier so drop it completely and use just the extrapolated point.

You need to experiment with values for X, P and threshold and see what works for you. What's good for high speed racing is not good for walking applications. Also add a check if you have too many extrapolated nodes since that means that this algorithm thinks most of the data is wrong and that shouldn't be true. You can empty the list of points and start from scratch.