Dados GPS suaves

Estou trabalhando com dados de GPS, obtendo valores a cada segundo e exibindo a posição atual em um mapa. O problema é que, às vezes (especialmente quando a precisão é baixa), os valores variam muito, fazendo com que a posição atual "salte" entre pontos distantes no mapa.

Eu estava pensando sobre algum método fácil o suficiente para evitar isso. Como primeira idéia, pensei em descartar valores com precisão além de determinado limite, mas acho que existem outras maneiras melhores de fazer isso. Qual é a maneira usual pelos programas executarem isso?

Aqui está um filtro Kalman simples que pode ser usado exatamente nessa situação. Veio de algum trabalho que fiz em dispositivos Android.

A teoria geral dos filtros de Kalman trata de estimativas para vetores, com a precisão das estimativas representadas por matrizes de covariância. No entanto, para estimar a localização em dispositivos Android, a teoria geral se reduz a um caso muito simples. Os fornecedores de localização Android fornecem a localização como latitude e longitude, juntamente com uma precisão especificada como um número único medido em metros. Isso significa que, em vez de uma matriz de covariância, a precisão no filtro Kalman pode ser medida por um único número, mesmo que a localização no filtro Kalman seja medida por dois números. Além disso, o fato de latitude, longitude e metros serem efetivamente todas as unidades diferentes pode ser ignorado, porque se você colocar fatores de escala no filtro Kalman para convertê-los em unidades iguais,

O código pode ser aprimorado, pois assume que a melhor estimativa da localização atual é a última localização conhecida e, se alguém estiver em movimento, deve ser possível usar os sensores do Android para produzir uma estimativa melhor. O código possui um único parâmetro livre Q, expresso em metros por segundo, que descreve a rapidez com que a precisão diminui na ausência de novas estimativas de localização. Um parâmetro Q mais alto significa que a precisão diminui mais rapidamente. Os filtros Kalman geralmente funcionam melhor quando a precisão diminui um pouco mais rapidamente do que se poderia esperar, então, para andar com um telefone Android, acho que Q = 3 metros por segundo funciona bem, mesmo que eu geralmente ande mais devagar do que isso. Mas se estiver viajando em um carro veloz, obviamente deverá ser usado um número muito maior.

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

O que você está procurando é chamado de Filtro Kalman . É frequentemente usado para suavizar dados de navegação . Não é necessariamente trivial, e você pode fazer muitos ajustes, mas é uma abordagem muito padrão e funciona bem. Existe uma biblioteca KFilter disponível, que é uma implementação em C ++.

Meu próximo substituto seria o mínimo de quadrados . Um filtro Kalman suavizará os dados, levando em consideração as velocidades, enquanto uma abordagem de ajuste de mínimos quadrados usará apenas informações posicionais. Ainda assim, é definitivamente mais simples de implementar e entender. Parece que a Biblioteca Científica GNU pode ter uma implementação disso.

Isso pode chegar um pouco tarde ...

Eu escrevi este KalmanLocationManager para Android, que agrupa os dois provedores de localização mais comuns, Rede e GPS, kalman filtra os dados e fornece atualizações para um LocationListener(como os dois provedores 'reais').

Eu o uso principalmente para "interpolar" entre leituras - para receber atualizações (previsões de posição) a cada 100 milis por exemplo (em vez da taxa máxima de gps de um segundo), o que me proporciona uma melhor taxa de quadros ao animar minha posição.

Na verdade, ele usa três filtros kalman, para cada dimensão: latitude, longitude e altitude. Eles são independentes, de qualquer maneira.

Isso facilita muito a matemática da matriz: em vez de usar uma matriz de transição de estado 6x6, eu uso 3 matrizes 2x2 diferentes. Na verdade, no código, não uso matrizes. Resolvidas todas as equações e todos os valores são primitivos (duplo).

O código fonte está funcionando e há uma atividade de demonstração. Desculpem a falta de javadoc em alguns lugares, eu vou me atualizar.

Você não deve calcular a velocidade a partir da mudança de posição por vez. O GPS pode ter posições imprecisas, mas possui velocidade precisa (acima de 5 km / h). Portanto, use a velocidade do carimbo de localização GPS. Além disso, você não deve fazer isso com naturalidade, embora funcione na maioria das vezes.

As posições de GPS, como entregues, já são filtradas por Kalman, você provavelmente não pode melhorar; no pós-processamento, geralmente você não tem as mesmas informações como o chip de GPS.

Você pode suavizá-lo, mas isso também apresenta erros.

Apenas certifique-se de remover as posições quando o dispositivo está parado, isso remove as posições de salto, que alguns dispositivos / configurações não removem.

Eu costumo usar os acelerômetros. Uma mudança repentina de posição em um curto período implica em alta aceleração. Se isso não se reflete na telemetria do acelerômetro, é quase certamente devido a uma alteração nos "três melhores" satélites usados ​​para calcular a posição (à qual me refiro como teleporte por GPS).

Quando um ativo está parado e pulando devido ao teleporte por GPS, se você computar progressivamente o centróide, estará efetivamente cruzando um conjunto cada vez maior de cartuchos, melhorando a precisão.

Para fazer isso quando o ativo não estiver em repouso, você deve estimar sua provável próxima posição e orientação com base na velocidade, rumo e dados de aceleração linear e rotacional (se você tiver giroscópios). É mais ou menos o que o famoso filtro K faz. Você pode adquirir tudo em hardware por cerca de US $ 150 em um AHRS contendo tudo, menos o módulo GPS, e com um conector para conectar um. Possui sua própria CPU e filtragem de Kalman a bordo; os resultados são estáveis ​​e muito bons. A orientação inercial é altamente resistente ao jitter, mas oscila com o tempo. O GPS é propenso a tremores, mas não varia com o tempo, eles foram feitos praticamente para compensar um ao outro.

Um método que utiliza menos matemática / teoria é amostrar 2, 5, 7 ou 10 pontos de dados de cada vez e determinar aqueles que são discrepantes. Uma medida menos precisa de um valor externo que um filtro Kalman é usar o algoritmo a seguir para obter todas as distâncias entre pares entre pontos e eliminar o que estiver mais distante dos outros. Normalmente esses valores são substituídos pelo valor mais próximo do valor externo que você está substituindo

Por exemplo

Suavização em cinco pontos de amostra A, B, C, D, E

ATOTAL = SOMA de distâncias AB AC AD AE

BTOTAL = SOMA de distâncias AB BC BD BE

CTOTAL = SOMA de distâncias AC BC CD CE

DTOTAL = SOMA de distâncias DA DB DC DE

ETOTAL = SOMA de distâncias EA EB EC DE

Se BTOTAL for maior, você substituirá o ponto B por D se BD = min {AB, BC, BD, BE}

Essa suavização determina discrepantes e pode ser aumentada usando o ponto médio do BD em vez do ponto D para suavizar a linha posicional. Sua milhagem pode variar e existem soluções matematicamente mais rigorosas.

Quanto ao mínimo de quadrados, aqui estão algumas outras coisas para experimentar:

1. Só porque o tamanho dos mínimos quadrados não significa que ele precisa ser linear. Você pode ajustar ao quadrado uma curva quadrática com os mínimos quadrados, então isso ajustaria um cenário no qual o usuário está acelerando. (Observe que por mínimos quadrados cabe, quero dizer, usar as coordenadas como variável dependente e o tempo como variável independente.)

2. Você também pode tentar ponderar os pontos de dados com base na precisão relatada. Quando a precisão é baixa, esses pontos de dados são mais baixos.

3. Outra coisa que você pode querer tentar é, em vez de exibir um único ponto, se a precisão for baixa exibir um círculo ou algo que indique o intervalo no qual o usuário pode se basear na precisão relatada. (É isso que o aplicativo Google Maps interno do iPhone faz.)

Você também pode usar um spline. Alimente os valores que você possui e interpole pontos entre os pontos conhecidos. Vincular isso com um ajuste de mínimos quadrados, média móvel ou filtro kalman (como mencionado em outras respostas) permite calcular os pontos entre os pontos "conhecidos".

Ser capaz de interpolar os valores entre seus conhecidos fornece uma transição suave e agradável / uma aproximação / razoável de quais dados estariam presentes se você tivesse uma fidelidade mais alta. http://en.wikipedia.org/wiki/Spline_interpolation

Splines diferentes têm características diferentes. Os que eu já vi mais comumente usados ​​são os splines Akima e Cubic.

Outro algoritmo a considerar é o algoritmo de simplificação de linha de Ramer-Douglas-Peucker, que é bastante usado na simplificação de dados de GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

Voltando aos Filtros Kalman ... Encontrei uma implementação C para um filtro Kalman para dados de GPS aqui: http://github.com/lacker/ikalman Ainda não testei, mas parece promissor.

Mapeado para CoffeeScript, se alguém estiver interessado. ** editar -> desculpe também por usar o backbone, mas você entendeu.

Modificado ligeiramente para aceitar um farol com atributos

{latitude: item.lat, longitude: item.lng, data: nova data (item.effective_at), precisão: 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]

Transformei o código Java de @Stochastically em 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
        }
    }
}

Aqui está uma implementação Javascript da implementação Java do @ Stochastically para qualquer pessoa que precise:

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

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