减少 GPS 抖动

Question

我的应用程序基于 GPS 数据，为此我使用了Fused-location-provider。 从现在开始，我看到有一个 GPS 抖动，一些 GPS 坐标不在路上。这是无法接受的。我试图做的是实现Kalman filter，我做到了：

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

每当我收到新位置时，我都会使用它

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

这有助于获得更准确的结果，但仍然 - 它没有修复路外的 GPS 抖动（见图）：

我想知道的事情：

1. 我的卡尔曼滤波器是否正确实施？有没有办法改进当前的解决方案？
2. 如何避免坐标在路外的场景？ （类似于 snap-to-roads 功能，但作为应用程序内部的一种算法）

Answer 1

您的图片上似乎不是抖动（或不仅仅是抖动），而是 GPS 数据的差距：

在图片上的点 1 和 2 之间的道路上没有点，并且不可能通过任何卡尔曼滤波器实现添加它们（如果它们在源原始 GPS 数据中不存在），因为没有关于道路位置的信息.如果无法更改跟踪设备的固件，可以使用Google Maps Roads API 中的Snap to Roads，如this 答案：

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


        }
    }

}

并非全部，但至少对于详细的路线视图（由于限制：每个用户 (IP) 每天 100 个 GPS 点和 2500 个请求以及每秒 10 个请求）“在线”或 “预处理”完整路线一次，存储和显示不是原始的，而是已经处理的路线 - 这是一种“类似于快速道路功能的东西，但作为应用程序内部的算法”。