Reibungslose GPS-Daten

145

Ich arbeite mit GPS-Daten, erhalte jede Sekunde Werte und zeige die aktuelle Position auf einer Karte an. Das Problem ist, dass manchmal (besonders wenn die Genauigkeit niedrig ist) die Werte stark variieren, wodurch die aktuelle Position zwischen entfernten Punkten in der Karte "springt".

Ich habe mich über eine Methode gewundert, die einfach genug ist, um dies zu vermeiden. Als erste Idee habe ich darüber nachgedacht, Werte mit einer Genauigkeit über einen bestimmten Schwellenwert hinaus zu verwerfen, aber ich denke, es gibt einige andere bessere Möglichkeiten. Wie führen Programme dies normalerweise durch?

Al.
quelle
Ich spüre die schlechten Auswirkungen des "GPS-Rauschens" beim Versuch, zugehörige (abgeleitete) Werte wie Geschwindigkeit und Steigung zu berechnen, die speziell für Tracklogs mit hoher Abtastrate sehr diskontinuierlich sind (da die Zeit eine ganzzahlige [eine Sekunde] Auflösung hat).
Heltonbiker
4
(Auch wenn Sie durch Hauptstraßen navigieren, können Sie den Algorithmus "An Straßen
ausrichten
Ich stehe diesem Problem auch für die beste Genauigkeit gegenüber.
ViruMax

Antworten:

80

Hier ist ein einfacher Kalman-Filter, der genau für diese Situation verwendet werden kann. Es kam von einer Arbeit, die ich auf Android-Geräten gemacht habe.

In der allgemeinen Kalman-Filtertheorie dreht sich alles um Schätzungen für Vektoren, wobei die Genauigkeit der Schätzungen durch Kovarianzmatrizen dargestellt wird. Für die Standortschätzung auf Android-Geräten reduziert sich die allgemeine Theorie jedoch auf einen sehr einfachen Fall. Android-Standortanbieter geben den Standort als Breiten- und Längengrad sowie eine Genauigkeit an, die als einzelne Zahl in Metern angegeben wird. Dies bedeutet, dass anstelle einer Kovarianzmatrix die Genauigkeit im Kalman-Filter durch eine einzelne Zahl gemessen werden kann, obwohl die Position im Kalman-Filter durch zwei Zahlen gemessen wird. Auch die Tatsache, dass Breite, Länge und Meter effektiv alle unterschiedliche Einheiten sind, kann ignoriert werden, denn wenn Sie Skalierungsfaktoren in den Kalman-Filter einfügen, um sie alle in dieselben Einheiten umzuwandeln,

Der Code könnte verbessert werden, da davon ausgegangen wird, dass die beste Schätzung des aktuellen Standorts der letzte bekannte Standort ist. Wenn sich jemand bewegt, sollte es möglich sein, die Sensoren von Android zu verwenden, um eine bessere Schätzung zu erstellen. Der Code hat einen einzelnen freien Parameter Q, ausgedrückt in Metern pro Sekunde, der beschreibt, wie schnell die Genauigkeit abnimmt, wenn keine neuen Standortschätzungen vorliegen. Ein höherer Q-Parameter bedeutet, dass die Genauigkeit schneller abnimmt. Kalman-Filter funktionieren im Allgemeinen besser, wenn die Genauigkeit etwas schneller abnimmt als erwartet. Wenn ich also mit einem Android-Telefon herumlaufe, funktioniert Q = 3 Meter pro Sekunde einwandfrei, obwohl ich im Allgemeinen langsamer gehe. Aber wenn Sie in einem schnellen Auto fahren, sollte natürlich eine viel größere Anzahl verwendet werden.

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;
        }
    }
}
Stochastisch
quelle
1
Sollte die Varianzberechnung nicht lauten: Varianz + = TimeInc_milliseconds * TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000000
Horacio
4
@ Horacio, ich weiß warum du das denkst, aber nein! Mathematisch wird die Unsicherheit hier durch einen Wiener-Prozess modelliert (siehe en.wikipedia.org/wiki/Wiener_process ) und mit einem Wiener-Prozess wächst die Varianz linear mit der Zeit. Die Variable Q_metres_per_secondentspricht der Variablen sigmaim Abschnitt "Verwandte Prozesse" in diesem Wikipedia-Artikel. Q_metres_per_secondist eine Standardabweichung und wird in Metern gemessen, daher sind Meter und nicht Meter / Sekunden die Einheiten. Sie entspricht der Standardabweichung der Verteilung nach Ablauf von 1 Sekunde.
Stochastisch
3
Ich habe diesen Ansatz und den Code ausprobiert, aber er hat die Gesamtentfernung zu stark verkürzt. Hat es zu ungenau gemacht.
Andreas Rudolph
1
@ user2999943 Ja, verwenden Sie den Code, um die Koordinaten zu verarbeiten, die Sie von onLocationChanged () erhalten.
Stochastisch
2
@Koray Wenn Sie keine Genauigkeitsinformationen haben, können Sie keinen Kalman-Filter verwenden. Es ist völlig grundlegend für das, was der Kalman-Filter versucht.
Stochastisch
75

Was Sie suchen, wird als Kalman-Filter bezeichnet . Es wird häufig verwendet, um Navigationsdaten zu glätten . Es ist nicht unbedingt trivial und es gibt eine Menge Tuning, die Sie durchführen können, aber es ist ein sehr Standardansatz und funktioniert gut. Es ist eine KFilter-Bibliothek verfügbar, bei der es sich um eine C ++ - Implementierung handelt.

Mein nächster Fallback wäre die Anpassung der kleinsten Quadrate . Ein Kalman-Filter glättet die Daten unter Berücksichtigung von Geschwindigkeiten, während bei einem Ansatz zur Anpassung der kleinsten Quadrate nur Positionsinformationen verwendet werden. Dennoch ist es definitiv einfacher zu implementieren und zu verstehen. Es sieht so aus, als ob die GNU Scientific Library eine Implementierung davon haben könnte.

Chris Arguin
quelle
1
Danke Chris. Ja, ich habe während einer Suche über Kalman gelesen, aber es geht sicherlich etwas über meine mathematischen Kenntnisse hinaus. Kennen Sie Beispielcode, der leicht zu lesen (und zu verstehen!) Ist, oder noch besser, eine Implementierung, die verfügbar ist? (C / C ++ / Java)
Al.
1
@Al Leider ist meine einzige Belichtung mit Kalman-Filtern die Arbeit, daher habe ich einen wunderbar eleganten Code, den ich Ihnen nicht zeigen kann.
Chris Arguin
Kein Problem :-) Ich habe versucht zu suchen, aber aus irgendeinem Grund scheint es, dass dieses Kalman-Ding schwarze Magie ist. Viele Theorieseiten, aber wenig bis gar kein Code. Danke, ich werde die anderen Methoden ausprobieren.
Al.
2
kalman.sourceforge.net/index.php Hier ist die C ++ - Implementierung des Kalman-Filters.
Rostyslav Druzhchenko
1
@ ChrisArguin Gern geschehen. Lassen Sie mich wissen, ob das Ergebnis gut ist.
Rostyslav Druzhchenko
11

Dies könnte etwas spät kommen ...

Ich habe diesen KalmanLocationManager für Android geschrieben, der die beiden gängigsten Standortanbieter, Netzwerk und GPS, zusammenfasst, die Daten kalmanfiltert und Updates an a liefert LocationListener(wie die beiden "echten" Anbieter).

Ich benutze es meistens, um zwischen den Messwerten zu "interpolieren" - um beispielsweise alle 100 Millis Aktualisierungen (Positionsvorhersagen) zu erhalten (anstelle der maximalen GPS-Rate von einer Sekunde), was mir eine bessere Bildrate beim Animieren meiner Position gibt.

Tatsächlich werden drei Kalman-Filter für jede Dimension verwendet: Breite, Länge und Höhe. Sie sind sowieso unabhängig.

Dies macht die Matrixmathematik viel einfacher: Anstatt eine 6x6-Zustandsübergangsmatrix zu verwenden, verwende ich 3 verschiedene 2x2-Matrizen. Eigentlich verwende ich im Code überhaupt keine Matrizen. Alle Gleichungen und alle Werte sind Primitive (doppelt) gelöst.

Der Quellcode funktioniert und es gibt eine Demo-Aktivität. Entschuldigung für den Mangel an Javadoc an einigen Stellen, ich werde aufholen.

villoren
quelle
1
Ich habe versucht, Ihren Lib-Code zu verwenden. Ich habe einige unerwünschte Ergebnisse erhalten. Ich bin mir nicht sicher, ob ich etwas falsch mache. (Unten ist die Bild-URL, Blau ist der Pfad der gefilterten Standorte, Orange sind Rohstandorte.
umesh
Die Spitzen, die vom Mittelwert (orange Linie) "wachsen", sehen aus wie Aktualisierungen von Netzwerkanbietern. Können Sie versuchen, sowohl unformatierte Netzwerk- als auch GPS-Updates zu zeichnen? Vielleicht sind Sie ohne Netzwerkaktualisierungen besser dran, je nachdem, was Sie erreichen möchten. Übrigens, woher bekommen Sie diese rohen orangefarbenen Updates?
Villoren
1
orange Punkte sind von GPS-Anbieter, und die blauen sind von Kalman, ich habe Protokolle auf der Karte
aufgezeichnet
Könnten Sie mir diese Daten in einem Textformat senden? Für jede Standortaktualisierung ist das Feld Location.getProvider () festgelegt. Nur eine Datei mit allen Location.toString ().
Villoren
9

Sie sollten die Geschwindigkeit nicht aus der Positionsänderung pro Zeit berechnen. GPS hat möglicherweise ungenaue Positionen, aber eine genaue Geschwindigkeit (über 5 km / h). Verwenden Sie also die Geschwindigkeit vom GPS-Positionsstempel. Und außerdem sollten Sie das natürlich nicht tun, obwohl es meistens funktioniert.

Die gelieferten GPS-Positionen sind bereits Kalman-gefiltert. Sie können sie wahrscheinlich nicht verbessern. Bei der Nachbearbeitung haben Sie normalerweise nicht die gleichen Informationen wie beim GPS-Chip.

Sie können es glätten, dies führt jedoch auch zu Fehlern.

Stellen Sie einfach sicher, dass Sie die Positionen entfernen, wenn das Gerät stillsteht. Dadurch werden Sprungpositionen entfernt, die einige Geräte / Konfigurationen nicht entfernen.

AlexWien
quelle
5
Könnten Sie bitte einige Referenzen dazu angeben?
ivyleavedtoadflax
1
Es gibt viele Informationen und viel Berufserfahrung in diesen Sätzen. Für welchen Satz möchten Sie genau eine Referenz? für Geschwindigkeit: Suche nach Doppler-Effekt und GPS. interner Kalman? Dies ist grundlegendes GPS-Wissen. Jedes Papier oder Buch beschreibt, wie ein GPS-Chip intern funktioniert. Smootig-Fehler: Immer Glätten führen zu Fehlern. still stehen? Versuch es.
AlexWien
2
Das "Herumspringen" im Stillstand ist nicht die einzige Fehlerquelle. Es gibt auch Signalreflexionen (z. B. von Bergen), bei denen die Position herumspringt. Meine GPS-Chips (z. B. Garmin Dakota 20, SonyEricsson Neo) haben dies nicht herausgefiltert ... Und was wirklich ein Witz ist, ist der Höhenwert der GPS-Signale, wenn sie nicht mit Luftdruck kombiniert werden. Diese Werte werden nicht gefiltert oder ich möchte die ungefilterten Werte nicht sehen.
Hgoebl
1
@AlexWien GPS berechnet die Entfernung von einem Punkt zu einem Zeitpunkt zu einer Toleranz, die Ihnen eine Kugel mit Dicke gibt, eine Hülle, die um einen Satelliten zentriert ist. Sie befinden sich irgendwo in diesem Shell-Volume. Der Schnittpunkt von drei dieser Shell-Volumes ergibt ein Positionsvolumen, dessen Schwerpunkt Ihre berechnete Position ist. Wenn Sie eine Reihe von gemeldeten Positionen haben und wissen, dass der Sensor in Ruhe ist, schneidet die Berechnung des Schwerpunkts effektiv viel mehr Schalen und verbessert so die Präzision. Fehler in diesem Fall wird reduziert .
Peter Wone
6
"Die gelieferten GPS-Positionen sind bereits Kalman-gefiltert, Sie können sie wahrscheinlich nicht verbessern." Wenn Sie auf eine Quelle verweisen können, die dies beispielsweise für moderne Smartphones bestätigt, wäre dies sehr nützlich. Ich kann selbst keine Beweise dafür sehen. Selbst eine einfache Kalman-Filterung der Rohpositionen eines Geräts deutet stark darauf hin, dass dies nicht der Fall ist. Die rohen Orte tanzen unregelmäßig herum, während die gefilterten Orte meistens nahe am realen (bekannten) Ort liegen.
Sobri
6

Normalerweise benutze ich die Beschleunigungsmesser. Ein plötzlicher Positionswechsel in kurzer Zeit impliziert eine hohe Beschleunigung. Wenn sich dies nicht in der Beschleunigungsmesser-Telemetrie widerspiegelt, liegt dies mit ziemlicher Sicherheit an einer Änderung der "besten drei" Satelliten, die zur Berechnung der Position verwendet werden (auf die ich als GPS-Teleportation beziehe).

Wenn ein Objekt aufgrund von GPS-Teleportation in Ruhe ist und herumhüpft und Sie den Schwerpunkt nach und nach berechnen, schneiden Sie effektiv einen immer größeren Satz von Muscheln und verbessern so die Präzision.

Um dies zu tun, wenn sich der Vermögenswert nicht in Ruhe befindet, müssen Sie seine wahrscheinliche nächste Position und Ausrichtung basierend auf Geschwindigkeit, Kurs sowie linearen und rotatorischen (wenn Sie Gyros haben) Beschleunigungsdaten schätzen. Dies ist mehr oder weniger das, was der berühmte K-Filter tut. Sie können das Ganze in Hardware für etwa 150 US-Dollar auf einem AHRS erhalten, das alles außer dem GPS-Modul enthält, und mit einer Buchse, um eines anzuschließen. Es verfügt über eine eigene CPU- und Kalman-Filterung an Bord. Die Ergebnisse sind stabil und recht gut. Die Trägheitsführung ist sehr widerstandsfähig gegen Jitter, driftet jedoch mit der Zeit. GPS ist anfällig für Jitter, driftet aber nicht mit der Zeit, sie wurden praktisch entwickelt, um sich gegenseitig zu kompensieren.

Peter Wone
quelle
4

Eine Methode, die weniger Mathematik / Theorie verwendet, besteht darin, 2, 5, 7 oder 10 Datenpunkte gleichzeitig abzutasten und diejenigen zu bestimmen, die Ausreißer sind. Ein weniger genaues Maß für einen Ausreißer als ein Kalman-Filter ist die Verwendung des folgenden Algorithmus zu verwenden, um alle paarweisen Abstände zwischen Punkten zu ermitteln und den am weitesten von den anderen entfernten zu entfernen. In der Regel werden diese Werte durch den Wert ersetzt, der dem von Ihnen zu ersetzenden Außenwert am nächsten kommt

Beispielsweise

Glätten an fünf Probenpunkten A, B, C, D, E.

ATOTAL = Summe der Abstände AB AC AD AE

BTOTAL = Summe der Abstände AB BC BD BE

CTOTAL = Summe der Abstände AC BC CD CE

DTOTAL = Summe der Abstände DA DB DC DE

ETOTAL = Summe der Abstände EA EB EC DE

Wenn BTOTAL am größten ist, würden Sie Punkt B durch D ersetzen, wenn BD = min {AB, BC, BD, BE}

Diese Glättung bestimmt Ausreißer und kann durch Verwendung des Mittelpunkts von BD anstelle von Punkt D zum Glätten der Positionslinie erweitert werden. Ihr Kilometerstand kann variieren und es gibt mathematisch strengere Lösungen.

ojblass
quelle
Danke, ich werde es auch versuchen. Beachten Sie, dass ich die aktuelle Position glätten möchte, da diese angezeigt und zum Abrufen einiger Daten verwendet wird. Ich interessiere mich nicht für vergangene Punkte. Meine ursprüngliche Idee war es, gewichtete Mittel zu verwenden, aber ich muss immer noch sehen, was am besten ist.
Al.
1
Al, dies scheint eine Form von gewichteten Mitteln zu sein. Sie müssen "vergangene" Punkte verwenden, wenn Sie eine Glättung durchführen möchten, da das System mehr als die aktuelle Position haben muss, um zu wissen, wo auch geglättet werden muss. Wenn Ihr GPS einmal pro Sekunde Datenpunkte erfasst und Ihr Benutzer einmal alle fünf Sekunden auf den Bildschirm schaut, können Sie 5 Datenpunkte verwenden, ohne dass er es bemerkt! Ein gleitender Durchschnitt würde sich ebenfalls nur um einen dp verzögern.
Karl
4

Da die kleinsten Quadrate passen, gibt es noch ein paar andere Dinge, mit denen Sie experimentieren können:

  1. Nur weil die kleinsten Quadrate passen, heißt das nicht, dass es linear sein muss. Sie können eine quadratische Kurve mit kleinsten Quadraten an die Daten anpassen. Dies würde dann zu einem Szenario passen, in dem der Benutzer beschleunigt. (Beachten Sie, dass mit kleinsten Quadraten die Koordinaten als abhängige Variable und die Zeit als unabhängige Variable verwendet werden.)

  2. Sie können auch versuchen, die Datenpunkte anhand der gemeldeten Genauigkeit zu gewichten. Wenn die Genauigkeit gering ist, sind diese Datenpunkte niedriger.

  3. Eine andere Sache, die Sie versuchen möchten, ist, anstatt einen einzelnen Punkt anzuzeigen, wenn die Genauigkeit niedrig ist, einen Kreis oder etwas anzuzeigen, das den Bereich angibt, in dem der Benutzer auf der gemeldeten Genauigkeit basieren könnte. (Dies ist die Funktion der in das iPhone integrierten Google Maps-Anwendung.)

Alex319
quelle
3

Sie können auch einen Spline verwenden. Geben Sie Ihre Werte ein und interpolieren Sie Punkte zwischen Ihren bekannten Punkten. Wenn Sie dies mit einer Anpassung der kleinsten Quadrate, einem gleitenden Durchschnitt oder einem Kalman-Filter (wie in anderen Antworten erwähnt) verknüpfen, können Sie die Punkte zwischen Ihren "bekannten" Punkten berechnen.

In der Lage zu sein, die Werte zwischen Ihren bekannten Daten zu interpolieren, gibt Ihnen einen schönen reibungslosen Übergang und eine / vernünftige / Annäherung daran, welche Daten vorhanden wären, wenn Sie eine höhere Wiedergabetreue hätten. http://en.wikipedia.org/wiki/Spline_interpolation

Unterschiedliche Splines haben unterschiedliche Eigenschaften. Die am häufigsten verwendeten sind Akima und Cubic Splines.

Ein weiterer zu berücksichtigender Algorithmus ist der Ramer-Douglas-Peucker-Algorithmus zur Linienvereinfachung, der häufig zur Vereinfachung von GPS-Daten verwendet wird. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Aidos
quelle
3

Zurück zu den Kalman-Filtern ... Ich habe hier eine C-Implementierung für einen Kalman-Filter für GPS-Daten gefunden: http://github.com/lacker/ikalman Ich habe es noch nicht ausprobiert, aber es scheint vielversprechend.

KarateSnowMachine
quelle
0

Bei Interesse auf CoffeeScript abgebildet. ** edit -> sorry auch mit backbone, aber du kommst auf die idee.

Leicht modifiziert, um ein Leuchtfeuer mit Attributen zu akzeptieren

{Breite: item.lat, Länge: item.lng, Datum: neues Datum (item.effective_at), Genauigkeit: 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
quelle
Versucht, dies zu bearbeiten, aber es gibt einen Tippfehler in den letzten Zeilen, wo @latund gesetzt @lngsind. Sollte +=eher sein als=
jdixon04
0

Ich habe den Java-Code von @Stochastically in Kotlin umgewandelt

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
        }
    }
}
inspir_coding
quelle
0

Hier ist eine Javascript-Implementierung der Java-Implementierung von @ Stochastically für alle, die sie benötigen:

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

Anwendungsbeispiel:

   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
quelle