Гладкие данные GPS
Я работаю с данными GPS, получаю значения каждую секунду и отображаю текущее положение на карте. Проблема в том, что иногда (особенно при низкой точности) значения сильно меняются, что делает текущую позицию "прыгать" между удаленными точками на карте.
Я задавался вопросом о каком-то достаточно простом методе, чтобы избежать этого. В качестве первой идеи я подумал об отбрасывании значений с точностью выше определенного порога, но я думаю, что есть и другие лучшие способы сделать это. Как обычно программы выполняют это?
10 ответов
Вот простой фильтр Калмана, который можно использовать именно для этой ситуации. Это произошло от какой-то работы, которую я сделал на устройствах Android.
Общая теория фильтров Калмана - все об оценках для векторов, с точностью оценок, представленных ковариационными матрицами. Однако для оценки местоположения на устройствах Android общая теория сводится к очень простому случаю. Поставщики местоположения Android предоставляют местоположение в виде широты и долготы вместе с точностью, которая указывается в виде единого числа, измеряемого в метрах. Это означает, что вместо ковариационной матрицы точность в фильтре Калмана может измеряться одним числом, даже если местоположение в фильтре Калмана измеряется двумя числами. Также можно игнорировать тот факт, что широта, долгота и метры фактически являются разными единицами, потому что если вы добавляете коэффициенты масштабирования в фильтр Калмана, чтобы преобразовать их все в одинаковые единицы, то эти коэффициенты масштабирования в конечном итоге отменяются при преобразовании результатов. обратно в оригинальные единицы.
Код можно улучшить, поскольку он предполагает, что наилучшая оценка текущего местоположения - это последнее известное местоположение, и если кто-то движется, то можно использовать датчики Android для получения более точной оценки. Код имеет единственный свободный параметр Q, выраженный в метрах в секунду, который описывает, насколько быстро снижается точность при отсутствии каких-либо новых оценок местоположения. Более высокий параметр Q означает, что точность уменьшается быстрее. Фильтры Калмана, как правило, работают лучше, когда точность снижается немного быстрее, чем можно было бы ожидать, поэтому при обходе с телефоном Android я обнаружил, что Q=3 метра в секунду работает нормально, хотя обычно я иду медленнее. Но если вы путешествуете на быстрой машине, очевидно, следует использовать гораздо большее число.
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;
}
}
}
То, что вы ищете, называется фильтром Калмана. Он часто используется для сглаживания навигационных данных. Это не обязательно тривиально, и вы можете выполнить множество настроек, но это очень стандартный подход, который хорошо работает. Доступна библиотека KFilter, которая является реализацией C++.
Мой следующий запасной вариант будет соответствовать наименьших квадратов. Фильтр Калмана сгладит данные с учетом скоростей, тогда как подход наименьших квадратов будет использовать только информацию о положении. Тем не менее, это определенно проще для реализации и понимания. Похоже, что научная библиотека GNU может иметь такую реализацию.
Это может прийти немного поздно...
Я написал этот https://github.com/villoren/KalmanLocationManager для Android, который объединяет двух наиболее распространенных провайдеров определения местоположения: сеть и GPS, kalman-фильтр данных и доставляет обновления для LocationListener (как два "настоящих" провайдера).
Я использую его в основном для "интерполяции" между показаниями - например, для получения обновлений (прогнозов положения) каждые 100 миллисекунд (вместо максимальной скорости gps в одну секунду), что дает мне лучшую частоту кадров при анимации моей позиции.
На самом деле он использует три фильтра Калмана для каждого измерения: широта, долгота и высота. Во всяком случае, они независимы.
Это значительно упрощает математику: вместо использования одной матрицы перехода состояний 6x6 я использую 3 разные матрицы 2x2. На самом деле в коде я вообще не использую матрицы. Решены все уравнения и все значения являются примитивами (двойной).
Исходный код работает, и есть демонстрационная активность. Извините за отсутствие javadoc в некоторых местах, я догоню.
Вы не должны рассчитывать скорость от изменения положения за раз. GPS может иметь неточные положения, но имеет точную скорость (выше 5 км / ч). Так что используйте скорость из отметки местоположения GPS. И далее вы не должны делать это с курсом, хотя это работает в большинстве случаев.
Позиции GPS в том виде, в котором они были доставлены, уже отфильтрованы по Калману, и вы, вероятно, не сможете их улучшить. Обычно при постобработке у вас не такая информация, как у чипа GPS.
Вы можете сгладить его, но это также приводит к ошибкам.
Просто убедитесь, что вы удаляете позиции, когда устройство стоит на месте, это удаляет прыжковые позиции, которые не удаляются некоторыми устройствами / конфигурациями.
Я обычно использую акселерометры. Внезапное изменение положения за короткий период подразумевает высокое ускорение. Если это не отражено в телеметрии акселерометра, то это почти наверняка связано с изменением "лучших трех" спутников, используемых для вычисления положения (которое я называю GPS-телепортацией).
Когда актив находится в состоянии покоя и перемещается из-за телепортации GPS, если вы постепенно вычисляете центроид, вы эффективно пересекаете все больший и больший набор оболочек, повышая точность.
Чтобы сделать это, когда актив не находится в состоянии покоя, вы должны оценить его вероятную следующую позицию и ориентацию на основе данных скорости, курса и линейного и вращательного (если у вас есть гироскопы) ускорения. Это более или менее то, что делает знаменитый K-фильтр. Вы можете получить все это аппаратно примерно за 150 долларов на AHRS, содержащей все, кроме модуля GPS, и с разъемом для подключения. Он имеет свой собственный процессор и фильтрацию Калмана на борту; результаты стабильны и довольно хороши. Инерционное наведение очень устойчиво к джиттеру, но дрейфует со временем. GPS склонен к джиттеру, но не дрейфует со временем, они практически созданы для компенсации друг друга.
Один метод, который использует меньше математики / теории, - это выборка 2, 5, 7 или 10 точек данных за раз и определение тех, которые являются выбросами. Менее точный показатель выброса, чем фильтр Калмана, состоит в том, чтобы использовать следующий алгоритм для определения всех парных расстояний между точками и выброса одного, наиболее удаленного от остальных. Обычно эти значения заменяются значением, ближайшим к исходному значению, которое вы заменяете
Например
Сглаживание в пяти точках выборки A, B, C, D, E
ATOTAL = СУММА расстояний AB AC AD AE
BTOTAL = СУММА расстояний AB BC BD BE
CTOTAL = СУММА расстояний AC BC CD CE
DTOTAL = СУММА расстояний DA DB DC DE
ETOTAL = СУММА расстояний EA EB EC DE
Если BTOTAL является наибольшим, вы заменили бы точку B на D, если BD = min { AB, BC, BD, BE }
Это сглаживание определяет выбросы и может быть увеличено путем использования средней точки BD вместо точки D для сглаживания позиционной линии. Ваш пробег может варьироваться, и существуют более математически строгие решения.
Что касается наименьших квадратов, вот еще пара вещей, с которыми можно поэкспериментировать:
То, что он подходит по методу наименьших квадратов, не означает, что он должен быть линейным. Вы можете подгонять квадратичную кривую к данным методом наименьших квадратов, тогда это будет соответствовать сценарию, в котором пользователь ускоряется. (Обратите внимание, что под наименьшими квадратами я подразумеваю использование координат в качестве зависимой переменной и время в качестве независимой переменной.)
Вы также можете попробовать взвешивать точки данных на основе сообщенной точности. Когда точность мала, эти данные снижаются.
Еще одна вещь, которую вы, возможно, захотите попробовать, - это не отображать одну точку, а, если точность низкая, отобразить кружок или что-то, указывающее диапазон, в котором пользователь может основываться на сообщенной точности. (Это то, что делает встроенное приложение Google Maps для iPhone.)
Возвращаясь к фильтрам Калмана... Я нашел реализацию C для фильтра Калмана для данных GPS здесь: http://github.com/lacker/ikalman Я еще не пробовал, но это выглядит многообещающе.
Вот реализация Javascript реализации Java @Stochastically для всех, кто в ней нуждается:
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]
}
}
Пример использования:
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)
}
Вы также можете использовать сплайн. Введите значения, которые у вас есть, и интерполируйте точки между вашими известными точками. Связав это с подгонкой по методу наименьших квадратов, скользящим средним или фильтром Калмана (как упоминалось в других ответах), вы сможете рассчитать точки между вашими "известными" точками.
Возможность интерполировать значения между вашими известными дает вам хороший плавный переход и / разумное / приблизительное представление о том, какие данные были бы представлены, если бы у вас была более высокая точность. http://en.wikipedia.org/wiki/Spline_interpolation
Разные сплайны имеют разные характеристики. Наиболее часто используемые сплайны - Акима и Кубические сплайны.
Другой алгоритм, который следует рассмотреть, - это алгоритм упрощения линий Рамера-Дугласа-Пекера, он довольно часто используется для упрощения данных GPS. ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm)
Я преобразовал Java-код из @Stochastically в 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
/// https://stackru.com/questions/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
}
}
}
Отображается на CoffeeScript, если кто-то заинтересован. ** редактировать -> извините, используя магистраль тоже, но вы поняли идею.
Изменено немного, чтобы принять маяк с атрибутами
{широта: item.lat, долгота: item.lng, дата: новая дата (item.effective_at), точность: item.gps_accuracy}
MIN_ACCURACY = 1
# mapped from http://stackru.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]