Płynne dane GPS

Pracuję z danymi GPS, pobieram wartości co sekundę i wyświetlam aktualną pozycję na mapie. Problem polega na tym, że czasami (szczególnie gdy dokładność jest niska) wartości bardzo się różnią, przez co bieżąca pozycja „przeskakuje” między odległymi punktami na mapie.

Zastanawiałem się nad jakąś dość prostą metodą, aby tego uniknąć. Pierwszym pomysłem było odrzucenie wartości z dokładnością przekraczającą pewien próg, ale myślę, że są inne lepsze sposoby na zrobienie tego. Jak zwykle wykonują to programy?

Oto prosty filtr Kalmana, którego można użyć w dokładnie takiej sytuacji. Pochodzi z pracy, którą wykonałem na urządzeniach z Androidem.

Ogólna teoria filtru Kalmana dotyczy oszacowań wektorów, z dokładnością oszacowań reprezentowaną przez macierze kowariancji. Jednak w przypadku szacowania lokalizacji na urządzeniach z Androidem ogólna teoria sprowadza się do bardzo prostego przypadku. Dostawcy lokalizacji w systemie Android podają lokalizację jako szerokość i długość geograficzną, wraz z dokładnością określaną jako pojedyncza liczba mierzona w metrach. Oznacza to, że zamiast macierzy kowariancji, dokładność w filtrze Kalmana może być mierzona za pomocą jednej liczby, nawet jeśli lokalizacja w filtrze Kalmana jest mierzona przez dwie liczby. Również fakt, że szerokość, długość i metry są efektywnymi różnymi jednostkami, można zignorować, ponieważ jeśli umieścisz współczynniki skalowania w filtrze Kalmana, aby przekonwertować je wszystkie na te same jednostki,

Kod można by ulepszyć, bo zakłada, że ​​najlepszym oszacowaniem aktualnej lokalizacji jest ostatnia znana lokalizacja, a jeśli ktoś się przemieszcza, powinno być możliwe wykorzystanie czujników Androida do uzyskania lepszego oszacowania. Kod ma jeden wolny parametr Q, wyrażony w metrach na sekundę, który opisuje, jak szybko spada dokładność przy braku jakichkolwiek nowych szacunków lokalizacji. Wyższy parametr Q oznacza, że ​​dokładność spada szybciej. Filtry Kalmana generalnie działają lepiej, gdy dokładność spada nieco szybciej niż można by się spodziewać, więc podczas chodzenia z telefonem z Androidem stwierdzam, że Q = 3 metry na sekundę działa dobrze, mimo że ogólnie chodzę wolniej. Ale jeśli podróżujesz szybkim samochodem, oczywiście należy użyć znacznie większej liczby.

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

To, czego szukasz, nazywa się filtrem Kalmana . Jest często używany do wygładzania danych nawigacyjnych . Niekoniecznie jest to trywialne i jest wiele możliwości dostrojenia, ale jest to bardzo standardowe podejście i działa dobrze. Dostępna jest biblioteka KFilter, która jest implementacją C ++.

Moją następną alternatywą byłoby dopasowanie metodą najmniejszych kwadratów . Filtr Kalmana wygładzi dane biorąc pod uwagę prędkości, podczas gdy metoda dopasowania metodą najmniejszych kwadratów będzie po prostu używać informacji o położeniu. Mimo to jest zdecydowanie łatwiejszy do wdrożenia i zrozumienia. Wygląda na to, że Biblioteka naukowa GNU może mieć taką implementację.

To może przyjść trochę późno ...

ja to napisałem KalmanLocationManager dla Androida, który otacza dwóch najpopularniejszych dostawców lokalizacji, sieć i GPS, kalman filtruje dane i dostarcza aktualizacje doLocationListener (podobnie jak dwóch „prawdziwych” dostawców).

Używam go głównie do „interpolacji” między odczytami - na przykład do otrzymywania aktualizacji (przewidywań pozycji) co 100 milisekund (zamiast maksymalnej częstotliwości GPS wynoszącej jedną sekundę), co daje mi lepszą liczbę klatek na sekundę podczas animowania mojej pozycji.

W rzeczywistości używa trzech filtrów Kalmana dla każdego wymiaru: szerokości geograficznej, długości geograficznej i wysokości. W każdym razie są niezależni.

To sprawia, że ​​matematyka macierzowa jest znacznie łatwiejsza: zamiast jednej macierzy przejść stanów 6x6, używam 3 różnych macierzy 2x2. Właściwie w kodzie w ogóle nie używam macierzy. Rozwiązano wszystkie równania i wszystkie wartości są prymitywami (podwójne).

Kod źródłowy działa i jest czynność demonstracyjna. Przepraszam za brak javadoc w niektórych miejscach, nadrobię zaległości.

Nie należy obliczać prędkości na podstawie zmiany pozycji w czasie. GPS może mieć niedokładne pozycje, ale ma dokładną prędkość (powyżej 5 km / h). Więc użyj prędkości ze znacznika lokalizacji GPS. I dalej nie powinieneś tego robić oczywiście, chociaż działa to w większości przypadków.

Pozycje GPS w stanie dostarczonym są już filtrowane przez Kalmana, prawdopodobnie nie można ich poprawić, w postprocessingu zwykle nie masz tych samych informacji, co chip GPS.

Możesz to wygładzić, ale to również wprowadza błędy.

Tylko upewnij się, że usuwasz pozycje, gdy urządzenie stoi nieruchomo, usuwa to pozycje skoku, których niektóre urządzenia / konfiguracje nie usuwają.

Zwykle używam akcelerometrów. Nagła zmiana pozycji w krótkim okresie oznacza duże przyspieszenie. Jeśli nie znajduje to odzwierciedlenia w telemetrii akcelerometru, to prawie na pewno jest to spowodowane zmianą w „trzech najlepszych” satelitach używanych do obliczania pozycji (co nazywam teleportacją GPS).

Kiedy zasób jest w stanie spoczynku i skacze z powodu teleportacji GPS, jeśli stopniowo obliczasz środek ciężkości, skutecznie przecinasz coraz większy zestaw pocisków, poprawiając precyzję.

Aby to zrobić, gdy zasób nie jest w spoczynku, musisz oszacować jego prawdopodobną następną pozycję i orientację w oparciu o dane dotyczące prędkości, kursu oraz przyspieszenia liniowego i obrotowego (jeśli masz żyroskopy). Tak mniej więcej robi słynny filtr K. Całość można kupić w sprzęcie za około 150 USD na AHRS zawierającym wszystko oprócz modułu GPS i z gniazdem do podłączenia. Posiada własny procesor i filtrowanie Kalmana na pokładzie; wyniki są stabilne i całkiem dobre. Prowadzenie bezwładnościowe jest wysoce odporne na drgania, ale z czasem ulega dryfowaniu. GPS jest podatny na jitter, ale nie dryfuje z czasem, praktycznie zostały stworzone, aby się wzajemnie kompensować.

Jedną z metod, która wykorzystuje mniej matematyki / teorii, jest próbkowanie 2, 5, 7 lub 10 punktów danych naraz i określenie tych, które są wartościami odstającymi. Mniej dokładną miarą wartości odstającej niż filtr Kalmana jest użycie następującego algorytmu do obliczenia odległości między punktami wszystkich par i odrzucenia tego, który jest najbardziej oddalony od pozostałych. Zazwyczaj te wartości są zastępowane wartością najbliższą wartości odstającej, którą zastępujesz

Na przykład

Wygładzanie w pięciu punktach próbkowania A, B, C, D, E

ATOTAL = SUMA odległości AB AC AD AE

BTOTAL = SUMA odległości AB BC BD BE

CTOTAL = SUMA odległości AC BC CD CE

DTOTAL = SUMA odległości DA DB DC DE

ETOTAL = SUMA odległości EA EB EC DE

Jeśli BTOTAL jest największy, należy zamienić punkt B na D, jeśli BD = min {AB, BC, BD, BE}

To wygładzanie określa wartości odstające i może być wzmocnione przez użycie środka BD zamiast punktu D do wygładzenia linii pozycyjnej. Twój przebieg może się różnić i istnieją bardziej rygorystyczne matematycznie rozwiązania.

Jeśli chodzi o dopasowanie metodą najmniejszych kwadratów, oto kilka innych rzeczy do eksperymentowania:

1. To, że pasuje do najmniejszych kwadratów, nie oznacza, że ​​musi być liniowe. Możesz dopasować krzywą kwadratową do danych metodą najmniejszych kwadratów, wtedy pasowałoby to do scenariusza, w którym użytkownik przyspiesza. (Zauważ, że przez dopasowanie metodą najmniejszych kwadratów mam na myśli użycie współrzędnych jako zmiennej zależnej i czasu jako zmiennej niezależnej).

2. Możesz także spróbować zważyć punkty danych na podstawie podanej dokładności. Gdy dokładność jest niska, te punkty danych są niższe.

3. Inną rzeczą, którą możesz chcieć spróbować, jest zamiast wyświetlania pojedynczego punktu, jeśli dokładność jest niska, wyświetl okrąg lub coś wskazującego zakres, w którym użytkownik może opierać się na raportowanej dokładności. (To właśnie robi wbudowana aplikacja Google Maps w telefonie iPhone).

Możesz także użyć splajnu. Wprowadź wartości, które masz i interpoluj punkty między znanymi punktami. Powiązanie tego z dopasowaniem najmniejszych kwadratów, średnią ruchomą lub filtrem Kalmana (jak wspomniano w innych odpowiedziach) daje możliwość obliczenia punktów pomiędzy „znanymi” punktami.

Możliwość interpolacji wartości między znanymi daje przyjemne płynne przejście i / rozsądne / przybliżenie tego, jakie dane byłyby obecne, gdybyś miał wyższą dokładność. http://en.wikipedia.org/wiki/Spline_interpolation

Różne splajny mają różne cechy. Najczęściej używanymi, które widziałem, są krzywe Akima i Cubic.

Innym algorytmem, który należy wziąć pod uwagę, jest algorytm upraszczania linii Ramera-Douglasa-Peuckera, który jest dość powszechnie stosowany w upraszczaniu danych GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Wracając do filtrów Kalmana ... Znalazłem implementację C dla filtra Kalmana dla danych GPS tutaj: http://github.com/lacker/ikalman Nie wypróbowałem jeszcze tego, ale wydaje się obiecujący.

Zmapowane do CoffeeScript, jeśli ktoś jest zainteresowany. ** edit -> przepraszam za używanie kręgosłupa, ale masz pomysł.

Nieznacznie zmodyfikowano, aby zaakceptować beacon z atrybutami

{latitude: item.lat, longitude: item.lng, date: new Date (item.effective_at), dokładność: 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]

Przekształciłem kod Javy z @Stochastically na 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
        }
    }
}

Oto implementacja Javascript implementacji Java @ Stochastically dla każdego, kto jej potrzebuje:

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

Przykład użycia:

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