减少 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. 如何避免出现坐标在道路之外的场景？（类似于捕捉道路功能，但作为应用程序内部的算法）

Answer 1

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

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

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 个请求。）“在线”或“预处理”一次完整路线，存储并显示不是原始的，而是已经处理过的路线- 这是一种“类似于捕捉道路功能的东西，但作为应用程序内部的算法”。