Datos GPS suaves

Estoy trabajando con datos GPS, obteniendo valores cada segundo y mostrando la posición actual en un mapa. El problema es que a veces (especialmente cuando la precisión es baja) los valores varían mucho, lo que hace que la posición actual "salte" entre puntos distantes en el mapa.

Me preguntaba acerca de algún método lo suficientemente fácil como para evitar esto. Como primera idea, pensé en descartar valores con precisión más allá de cierto umbral, pero creo que hay otras formas mejores de hacerlo. ¿Cuál es la forma habitual en que los programas realizan esto?

Aquí hay un filtro simple de Kalman que podría usarse exactamente para esta situación. Vino de un trabajo que hice en dispositivos Android.

La teoría general del filtro de Kalman se trata de estimaciones para vectores, con la precisión de las estimaciones representadas por matrices de covarianza. Sin embargo, para estimar la ubicación en dispositivos Android, la teoría general se reduce a un caso muy simple. Los proveedores de ubicación de Android dan la ubicación como latitud y longitud, junto con una precisión que se especifica como un solo número medido en metros. Esto significa que, en lugar de una matriz de covarianza, la precisión en el filtro de Kalman se puede medir con un solo número, aunque la ubicación en el filtro de Kalman se mide con dos números. También se puede ignorar el hecho de que la latitud, la longitud y los metros son efectivamente todas las unidades diferentes, porque si coloca factores de escala en el filtro de Kalman para convertirlos a todos en las mismas unidades,

El código podría mejorarse, ya que supone que la mejor estimación de la ubicación actual es la última ubicación conocida, y si alguien se está moviendo, debería ser posible usar los sensores de Android para producir una mejor estimación. El código tiene un único parámetro libre Q, expresado en metros por segundo, que describe la rapidez con que decae la precisión en ausencia de nuevas estimaciones de ubicación. Un parámetro Q más alto significa que la precisión disminuye más rápido. Los filtros Kalman generalmente funcionan mejor cuando la precisión disminuye un poco más rápido de lo que cabría esperar, por lo que para caminar con un teléfono Android encuentro que Q = 3 metros por segundo funciona bien, aunque generalmente camino más lento que eso. Pero si viaja en un automóvil rápido, obviamente debería usarse un número mucho mayor.

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

Lo que estás buscando se llama Filtro Kalman . Se usa con frecuencia para suavizar los datos de navegación . No es necesariamente trivial, y hay muchos ajustes que puede hacer, pero es un enfoque muy estándar y funciona bien. Hay una biblioteca KFilter disponible que es una implementación de C ++.

Mi siguiente alternativa sería el ajuste de mínimos cuadrados . Un filtro de Kalman suavizará los datos teniendo en cuenta las velocidades, mientras que un enfoque de ajuste de mínimos cuadrados solo usará información posicional. Aún así, es definitivamente más sencillo de implementar y comprender. Parece que la Biblioteca Científica GNU puede tener una implementación de esto.

Esto podría llegar un poco tarde ...

Escribí este KalmanLocationManager para Android, que envuelve los dos proveedores de ubicación más comunes, Red y GPS, kalman-filtra los datos y entrega actualizaciones a LocationListener(como los dos proveedores 'reales').

Lo uso principalmente para "interpolar" entre lecturas, para recibir actualizaciones (predicciones de posición) cada 100 milis por ejemplo (en lugar de la velocidad máxima de gps de un segundo), lo que me da una mejor velocidad de cuadros al animar mi posición.

En realidad, utiliza tres filtros kalman, en cada dimensión: latitud, longitud y altitud. Son independientes, de todos modos.

Esto hace que las matemáticas de la matriz sean mucho más fáciles: en lugar de usar una matriz de transición de estado de 6x6, uso 3 matrices diferentes de 2x2. En realidad en el código, no uso matrices en absoluto. Resolvió todas las ecuaciones y todos los valores son primitivos (doble).

El código fuente está funcionando y hay una actividad de demostración. Perdón por la falta de javadoc en algunos lugares, me pondré al día.

No debe calcular la velocidad a partir del cambio de posición por tiempo. El GPS puede tener posiciones inexactas, pero tiene una velocidad precisa (superior a 5 km / h). Así que usa la velocidad del sello de ubicación GPS. Y además, no debes hacer eso con el curso, aunque funciona la mayoría de las veces.

Las posiciones de GPS, tal como se entregan, ya están filtradas por Kalman, probablemente no pueda mejorar, en el procesamiento posterior generalmente no tiene la misma información que el chip GPS.

Puede suavizarlo, pero esto también introduce errores.

Solo asegúrese de eliminar las posiciones cuando el dispositivo se detiene, esto elimina las posiciones de salto, que algunos dispositivos / Configuraciones no eliminan.

Usualmente uso los acelerómetros. Un cambio repentino de posición en un período corto implica una alta aceleración. Si esto no se refleja en la telemetría del acelerómetro, es casi seguro que se debe a un cambio en los "tres mejores" satélites utilizados para calcular la posición (a lo que me refiero como teletransportación GPS).

Cuando un activo está en reposo y saltando debido al teletransporte GPS, si calcula progresivamente el centroide, efectivamente está intersectando un conjunto de conchas cada vez más grande, mejorando la precisión.

Para hacer esto cuando el activo no está en reposo, debe estimar su probable próxima posición y orientación en función de la velocidad, el rumbo y los datos de aceleración lineal y rotacional (si tiene giroscopios). Esto es más o menos lo que hace el famoso filtro K. Puede obtener todo en hardware por alrededor de $ 150 en un AHRS que contiene todo menos el módulo GPS, y con un conector para conectar uno. Tiene su propia CPU y Kalman filtrado a bordo; Los resultados son estables y bastante buenos. La guía de inercia es altamente resistente a la fluctuación pero se desplaza con el tiempo. El GPS es propenso a las fluctuaciones, pero no cambia con el tiempo, prácticamente se hicieron para compensarse entre sí.

Un método que usa menos matemática / teoría es muestrear 2, 5, 7 o 10 puntos de datos a la vez y determinar los valores atípicos. Una medida menos precisa de un valor atípico que un filtro de Kalman es utilizar el siguiente algoritmo para tomar todas las distancias entre pares entre puntos y tirar el que está más alejado de los demás. Por lo general, esos valores se reemplazan con el valor más cercano al valor periférico que está reemplazando

Por ejemplo

Suavizado en cinco puntos de muestra A, B, C, D, E

ATOTAL = SUMA de distancias AB AC AD AE

BTOTAL = SUMA de distancias AB BC BD BE

CTOTAL = SUMA de distancias AC BC CD CE

DTOTAL = SUMA de distancias DA DB DC DE

ETOTAL = SUMA de distancias EA EB EC DE

Si BTOTAL es mayor, reemplazaría el punto B con D si BD = min {AB, BC, BD, BE}

Este suavizado determina valores atípicos y puede aumentarse utilizando el punto medio de BD en lugar del punto D para suavizar la línea posicional. Su kilometraje puede variar y existen soluciones matemáticamente más rigurosas.

En cuanto al ajuste de mínimos cuadrados, aquí hay un par de otras cosas para experimentar:

1. El hecho de que su ajuste sea de mínimos cuadrados no significa que tenga que ser lineal. Puede ajustar por mínimos cuadrados una curva cuadrática a los datos, entonces esto se ajustaría a un escenario en el que el usuario está acelerando. (Tenga en cuenta que por ajuste de mínimos cuadrados me refiero a usar las coordenadas como la variable dependiente y el tiempo como la variable independiente).

2. También puede intentar ponderar los puntos de datos según la precisión informada. Cuando la precisión es baja, esos puntos de datos son más bajos.

3. Otra cosa que quizás desee probar es en lugar de mostrar un solo punto, si la precisión es baja, muestre un círculo o algo que indique el rango en el que el usuario podría basarse en la precisión informada. (Esto es lo que hace la aplicación Google Maps integrada del iPhone).

También puedes usar una spline. Ingrese los valores que tiene e interpole puntos entre sus puntos conocidos. Al vincular esto con un ajuste de mínimos cuadrados, un promedio móvil o un filtro kalman (como se menciona en otras respuestas) le da la capacidad de calcular los puntos entre sus puntos "conocidos".

Ser capaz de interpolar los valores entre sus conocimientos le proporciona una transición suave y agradable y una aproximación / razonable / de qué datos estarían presentes si tuviera una mayor fidelidad. http://en.wikipedia.org/wiki/Spline_interpolation

Diferentes splines tienen diferentes características. Los que he visto más comúnmente utilizados son Akima y splines cúbicos.

Otro algoritmo a considerar es el algoritmo de simplificación de línea Ramer-Douglas-Peucker, que se usa con bastante frecuencia en la simplificación de datos GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Volviendo a los filtros Kalman ... Encontré una implementación en C para un filtro Kalman para datos GPS aquí: http://github.com/lacker/ikalman No lo he probado todavía, pero parece prometedor.

Asignado a CoffeeScript si alguien está interesado. ** editar -> perdón por usar backbone también, pero entiendes la idea.

Modificado ligeramente para aceptar una baliza con attribs

{latitud: item.lat, longitud: item.lng, fecha: nueva fecha (item.effective_at), precisión: 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]

He transformado el código Java de @Stochastically a 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
        }
    }
}

Aquí hay una implementación de Javascript de la implementación Java de @ Stochastically para cualquiera que lo necesite:

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

Ejemplo de uso:

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