Reduce GPS jitters

Question

my application is based on GPS data and for that I'm using Fused-location-provider. From now and then I have seen that there's a gps jitters and some GPS coordinates are off the road. This is unacceptable. What I tried to do was to implement Kalman filter and I did:

fun process(
    newSpeed: Float,
    newLatitude: Double,
    newLongitude: Double,
    newTimeStamp: Long,
    newAccuracy: Float
) {

    if (variance < 0) { // if variance < 0, object is unitialised, so initialise with current values
        setState(newLatitude, newLongitude, newTimeStamp, newAccuracy)
    } else { // else apply Kalman filter
        val duration = newTimeStamp - timeStamp
        if (duration > 0) { // time has moved on, so the uncertainty in the current position increases
            variance += duration * newSpeed * newSpeed / 1000
            timeStamp = newTimeStamp
        }

        val k = variance / (variance + newAccuracy * newAccuracy)

        latitude += k * (newLatitude - latitude)
        longitude += k * (newLongitude - longitude)
        variance *= (1 - k)

        retrieveLocation(newSpeed, longitude, latitude, duration, newAccuracy)
    }
}

And I'm using it whenever I receive new location as

KalmanFilter.process(
    it.newSpeed,
    it.newLatitude,
    it.newLongitude,
    it.newTimeStamp,
    it.newAccuracy
)

This helped to receive little bit more accurate results, but still - it did not fix the GPS jitter that was outside the road (see image):

Things I want to know:

1. Is my kalman filter implemented correctly? Is there way to improve current solution?
2. How to avoid a scenerios where coordinate is outside the road? (Something similar to snap-to-roads functionality, but as an algorithm inside app)

Answer 1

Seems on your picture is not jitter (or not only jitter), but gap in GPS data:

there are no points on road between points 1 and 2 on picture and there is no possibilities to add them with any Kalman filter implementation (if they absent in source raw GPS data) because there is no information about the location of the road. If you cannot change the firmware of the tracker device, you can use Snap to Roads of Google Maps Roads API like in this answer:

public class MainActivity extends AppCompatActivity implements OnMapReadyCallback {

    private GoogleMap mGoogleMap;
    private MapFragment mapFragment;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);

        mapFragment = (MapFragment) getFragmentManager()
                .findFragmentById(R.id.map_fragment);
        mapFragment.getMapAsync(this);
    }

    @Override
    public void onMapReady(GoogleMap googleMap) {
        mGoogleMap = googleMap;

        List<LatLng> sourcePoints = new ArrayList<>();
        sourcePoints.add(new LatLng(-35.27801,149.12958));
        sourcePoints.add(new LatLng(-35.28032,149.12907));
        sourcePoints.add(new LatLng(-35.28099,149.12929));
        sourcePoints.add(new LatLng(-35.28144,149.12984));
        sourcePoints.add(new LatLng(-35.28194,149.13003));
        sourcePoints.add(new LatLng(-35.28282,149.12956));
        sourcePoints.add(new LatLng(-35.28302,149.12881));
        sourcePoints.add(new LatLng(-35.28473,149.12836));

        PolylineOptions polyLineOptions = new PolylineOptions();
        polyLineOptions.addAll(sourcePoints);
        polyLineOptions.width(5);
        polyLineOptions.color(Color.BLUE);
        mGoogleMap.addPolyline(polyLineOptions);

        mGoogleMap.animateCamera(CameraUpdateFactory.newLatLngZoom(sourcePoints.get(0), 15));


        List<LatLng> snappedPoints = new ArrayList<>();
        new GetSnappedPointsAsyncTask().execute(sourcePoints, null, snappedPoints);
    }


    private String buildRequestUrl(List<LatLng> trackPoints) {
        StringBuilder url = new StringBuilder();
        url.append("https://roads.googleapis.com/v1/snapToRoads?path=");

        for (LatLng trackPoint : trackPoints) {
            url.append(String.format("%8.5f", trackPoint.latitude));
            url.append(",");
            url.append(String.format("%8.5f", trackPoint.longitude));
            url.append("|");
        }
        url.delete(url.length() - 1, url.length());
        url.append("&interpolate=true");
        url.append(String.format("&key=%s", <your_Google_Maps_API_key>);

        return url.toString();
    }


    private class GetSnappedPointsAsyncTask extends AsyncTask<List<LatLng>, Void, List<LatLng>> {

        protected void onPreExecute() {
            super.onPreExecute();
        }

        protected List<LatLng> doInBackground(List<LatLng>... params) {

            List<LatLng> snappedPoints = new ArrayList<>();

            HttpURLConnection connection = null;
            BufferedReader reader = null;

            try {
                URL url = new URL(buildRequestUrl(params[0]));
                connection = (HttpURLConnection) url.openConnection();
                connection.setRequestMethod("GET");
                connection.connect();

                InputStream stream = connection.getInputStream();

                reader = new BufferedReader(new InputStreamReader(stream));
                StringBuilder jsonStringBuilder = new StringBuilder();

                StringBuffer buffer = new StringBuffer();
                String line = "";

                while ((line = reader.readLine()) != null) {
                    buffer.append(line+"\n");
                    jsonStringBuilder.append(line);
                    jsonStringBuilder.append("\n");
                }

                JSONObject jsonObject = new JSONObject(jsonStringBuilder.toString());
                JSONArray snappedPointsArr = jsonObject.getJSONArray("snappedPoints");

                for (int i = 0; i < snappedPointsArr.length(); i++) {
                    JSONObject snappedPointLocation = ((JSONObject) (snappedPointsArr.get(i))).getJSONObject("location");
                    double lattitude = snappedPointLocation.getDouble("latitude");
                    double longitude = snappedPointLocation.getDouble("longitude");
                    snappedPoints.add(new LatLng(lattitude, longitude));
                }

            } catch (MalformedURLException e) {
                e.printStackTrace();
            } catch (IOException e) {
                e.printStackTrace();
            } catch (JSONException e) {
                e.printStackTrace();
            } finally {
                if (connection != null) {
                    connection.disconnect();
                }
                try {
                    if (reader != null) {
                        reader.close();
                    }
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }

            return snappedPoints;
        }

        @Override
        protected void onPostExecute(List<LatLng> result) {
            super.onPostExecute(result);

            PolylineOptions polyLineOptions = new PolylineOptions();
            polyLineOptions.addAll(result);
            polyLineOptions.width(5);
            polyLineOptions.color(Color.RED);
            mGoogleMap.addPolyline(polyLineOptions);

            LatLngBounds.Builder builder = new LatLngBounds.Builder();
            builder.include(result.get(0));
            builder.include(result.get(result.size()-1));
            LatLngBounds bounds = builder.build();
            mGoogleMap.animateCamera(CameraUpdateFactory.newLatLngBounds(bounds, 10));


        }
    }

}

not for all, but at least for detailed route view (because of limits: 100 GPS points and 2500 request per day per user (IP) and 10 requests per sec.) "online" or "preprocess" full route once, store and show not raw, but already processed route - that is a kind of "Something similar to snap-to-roads functionality, but as an algorithm inside app".