Données GPS fluides

Je travaille avec des données GPS, j'obtiens des valeurs toutes les secondes et j'affiche la position actuelle sur une carte. Le problème est que parfois (spécialement lorsque la précision est faible) les valeurs varient beaucoup, ce qui fait que la position actuelle «saute» entre des points distants de la carte.

Je me posais des questions sur une méthode assez simple pour éviter cela. Comme première idée, j'ai pensé à rejeter les valeurs avec une précision au-delà d'un certain seuil, mais je suppose qu'il existe d'autres meilleures façons de faire. Quelle est la manière habituelle dont les programmes effectuent cela?

Voici un simple filtre de Kalman qui pourrait être utilisé exactement dans cette situation. Cela vient d'un travail que j'ai effectué sur les appareils Android.

La théorie générale des filtres de Kalman concerne les estimations des vecteurs, la précision des estimations étant représentée par des matrices de covariance. Cependant, pour estimer l'emplacement sur les appareils Android, la théorie générale se réduit à un cas très simple. Les fournisseurs de localisation Android donnent l'emplacement sous forme de latitude et de longitude, avec une précision qui est spécifiée sous la forme d'un nombre unique mesuré en mètres. Cela signifie qu'au lieu d'une matrice de covariance, la précision du filtre de Kalman peut être mesurée par un seul nombre, même si l'emplacement dans le filtre de Kalman est mesuré par deux nombres. De plus, le fait que la latitude, la longitude et les mètres soient effectivement des unités différentes peut être ignoré, car si vous mettez des facteurs de mise à l'échelle dans le filtre de Kalman pour les convertir tous dans les mêmes unités,

Le code pourrait être amélioré, car il suppose que la meilleure estimation de l'emplacement actuel est le dernier emplacement connu, et si quelqu'un se déplace, il devrait être possible d'utiliser les capteurs d'Android pour produire une meilleure estimation. Le code a un seul paramètre libre Q, exprimé en mètres par seconde, qui décrit la vitesse à laquelle la précision diminue en l'absence de toute nouvelle estimation d'emplacement. Un paramètre Q plus élevé signifie que la précision diminue plus rapidement. Les filtres de Kalman fonctionnent généralement mieux lorsque la précision diminue un peu plus rapidement que ce à quoi on pourrait s'attendre, donc pour me promener avec un téléphone Android, je trouve que Q = 3 mètres par seconde fonctionne bien, même si je marche généralement plus lentement que cela. Mais si vous voyagez dans une voiture rapide, un nombre beaucoup plus important doit évidemment être utilisé.

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

Ce que vous recherchez s'appelle un filtre de Kalman . Il est fréquemment utilisé pour lisser les données de navigation . Ce n'est pas nécessairement anodin, et vous pouvez faire beaucoup de réglages, mais c'est une approche très standard et fonctionne bien. Il existe une bibliothèque KFilter disponible qui est une implémentation C ++.

Ma prochaine solution de rechange serait l' ajustement des moindres carrés . Un filtre de Kalman lissera les données en tenant compte des vitesses, alors qu'une approche d'ajustement des moindres carrés utilisera simplement les informations de position. Pourtant, il est nettement plus simple à mettre en œuvre et à comprendre. Il semble que la bibliothèque scientifique GNU puisse en avoir une implémentation.

Cela pourrait arriver un peu tard ...

J'ai écrit ce KalmanLocationManager pour Android, qui englobe les deux fournisseurs de localisation les plus courants, le réseau et le GPS, kalman-filtre les données et fournit des mises à jour à un LocationListener(comme les deux «vrais» fournisseurs).

Je l'utilise principalement pour "interpoler" entre les lectures - pour recevoir des mises à jour (prédictions de position) toutes les 100 millis par exemple (au lieu de la fréquence GPS maximale d'une seconde), ce qui me donne une meilleure fréquence d'images lors de l'animation de ma position.

En fait, il utilise trois filtres kalman, activés pour chaque dimension: latitude, longitude et altitude. Ils sont indépendants, de toute façon.

Cela rend le calcul matriciel beaucoup plus facile: au lieu d'utiliser une matrice de transition d'état 6x6, j'utilise 3 matrices 2x2 différentes. En fait, dans le code, je n'utilise pas du tout de matrices. Résolu toutes les équations et toutes les valeurs sont des primitives (double).

Le code source fonctionne et il y a une activité de démonstration. Désolé pour le manque de javadoc à certains endroits, je vais me rattraper.

Vous ne devez pas calculer la vitesse à partir du changement de position par heure. Le GPS peut avoir des positions inexactes, mais il a une vitesse précise (supérieure à 5 km / h). Utilisez donc la vitesse du tampon de localisation GPS. Et de plus, vous ne devriez pas faire cela avec bien sûr, même si cela fonctionne la plupart du temps.

Les positions GPS, telles que livrées, sont déjà filtrées Kalman, vous ne pouvez probablement pas vous améliorer, en post-traitement généralement vous n'avez pas les mêmes informations que la puce GPS.

Vous pouvez le lisser, mais cela introduit également des erreurs.

Assurez-vous simplement que vous supprimez les positions lorsque l'appareil est immobile, cela supprime les positions de saut, que certains appareils / configurations ne suppriment pas.

J'utilise habituellement les accéléromètres. Un changement soudain de position sur une courte période implique une forte accélération. Si cela n'est pas reflété dans la télémétrie de l'accéléromètre, c'est presque certainement dû à un changement dans les "trois meilleurs" satellites utilisés pour calculer la position (que j'appelle la téléportation GPS).

Lorsqu'un actif est au repos et sautille en raison de la téléportation GPS, si vous calculez progressivement le centre de gravité, vous intersectez effectivement un ensemble de coques de plus en plus grand, améliorant ainsi la précision.

Pour ce faire lorsque l'actif n'est pas au repos, vous devez estimer sa prochaine position et son orientation probables en fonction de la vitesse, du cap et des données d'accélération linéaire et rotationnelle (si vous avez des gyroscopes). C'est plus ou moins ce que fait le fameux filtre K. Vous pouvez obtenir le tout en matériel pour environ 150 $ sur un AHRS contenant tout sauf le module GPS, et avec une prise pour en connecter un. Il a son propre processeur et le filtrage Kalman à bord; les résultats sont stables et assez bons. Le guidage inertiel est très résistant à la gigue mais dérive avec le temps. Le GPS est sujet à la gigue mais ne dérive pas avec le temps, ils ont pratiquement été faits pour se compenser.

Une méthode qui utilise moins de mathématiques / théorie consiste à échantillonner 2, 5, 7 ou 10 points de données à la fois et à déterminer ceux qui sont des valeurs aberrantes. Une mesure moins précise d'une valeur aberrante qu'un filtre de Kalman consiste à utiliser l' algorithme suivant pour prendre toutes les distances par paire entre les points et rejeter celle qui est la plus éloignée des autres. En général, ces valeurs sont remplacées par la valeur la plus proche de la valeur aberrante que vous remplacez

Par exemple

Lissage à cinq points d'échantillonnage A, B, C, D, E

ATOTAL = SOMME des distances AB AC AD AE

BTOTAL = SOMME des distances AB BC BD BE

CTOTAL = SOMME des distances AC BC CD CE

DTOTAL = SOMME des distances DA DB DC DE

ETOTAL = SOMME des distances EA EB EC DE

Si BTOTAL est le plus grand, vous remplaceriez le point B par D si BD = min {AB, BC, BD, BE}

Ce lissage détermine les valeurs aberrantes et peut être augmenté en utilisant le point médian de BD au lieu du point D pour lisser la ligne de position. Votre kilométrage peut varier et des solutions plus rigoureuses mathématiquement existent.

En ce qui concerne l'ajustement des moindres carrés, voici quelques autres choses à expérimenter:

1. Ce n'est pas parce qu'il est ajusté aux moindres carrés qu'il doit être linéaire. Vous pouvez ajuster une courbe quadratique aux données par les moindres carrés, cela conviendrait alors à un scénario dans lequel l'utilisateur accélère. (Notez que par ajustement des moindres carrés, je veux dire utiliser les coordonnées comme variable dépendante et le temps comme variable indépendante.)

2. Vous pouvez également essayer de pondérer les points de données en fonction de la précision rapportée. Lorsque la précision est faible, ces points de données sont inférieurs.

3. Une autre chose que vous voudrez peut-être essayer est plutôt que d'afficher un seul point, si la précision est faible, affichez un cercle ou quelque chose indiquant la plage dans laquelle l'utilisateur pourrait être basé sur la précision rapportée. (C'est ce que fait l'application Google Maps intégrée à l'iPhone.)

Vous pouvez également utiliser une spline. Introduisez les valeurs que vous avez et interpolez les points entre vos points connus. Lier cela avec un ajustement des moindres carrés, une moyenne mobile ou un filtre de kalman (comme mentionné dans d'autres réponses) vous donne la possibilité de calculer les points entre vos points "connus".

Être capable d'interpoler les valeurs entre vos connus vous donne une belle transition en douceur et une / raisonnable / approximation de quelles données seraient présentes si vous aviez une plus haute fidélité. http://en.wikipedia.org/wiki/Spline_interpolation

Différentes splines ont des caractéristiques différentes. Ceux que j'ai vu les plus couramment utilisés sont les splines Akima et Cubic.

Un autre algorithme à considérer est l'algorithme de simplification de ligne Ramer-Douglas-Peucker, il est assez couramment utilisé dans la simplification des données GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Pour en revenir aux filtres Kalman ... J'ai trouvé une implémentation C pour un filtre Kalman pour les données GPS ici: http://github.com/lacker/ikalman Je ne l'ai pas encore essayé, mais cela semble prometteur.

Mappé sur CoffeeScript si quelqu'un est intéressé. ** modifier -> désolé d'utiliser backbone aussi, mais vous voyez l'idée.

Modifié légèrement pour accepter une balise avec des attributs

{latitude: item.lat, longitude: item.lng, date: nouvelle Date (item.effective_at), précision: 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]

J'ai transformé le code Java de @Stochastically en 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
        }
    }
}

Voici une implémentation Javascript de l'implémentation Java de @ Stochastically pour tous ceux qui en ont besoin:

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

Exemple d'utilisation:

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