Гладкие данные GPS

Я работаю с данными GPS, получаю значения каждую секунду и отображаю текущее положение на карте. Проблема в том, что иногда (особенно при низкой точности) значения сильно меняются, что делает текущую позицию «прыгать» между удаленными точками на карте.

Я задавался вопросом о каком-то достаточно простом способе избежать этого. В качестве первой идеи я подумал об отбрасывании значений с точностью выше определенного порога, но я думаю, что есть и другие более эффективные способы. Как обычно программы выполняют это?

Вот простой фильтр Калмана, который можно использовать именно для этой ситуации. Это произошло из-за работы, которую я сделал на устройствах 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;
        }
    }
}

То, что вы ищете, называется фильтром Калмана . Он часто используется для сглаживания навигационных данных . Это не обязательно тривиально, и вы можете выполнить множество настроек, но это очень стандартный подход, который хорошо работает. Там есть библиотека KFilter доступен , которая является реализацией C ++.

Мой следующий запасной вариант будет соответствовать наименьших квадратов . Фильтр Калмана сгладит данные с учетом скоростей, тогда как подход наименьших квадратов будет просто использовать информацию о положении. Тем не менее, это определенно проще для реализации и понимания. Похоже, что научная библиотека GNU может иметь такую реализацию.

Это может прийти немного поздно ...

Я написал этот KalmanLocationManager для Android, который объединяет двух наиболее распространенных провайдеров определения местоположения: сеть и GPS, kalman-фильтр данных и доставляет обновления LocationListener(как два «настоящих» провайдера).

Я использую его в основном для «интерполяции» между показаниями - например, для получения обновлений (прогнозов положения) каждые 100 миллисекунд (вместо максимальной скорости GPS в одну секунду), что дает мне лучшую частоту кадров при анимации моей позиции.

На самом деле он использует три фильтра Калмана для каждого измерения: широта, долгота и высота. Во всяком случае, они независимы.

Это значительно упрощает математику: вместо использования одной матрицы перехода состояний 6x6 я использую 3 разные матрицы 2x2. На самом деле в коде я вообще не использую матрицы. Решены все уравнения и все значения являются примитивами (двойной).

Исходный код работает, и есть демонстрационная активность. Извините за отсутствие javadoc в некоторых местах, я догоню.

Вы не должны рассчитывать скорость от изменения положения за раз. GPS может иметь неточные положения, но имеет точную скорость (выше 5 км / ч). Так что используйте скорость из отметки местоположения GPS. И далее вы не должны делать это с курсом, хотя это работает в большинстве случаев.

Полученные позиции GPS уже отфильтрованы по Калману, и вы, вероятно, не сможете их улучшить. Обычно при постобработке у вас не та же информация, что и у чипа GPS.

Вы можете сгладить его, но это также приводит к ошибкам.

Просто убедитесь, что вы удаляете позиции, когда устройство стоит на месте, это удаляет прыжковые позиции, которые некоторые устройства / конфигурации не удаляют.

Я обычно использую акселерометры. Внезапное изменение положения за короткий период подразумевает высокое ускорение. Если это не отражается в телеметрии акселерометра, то это почти наверняка связано с изменением «лучших трех» спутников, используемых для вычисления местоположения (которое я называю GPS-телепортацией).

Когда актив находится в состоянии покоя и перемещается из-за телепортации GPS, если вы постепенно вычисляете центроид, вы эффективно пересекаете все больший и больший набор оболочек, повышая точность.

Чтобы сделать это, когда актив не находится в состоянии покоя, вы должны оценить его вероятную следующую позицию и ориентацию на основе данных скорости, курса и линейного и вращательного (если у вас есть гироскопы) ускорения. Это более или менее то, что делает знаменитый K-фильтр. Вы можете получить все это аппаратно примерно за 150 долларов на AHRS, содержащей все, кроме модуля GPS, и с разъемом для подключения. Он имеет свой собственный процессор и фильтрацию Калмана на борту; результаты стабильны и довольно хороши. Инерционное наведение очень устойчиво к джиттеру, но дрейфует со временем. GPS склонен к джиттеру, но не дрейфует со временем, они практически созданы для компенсации друг друга.

Один метод, который использует меньше математики / теории, - это выборка 2, 5, 7 или 10 точек данных за раз и определение тех, которые являются выбросами. Менее точный показатель выброса, чем фильтр Калмана, заключается в использовании следующего алгоритма для определения всех парных расстояний между точками и выброса одного, наиболее удаленного от остальных. Обычно эти значения заменяются значением, ближайшим к исходному значению, которое вы заменяете.

Например

Сглаживание в пяти точках выборки 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 является наибольшим, вы заменили бы точку B на D, если BD = min {AB, BC, BD, BE}

Это сглаживание определяет выбросы и может быть увеличено путем использования средней точки BD вместо точки D для сглаживания позиционной линии. Ваш пробег может варьироваться, и существуют более математически строгие решения.

Что касается наименьших квадратов, вот еще пара вещей, с которыми можно поэкспериментировать:

1. То, что он подходит по методу наименьших квадратов, не означает, что он должен быть линейным. Вы можете подгонять квадратичную кривую к данным методом наименьших квадратов, тогда это будет соответствовать сценарию, в котором пользователь ускоряется. (Обратите внимание, что под наименьшими квадратами я имею в виду использование координат в качестве зависимой переменной и время в качестве независимой переменной.)

2. Вы также можете попробовать взвешивать точки данных на основе сообщенной точности. Когда точность низкая, эти данные снижаются.

3. Еще одна вещь, которую вы, возможно, захотите попробовать, - это не отображать одну точку, а, если точность низкая, отобразить кружок или что-то, указывающее диапазон, в котором пользователь может основываться на сообщенной точности. (Это то, что делает встроенное приложение Google Maps для iPhone.)

Вы также можете использовать сплайн. Введите значения, которые у вас есть, и интерполируйте точки между вашими известными точками. Связав это с подгонкой по методу наименьших квадратов, скользящим средним или фильтром Калмана (как уже упоминалось в других ответах), вы сможете рассчитать точки между вашими «известными» точками.

Возможность интерполировать значения между вашими известными дает вам хороший плавный переход и / разумное / приблизительное представление о том, какие данные были бы представлены, если бы у вас была более высокая точность. http://en.wikipedia.org/wiki/Spline_interpolation

Разные сплайны имеют разные характеристики. Наиболее часто используемые сплайны - Акима и Кубические сплайны.

Другим алгоритмом, который следует рассмотреть, является алгоритм упрощения линии Рамера-Дугласа-Пекера, он довольно часто используется для упрощения данных GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Возвращаясь к фильтрам Калмана ... Я нашел реализацию C для фильтра Калмана для данных GPS здесь: http://github.com/lacker/ikalman Я еще не пробовал, но это выглядит многообещающе.

Отображается на CoffeeScript, если кто-то заинтересован. ** редактировать -> извините, используя магистраль тоже, но вы поняли идею.

Немного изменен, чтобы принять маяк с атрибутами

{широта: 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]

Я преобразовал код 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
    /// /programming/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
        }
    }
}

Вот реализация Javascript Java-реализации @ Stochastically для тех, кто в ней нуждается:

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