平滑的GPS数据

时间：2009-07-15 23:04:32

标签： gps smoothing

我正在处理GPS数据，每秒获取值并在地图上显示当前位置。问题是有时候（特别是当准确度很低时）值变化很大，使得当前位置在地图中的远点之间“跳跃”。

我想知道一些简单的方法来避免这种情况。作为第一个想法，我考虑过准确地丢弃超过一定阈值的值，但我想还有其他一些更好的方法。程序执行此操作的常用方法是什么？

12 个答案:

答案 0 :(得分：73)

您要找的是Kalman Filter。它经常用于smooth navigational data。它不一定是微不足道的，你可以做很多调整，但它是一种非常标准的方法并且效果很好。有一个KFilter library可用，它是一个C ++实现。

我的下一个后备将是least squares fit。卡尔曼滤波器将考虑速度平滑数据，而最小二乘拟合方法将仅使用位置信息。尽管如此，实施和理解它仍然更简单。看起来GNU科学图书馆可能有implementation of this.

答案 1 :(得分：71)

这是一个简单的卡尔曼滤波器，可以用于这种情况。它来自我在Android设备上所做的一些工作。

通用卡尔曼滤波器理论是关于向量的估计，具有由协方差矩阵表示的估计的准确性。但是，为了估计Android设备上的位置，一般理论简化为一个非常简单的情况。 Android位置提供商将位置指定为纬度和经度，以及指定为以米为单位的单个数字的精度。这意味着，代替协方差矩阵，卡尔曼滤波器的精度可以通过单个数来测量，即使卡尔曼滤波器中的位置是由两个数字测量的。此外，纬度，经度和米实际上是所有不同单位的事实可以忽略，因为如果你将比例因子放入卡尔曼滤波器将它们全部转换为相同的单位，那么这些比例因子在转换结果时最终会被取消回到原来的单位。

代码可以改进，因为它假设当前位置的最佳估计是最后已知的位置，如果有人正在移动，则应该可以使用Android的传感器来产生更好的估计。该代码具有单个自由参数Q，以米/秒表示，其描述了在没有任何新位置估计的情况下精度衰减的速度。较高的Q参数意味着精度衰减得更快。卡尔曼滤波器通常在精度衰减比预期快一点时效果更好，因此对于使用Android手机走动我发现Q = 3米每秒工作正常，即使我通常走得比这慢。但如果乘坐快车，显然应该使用更多的数字。

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

答案 2 :(得分：11)

这可能会晚一点......

我为Android写了这个KalmanLocationManager，它包含了两个最常见的位置提供商，网络和GPS，kalman过滤数据，并向LocationListener提供更新（就像两个'真实'提供者）。

我主要用它来读取读数之间的“插值” - 例如每100毫秒接收一次更新（位置预测）（而不是一秒的最大gps速率），这使我在动画我的位置时具有更好的帧速率。

实际上，它为每个维度使用三个卡尔曼滤波器：纬度，经度和海拔高度。 无论如何，他们是独立的。

这使矩阵数学更容易：我使用3个不同的2x2矩阵，而不是使用一个6x6状态转换矩阵。实际上在代码中，我根本不使用矩阵。求解了所有方程，所有值都是基元（双）。

源代码正常运行，并且有一个演示活动。 很抱歉在某些地方缺少javadoc，我会赶上来。

答案 3 :(得分：8)

您不应该每次都根据位置变化来计算速度。 GPS可能有不准确的位置，但它具有准确的速度（超过5公里/小时）。所以使用GPS位置标记的速度。 而且你当然不应该这样做，尽管它大部分时间都有效。

GPS定位已经过卡尔曼过滤，你可能无法改进，在后期处理中通常你没有像GPS芯片那样的信息。

你可以平滑它，但这也会引入错误。

只需确保在设备静止时移除位置，这会移除某些设备/配置无法移除的跳跃位置。

答案 4 :(得分：4)

使用较少数学/理论的一种方法是一次采样2个，5个，7个或10个数据点并确定那些异常值。与卡尔曼滤波器相比，对异常值的不精确测量是使用以下algorithm来获取点之间的所有成对距离并丢弃距离其他点最远的距离。通常，这些值将替换为最接近您要替换的外围值的值

例如

在五个样品点A，B，C，D，E平滑

ATOTAL =距离的总和AB AC AD AE

BTOTAL =距离AB BC BD BE的总和

CTOTAL =距离AC BC CD CE

的总和

DTOTAL =距离的总和DA DB DC DE

ETOTAL =距离的总和EA EB EC DE

如果BTOTAL最大，如果BD = min {AB，BC，BD，BE}，则将B替换为B点

这种平滑确定了异常值，并且可以通过使用BD的中点而不是点D来平滑位置线来增强。您的里程可能会有所不同，并且存在更多数学上严格的解决方案。

答案 5 :(得分：4)

至于最小二乘拟合，这里还有其他几个要试验的东西：

  1. 仅仅因为它的最小二乘拟合并不意味着它必须是线性的。您可以将二次曲线最小二乘拟合到数据，然后这将适合用户正在加速的场景。 （注意，通过最小二乘拟合，我的意思是使用坐标作为因变量，将时间作为自变量。）

  2. 您还可以尝试根据报告的准确性对数据点进行加权。当精度较低时，这些数据点较低。

  3. 您可能想要尝试的另一件事是，而不是显示单个点，如果精确度低，则显示圆圈或指示用户可能基于报告的准确度的范围。 （这就是iPhone内置的Google地图应用程序所做的。）

答案 6 :(得分：4)

我通常使用加速度计。在短时间内突然改变位置意味着高加速度。如果这没有反映在加速度计遥测中，那几​​乎可以肯定是由于用于计算位置的“最好的三颗”卫星的变化（我称之为GPS远距传输）。

当资产处于静止状态并因GPS远距传输而跳跃时，如果逐步计算质心，则有效地交叉越来越大的壳体，从而提高精度。

要在资产未处于静止时执行此操作，您必须根据速度，航向，线性和旋转（如果您有陀螺仪）加速度数据估算其可能的下一个位置和方向。这或多或少是着名的K滤镜所做的。您可以在AHRS上获得大约150美元的全部物品，其中包含除GPS模块之外的所有物品，并使用千斤顶连接一个。它有自己的CPU和卡尔曼滤波器;结果稳定而且非常好。惯性制导对抖动具有很强的抵抗力，但随着时间的推移而漂移。 GPS容易产生抖动，但不随时间漂移，实际上它们是相互补偿的。

答案 7 :(得分：3)

您也可以使用样条线。输入您拥有的值并在已知点之间插入点。将其与最小二乘拟合，移动平均或卡尔曼滤波器（如其他答案中所述）相关联，使您能够计算“已知”点之间的点。

如果您具有更高的保真度，能够在您的知识之间插值，可以为您提供良好的平滑过渡和/合理/近似的数据。 http://en.wikipedia.org/wiki/Spline_interpolation

不同的样条线具有不同的特征。我见过的最常用的是Akima和Cubic splines。

另一种要考虑的算法是Ramer-Douglas-Peucker线简化算法，它在GPS数据的简化中非常常用。 （http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm）

答案 8 :(得分：3)

回到卡尔曼滤波器......我在这里找到了一个用于GPS数据的卡尔曼滤波器的C实现：http://github.com/lacker/ikalman我还没有尝试过，但它看起来很有希望。

答案 9 :(得分：0)

如果感兴趣的话，映射到CoffeeScript。 **编辑 - ＆gt;抱歉使用骨干，但你明白了。

稍微修改以接受带有attribs的信标

  

{latitude：item.lat，经度：item.lng，日期：新   日期（item.effective_at），准确度：item.gps_accuracy} 

MIN_ACCURACY = 1

# mapped from http://stackoverflow.com/questions/1134579/smooth-gps-data

class v.Map.BeaconFilter

  constructor: ->
    _.extend(this, Backbone.Events)

  process: (decay,beacon) ->

    accuracy     = Math.max beacon.accuracy, MIN_ACCURACY

    unless @variance?
      # if variance nil, inititalise some values
      @variance     = accuracy * accuracy
      @timestamp_ms = beacon.date.getTime();
      @lat          = beacon.latitude
      @lng          = beacon.longitude

    else

      @timestamp_ms = beacon.date.getTime() - @timestamp_ms

      if @timestamp_ms > 0
        # time has moved on, so the uncertainty in the current position increases
        @variance += @timestamp_ms * decay * decay / 1000;
        @timestamp_ms = beacon.date.getTime();

      # 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
      _k  = @variance / (@variance + accuracy * accuracy)
      @lat = _k * (beacon.latitude  - @lat)
      @lng = _k * (beacon.longitude - @lng)

      @variance = (1 - _k) * @variance

    [@lat,@lng]

答案 10 :(得分：0)

我已经将Java代码从@Stochastically转换为Kotlin 

class KalmanLatLong
{
    private val MinAccuracy: Float = 1f

    private var Q_metres_per_second: Float = 0f
    private var TimeStamp_milliseconds: Long = 0
    private var lat: Double = 0.toDouble()
    private var lng: Double = 0.toDouble()
    private var variance: Float =
        0.toFloat() // P matrix.  Negative means object uninitialised.  NB: units irrelevant, as long as same units used throughout

    fun KalmanLatLong(Q_metres_per_second: Float)
    {
        this.Q_metres_per_second = Q_metres_per_second
        variance = -1f
    }

    fun get_TimeStamp(): Long { return TimeStamp_milliseconds }
    fun get_lat(): Double { return lat }
    fun get_lng(): Double { return lng }
    fun get_accuracy(): Float { return Math.sqrt(variance.toDouble()).toFloat() }

    fun SetState(lat: Double, lng: Double, accuracy: Float, TimeStamp_milliseconds: Long)
    {
        this.lat = lat
        this.lng = lng
        variance = accuracy * accuracy
        this.TimeStamp_milliseconds = TimeStamp_milliseconds
    }

    /// <summary>
    /// Kalman filter processing for lattitude and longitude
    /// https://stackoverflow.com/questions/1134579/smooth-gps-data/15657798#15657798
    /// </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>
    fun Process(lat_measurement: Double, lng_measurement: Double, accuracy: Float, TimeStamp_milliseconds: Long)
    {
        var accuracy = accuracy
        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

            val 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.toFloat() * 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
            val 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
        }
    }
}

答案 11 :(得分：0)

这是@Stochastically为需要的人提供的Java实现的Javascript实现：

class GPSKalmanFilter {
  constructor (decay = 3) {
    this.decay = decay
    this.variance = -1
    this.minAccuracy = 1
  }

  process (lat, lng, accuracy, timestampInMs) {
    if (accuracy < this.minAccuracy) accuracy = this.minAccuracy

    if (this.variance < 0) {
      this.timestampInMs = timestampInMs
      this.lat = lat
      this.lng = lng
      this.variance = accuracy * accuracy
    } else {
      const timeIncMs = timestampInMs - this.timestampInMs

      if (timeIncMs > 0) {
        this.variance += (timeIncMs * this.decay * this.decay) / 1000
        this.timestampInMs = timestampInMs
      }

      const _k = this.variance / (this.variance + (accuracy * accuracy))
      this.lat += _k * (lat - this.lat)
      this.lng += _k * (lng - this.lng)

      this.variance = (1 - _k) * this.variance
    }

    return [this.lng, this.lat]
  }
}

用法示例：

   const kalmanFilter = new GPSKalmanFilter()
   const updatedCoords = []

    for (let index = 0; index < coords.length; index++) {
      const { lat, lng, accuracy, timestampInMs } = coords[index]
      updatedCoords[index] = kalmanFilter.process(lat, lng, accuracy, timestampInMs)
    }
相关问题
最新问题