Datos GPS suaves

145

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?

Alabama.
fuente
Siento los malos efectos del "ruido GPS" cuando trato de calcular los valores asociados (derivados) como la velocidad y la pendiente, que son muy discontinuos, especialmente para registros de seguimiento de alta frecuencia de muestreo (ya que el tiempo tiene una resolución entera [un segundo]).
heltonbiker
44
(también, si está navegando a través de carreteras principales, puede usar el algoritmo "ajustar a carreteras" siempre que tenga un buen conjunto de datos de hoja de ruta [correcto y preciso]. Solo un pensamiento)
heltonbiker
Me enfrento a este problema para la mejor precisión también.
ViruMax

Respuestas:

80

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;
        }
    }
}
Estocástico
fuente
1
¿No debería ser el cálculo de la varianza: varianza + = TimeInc_milliseconds * TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000000
Horacio
44
@Horacio, sé por qué piensas eso, ¡pero no! Matemáticamente, la incertidumbre aquí está siendo modelada por un proceso Wiener (ver en.wikipedia.org/wiki/Wiener_process ) y con un proceso Wiener la varianza crece linealmente con el tiempo. La variable Q_metres_per_secondcorresponde a la variable sigmaen la sección "Procesos relacionados" en ese artículo de Wikipedia. Q_metres_per_secondes una desviación estándar y se mide en metros, por lo que metros y no metros / segundos son sus unidades. Corresponde a la desviación estándar de la distribución después de que haya transcurrido 1 segundo.
Estocástico
3
Intenté este enfoque y el código, pero terminó acortando demasiado la distancia total. Lo hizo demasiado impreciso.
Andreas Rudolph el
1
@ user2999943 sí, use el código para procesar las coordenadas que obtiene de onLocationChanged ().
Estocástico
2
@Koray si no tiene información de precisión, entonces no puede usar un filtro de Kalman. Es completamente fundamental para lo que el filtro de Kalman está tratando de hacer.
Estocástico
75

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.

Chris Arguin
fuente
1
Gracias Chris Sí, leí acerca de Kalman mientras hacía algunas búsquedas, pero ciertamente está un poco más allá de mi conocimiento matemático. ¿Conoce algún código de muestra fácil de leer (y comprender) o, mejor aún, alguna implementación disponible? (C / C ++ / Java)
Al.
1
@Al Desafortunadamente, mi única exposición con los filtros Kalman es a través del trabajo, por lo que tengo un código maravillosamente elegante que no puedo mostrar.
Chris Arguin
No hay problema :-) Traté de buscar, pero por alguna razón parece que esta cosa de Kalman es magia negra. Muchas páginas de teoría pero poco o nada de código. Gracias, probaré los otros métodos.
Al.
2
kalman.sourceforge.net/index.php aquí está la implementación en C ++ del filtro Kalman.
Rostyslav Druzhchenko
1
@ChrisArguin De nada. Avísame si el resultado es bueno por favor.
Rostyslav Druzhchenko
11

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.

villoren
fuente
1
Traté de usar su código lib, obtuve algunos resultados no deseados, no estoy seguro de si estoy haciendo algo mal ... (A continuación se muestra la URL de la imagen, el azul es la ruta de las ubicaciones filtradas, el naranja son las ubicaciones sin procesar) app.box. com / s / w3uvaz007glp2utvgznmh8vlggvaiifk
umesh
Los picos que está viendo 'creciendo' desde la media (línea naranja) parecen actualizaciones de proveedores de red. ¿Puedes intentar trazar las actualizaciones de red en bruto y gps? Quizás estaría mejor sin actualizaciones de red, dependiendo de lo que intente lograr. Por cierto, ¿de dónde sacas esas actualizaciones de naranja sin procesar?
villoren
1
los puntos naranjas son del proveedor de gps, y los azules son de Kalman,
tracé los
¿Podría enviarme esos datos en algún formato de texto? Cada actualización de ubicación tiene el conjunto de campos Location.getProvider (). Solo un archivo con todo Location.toString ().
villoren
9

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.

AlexWien
fuente
55
¿Podría proporcionar algunas referencias para esto por favor?
ivyleavedtoadflax
1
Hay mucha información y mucha experiencia profesional en esas oraciones, ¿para qué oración exactamente quieres una referencia? para velocidad: busca el efecto doppler y el GPS. Kalman interno? Este es un conocimiento básico de GPS, cada documento o libro que describe cómo funciona internamente un chip GPS. smootig-errors: siempre suavizado introducir erros. ¿estarse quieto? Pruébalo.
AlexWien
2
El "saltar" cuando está parado no es la única fuente de error. También hay reflejos de señal (por ejemplo, desde montañas) donde la posición salta. Mis chips GPS (por ejemplo, Garmin Dakota 20, SonyEricsson Neo) no han filtrado esto ... Y lo que es realmente una broma es el valor de elevación de las señales GPS cuando no se combina con la presión barométrica. Estos valores no se filtran o no quiero ver los valores sin filtrar.
hgoebl
1
@AlexWien GPS calcula la distancia desde un punto a la vez hasta una tolerancia que le da una esfera con grosor, una carcasa centrada alrededor de un satélite. Estás en algún lugar de este volumen de shell. La intersección de tres de estos volúmenes de shell le proporciona un volumen de posición, cuyo centroide es su posición calculada. Si tiene un conjunto de posiciones informadas y sabe que el sensor está en reposo, calcular el centroide se cruza efectivamente con muchos más depósitos, lo que mejora la precisión. El error en este caso se reduce .
Peter Wone
66
"Las posiciones GPS, tal como se entregaron, ya están filtradas por Kalman, probablemente no puedas mejorar". Si puede señalar una fuente que confirme esto para los teléfonos inteligentes modernos (por ejemplo), sería muy útil. No puedo ver evidencia de ello yo mismo. Incluso el simple filtrado de Kalman de las ubicaciones en bruto de un dispositivo sugiere fuertemente que no es cierto. Las ubicaciones en bruto bailan erráticamente, mientras que las ubicaciones filtradas a menudo se mantienen cerca de la ubicación real (conocida).
sobri
6

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í.

Peter Wone
fuente
4

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.

ojblass
fuente
Gracias, también lo intentaré. Tenga en cuenta que quiero suavizar la posición actual, ya que es la que se muestra y la que se usa para recuperar algunos datos. No me interesan los puntos pasados. Mi idea original era usar medios ponderados, pero aún tengo que ver qué es lo mejor.
Al.
1
Al, esto parece ser una forma de medios ponderados. Necesitará usar puntos "pasados" si desea suavizar, porque el sistema necesita tener más de la posición actual para saber dónde suavizar también. Si su GPS toma puntos de datos una vez por segundo y su usuario mira la pantalla una vez cada cinco segundos, ¡puede usar 5 puntos de datos sin que él lo note! Un promedio móvil solo se retrasaría un dp también.
Karl el
4

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

Alex319
fuente
3

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 )

Aidos
fuente
3

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.

KarateSnowMáquina
fuente
0

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]
lucygenik
fuente
Intenté editar esto, pero hay un error tipográfico en las últimas líneas donde @laty @lngse configuran. Debería ser +=más que=
jdixon04
0

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
        }
    }
}
inspire_coding
fuente
0

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