Фильтрация треков GPS на F#, Часть I

Проблематика

Однажды я работал над проектом, где по маршруту автомобиля нужно было посчитать стоимость поездки. Маршрут строился на основании данных GPS.

Средняя поездка по Москве стоила около четырёхсот рублей, но однажды стоимость подскочила до девяти тысяч.Машина въехала в туннель, а по выезде попыталась восстановить связь со спутниками. Первой полученной ей координатой оказался центр Санкт-Петербурга. Через секунду координаты снова «вернулись» в Москву, но лишние 1300 километров увеличили стоимость поездки почти в двадцать пять раз.

Тот самый злополучный скачок

На рисунке вы видите фрагмент этого маршрута. Я увеличил масштаб, чтобы стали заметны детали. Зигзаг за пределы рисунка — и есть тот самый злополучный скачок.

Приглядевшись к маршруту, вы заметите, что и в других точках трек сопровождается неровностями, которые приводят к неточному вычислению цены.

Чтобы справиться с такими скачками, программисты применяют алгоритмы фильтрации и сглаживания. Мы обсудим их в этой статье, а также реализуем на языке программирования F#.

Данные GPS

Мобильные телефоны оснащены датчиками (сенсорами), которые позволяют определять положение, ориентацию и ускорение устройства. Нам для вычислений понадобятся:

Широта и долгота (latitude и longitude) — координаты устройства. Измеряются в градусах.
Скорость (speed). Измеряется в метрах в секунду.
Направление (heading) — направление движения относительно севера. Измеряется в градусах, положительное направление по часовой стрелке.
Метка времени (timestamp) — дата и время получения координаты.

Опишем класс SensorItem, в котором будем хранить эти параметры:

Types.fs

module Types

open System


type SensorItem(latitude: float, longitude: float, speed: float, heading: float, timestamp: DateTimeOffset) =
    member __.Latitude = latitude
    member __.Longitude = longitude
    member __.Timestamp = timestamp
    member __.Speed = speed
    member __.Heading = heading

    override __.GetHashCode() =
        hash (latitude, longitude, timestamp, speed, heading)

    override __.Equals(other) =
        match other with
        | :? SensorItem as x -> (latitude, longitude, timestamp, speed, heading) = (x.Latitude, x.Longitude, x.Timestamp, x.Speed, x.Heading)
        | _ -> false

Для типов проекта создадим модуль Types.fs. Строка module Types позволит в дальнейшем использовать объявленные типы в других модулях. F# не требует, чтобы имя файла и имя модуля совпадали, но мы будем придерживаться этого соглашения.

Строка open System позволяет обращаться к типам, описанным в пространстве имён System. Она нужна, поскольку мы используем DateTimeOffset, для которого в F# нет «родного» названия, такого как float. Интересно, что float в F# соответствует типу double в C#. Вас это удивляет? Вы не одиноки. Загадка объясняется просто — F# сделан из языка программирования OCaml, где float означает число с плавающей точкой двойной точности.

Числа двойной точности занимают восемь байт. Старые добрые четырёхбайтовые числа в F# называются float32.

Пять свойств класса SensorItem инициализируются в конструкторе и больше не меняются. В объектно-ориентированном программировании говорят, что такие классы реализуют паттерн объект-значение (Value Object). Паттерн Value Object описан Мартином Фаулеров в книге Архитектура копроративных программных приложений (часть II, глава 18 стр. 500) и в книге Эрика Эванса Предметно-ориентированное проектирование (глава 5, стр. 101).

Вам не кажется, что объекты-значения противоречат объекто-ориентированному программированию? Мы прячем состояние объекта, чтобы избежать несогласованного изменения этого состояния, и называем это инкапсуляцией. Но если состояние объекта нельзя изменить, какая от него может быть польза?

На этот вопрос отвечает десятая глава книги Эрика Эванса. Мотивацию для применения объектов-значений вы найдёте в разделе Функции без побочных эффектов (глава 10, стр. 226). Вкратце, такие объекты позволяют изолировать сложные вычисления и аккуратно покрыть их тестами. Эванс подробно разбирает задачу смешения красок и демонстрирует преимуществка объектов без изменяемого состояния.

У функциональных программистов проблем с неизменяемыми данными вообще не бывает. Они называют их иммутабельными.

Слово item в английском языке многозначно. Под sensor item мы понимаем показание датчика.

В функциях тестирования нам предстоит сравнивать друг с другом коллекции показаний, а, следовательно, и сами показания. Если вы писали под .NET, то знаете, что по умолчанию метод Equals считает равными объекты с одним и тем же адресом в памяти. Объекты SensorItem должны быть равны, если равны все их свойства.

Чтобы реализовать эту концепцию равенства, мы переопределим метод Equals. И снова, если вы пишете на .NET, то знаете, что переопределять Equals без GetHashCode неправильно. Подобное соглашение есть и в Java. F# здесь не оригинален.

Реализация GetHashCode максимально проста, мы создаём кортеж из наших переменных, записав их в скобках, и просим F# вычислить его хэш, вызвав функцию hash.

Внутри метода Equals встречается конструкция :? SensorItem as x. Означает она следующее: если параметр other имеет тип SensorItem, назовём его x и используем в правой части выражения после стрелки. | _ -> false означает, что во всех остальных случаях метод вернёт false, то есть переменные не будут считаться равными.

Для тех, кто хорошо знает C# я привожу тот же самый код на C#.

namespace Types
{
    using System;

    public class SensorItem
    {
        public double Latitude { get; }

        public double Longitude { get; }

        public double Speed { get; }

        public double Heading { get; }

        public DateTimeOffset Timestamp { get; }

        public SensorItem(double latitude, double longitude, double Speed, double Heading, DateTimeOffset timestamp)
        {
            Latitude = latitude;
            Longitude = longitude;
            Speed = speed;
            Heading = heading;
            Timestamp = timestamp;
        }

        public override int GetHashCode()
        {
            return (Latitude, Longitude, Speed, Heading, Timestamp).GetHashCode();
        }

        public override bool Equals(object other)
        {
            if (other is SensorItem otherItem)
            {
                return Latitude == otherItem.Latitude
                    && Longitude == otherItem.Longitude
                    && Speed == otherItem.Speed
                    && Heading == otherItem.Heading
                    && Timestamp == otherItem.Timestamp;
            }

            return false;
        }
    }
}

C# чуть более многословен, но большой разницы между кодом мы не видим. Можно сказать, что отступы в F# благотворно влияют на рамзер программы, потому что избавляют нас от фигурных скобок.

Этапы обработки трека

Как вы думаете, проста ли жизнь программиста? Не знаете? Переформулирую вопрос: сколько методов очистки трека нам придётся реализовать, чтобы получить хороший результат?

Когда то я думал, что этот метод один, и он называется фильтр Калмана. Оказалось, нет. Фильтр Калмана очень капризен, и не очень чистые данные могут его сломать.

Поэтому перед фильтрацией приходится стабилизировать трек, то есть убирать неправдоподобные данные или заменить их на правдоподобные. В статье GPS Data Filtration Method for Drive Cycle Analysis Applications вы найдёте описание способов стабилизации.

После стабилизации трек сглаживают. Для сглаживания мы используем фильтр Калмана.

Наконец, трек прореживают, то есть убирают из него точки, которые не влияют на результат. Например, если несколько точек лежат на одном отрезке, оставить можно только первую и последнюю — это не изменит длительность и продолжительность поездки.

Мы не станем подробно останавливаться на прореживании в этой статье.

К стабилизации относятся следующие методы обработки трека:

Удаление нулевых и отрицательных интервалов времени.
Устранение всплесков скорости.
Замена дрейфа нулевой скорости нулём.

Удаление нулевых и отрицательных интервалов времени (Removing Data with Duplicate/Negative Time Records)

Для того, чтобы вычислить координаты, датчик GPS опирается на время, передаваемое со спутников. Мы не станем углубляться в теорию, просто отметим, что вместе с каждыми координатами мы получаем метку времени (timestamp).

Накладки всё-таки возможны, и старая координата устройства может появиться в треке позже новой. Иногда две соседние координаты имеют одну и ту же метку.

В первом случае интервал времени между соседними точками отрицателен, во втором — равен нулю. В обоих случаях нам надо удалить координаты с отрицательными или нулевыми интервалами.

Для начала напишем заготовку функции, которая будет отбрасывать показания с временной меткой из прошлого.

Filters.fs

module Filters

open System
open Types


/// <summary>
/// Removes points with zero or negative time spans.
/// </summary>
let removeZeroOrNegativeTimespans points =
    points

В нашей первой версии функция removeZeroOrNegativeTimespans с параметром points просто возвращает этот параметр.

Я хочу написать модульные тексты для функции removeZeroOrNegativeTimespans. Visual Studio 2017 позволяет писать тесты для F# с помощью разных фреймворков. Мы будем использовать XUnit.

Для начала убедимся, что функция, получая пустой список, также возвращает пустой список.

FilterTests.fs

open System
open Xunit
open Types
open Filters


[<Fact>]
let ``removeZeroOrNegativeTimespans - with empty points - returns empty points`` () =
    let source = []

    let actual = removeZeroOrNegativeTimespans source

    Assert.Empty(actual)

Двойные обратные апострофы, то есть знаки `` позволяют использовать в названии функций произвольные символы, даже пробелы. Опираясь на рекомендацию Роя Оушерова (Искусство автономного тестирования, стр. 58–59) я назвал тестовую функцию в соответствии с шаблоном что тестируем — сценарий — ожидаемое поведение. Тестируем функцию removeZeroOrNegativeTimespans, сценарий — with empty points, то есть «с пустыми списком точек», ожидаемое поведение — returns empty points, то есть функция «возвращает пустой список точек».

Атрибут [<Fact>] подсказывает фреймворку XUnit, что функция с именем `removeZeroOrNegativeTimespans - with empty points - returns empty list` должна быть вызвана при запуске тестов. Утверждение Assert.Empty позволяет проверить, что список, возвращённый функцией removeZeroOrNegativeTimespans, пуст.

Подобным образом напишем функцию, которая вернёт список из одного элемента, получив такой же список в параметре.

[<Fact>]
let ``removeZeroOrNegativeTimespans - with single point - returns single point`` () =
    let source = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:00+03:00"))]

    let actual = removeZeroOrNegativeTimespans source

    let expected = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:00+03:00"))]
    Assert.Equal<seq<SensorItem>>(expected, actual)

Утверждение Assert.Equal<seq<SensorItem>> позволяет сравнить две последовательности элементов. К последовательностям в F# относятся перебираемые объекты, то есть объекты тех классов, которые реализуют интерфейсы IEnumerable и IEnumerable<T>. Список F# также перебираемый объект.

Первые два теста должны запускаться без ошибок, не смотря на то, что код нашей функции ничего не делает. Теперь напишем тест, который «сломает» наивную реализацию. Передадим в качестве параметра список из трёх элементов, в котором дата и время второго элемента будут совпадать с датой и временем первого.

[<Fact>]
let ``removeZeroOrNegativeTimespans - with zero timespan - removes point`` () =
    let source = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                  SensorItem(1.0, 1.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                  SensorItem(2.0, 2.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:16+03:00"))]

    let actual = removeZeroOrNegativeTimespans source

    let expected = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                    SensorItem(2.0, 2.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:16+03:00"))]
    Assert.Equal<seq<SensorItem>>(expected, actual)

Мы ожидаем, что функция removeZeroOrNegativeTimespans удалит из списка показание с нулевым интервалом, то есть вторую точку.

Этот тест у нас не пройдёт. Самое время написать корректную реализацию фильтра.

let removeZeroOrNegativeTimespans points =
    match points with
    | [] -> []
    | [p] -> [p]
    | p1::_ -> let tail = points
                       |> List.pairwise
                       |> List.filter (fun (p1: SensorItem, p2) -> p2.Timestamp - p1.Timestamp > TimeSpan.Zero)
                       |> List.map (fun (_, p) -> p)

               p1::tail

Мы воспользовались сопоставлением с образцом (pattern matching). Конструкция match points with похожа на switch в таких языках, как C/C++/Java/C#.

В случае, если мы получем пустой массив (образец []), функция возвращает пустой список. Если список содержит один элемент (образец [p]), мы снова возвращаем тот же самый список.

В последнем случае, когда список состоит более чем из одного элемента, мы берём точки списка попарно (функция List.pairwise), и оставляем только те пары, в которые дата и время второй точки больше, чем первой (функция List.filter). Наконец, мы выбрасываем первую точку из каждой пары. У нас остаётся список точек, начиная со второй, с правильной меткой времени.

Операция |> называется композицией функций. x |> f |> g |> h означает в классической записи h(g(f(x))).

На C# эта функция могла бы выглядеть так:

public IEnumerable<SensorItem> RemoveZeroOrNegativeTimespans(IEnumerable<SensorItem> points)
{
    switch (points.Count())
    {
        case 0:
        case 1:
            return points;

        default:
            var tail = points.Pairwise()
                             .Where(x => x.Item2.Timestamp - x.Item1.Timestamp > TimeSpan.Zero)
                             .Select(x => x.Item2);

            return points.Take(1)
                         .Union(tail);
    }
}

К сожалению, в классе EnumerableExtensions, который содержит стандартные расширения для итераторов IEnumerable<T> нет Pairwise. Мы можем написать этот метод самостоятельно, подсмотрев в решение на Stack Overflow.

public static IEnumerable<T> Pairwise<T>(this IEnumerable<T> source)
{
    TSource previous = default(TSource);

    using (var enumerator = source.GetEnumerator())
    {
        if (enumerator.MoveNext())
            previous = enumerator.Current;

        while (enumerator.MoveNext())
        {
            yield return (previous, enumerator.Current);

            previous = enumerator.Current;
        }
    }
}

Мы видим, что современный C# с LINQ позволяет писать очень функциональный код. Истинно императивное решение было бы таким.

public IReadOnlyList<SensorItem> RemoveZeroOrNegativeTimespans(IReadOnlyList<SensorItem> points)
{
    if (points.Count < 2)
        return points;

    var result = new List<SensorItem> { points[0] };

    for (int i = 1; i < points.Length; i++)
    {
        if (points[i].Timespan - points[i - 1].Timespan > TimeSpan.Zero)
            result.Add(points[i]);
    }

    return result;
}

Как видим, функциональный код нельзя назвать более сложным, хотя императивный привычнее для императивных программистов.

Особенность кода на F#, которая не бросилась вам в глаза, если вы не функциональный программист, это каррирование. Функции List.filter и List.map получают по два параметра. Первый мы записываем при вызове функции, но на самом деле мы не вызываем функцию. Мы создаём новую, с фиксированным первым параметром, которая всё ещё ожидает второго, чтобы выполниться.

В результате наш код читается так: 1) берём список точек (points); 2) составляем из них пары (List.pairwise); 3) отбрасываем те, которые не удовлетворяют условию; и 4) из каждой пары оставляем только второй элемент.

Запустим тесты. Сейчас проходят все три, значит, мы написали корректную функцию.

Напишем последний, четвёртый тест, который будет проверять, что функция не удаляет лишних элементов.

[<Fact>]
let ``removeZeroOrNegativeTimespans - with positime timespans - returns same list`` () =
    let source = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(1.0, 1.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                  SensorItem(2.0, 2.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:16+03:00"))]

    let actual = removeZeroOrNegativeTimespans source

    let expected = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                    SensorItem(1.0, 1.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                    SensorItem(2.0, 2.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:16+03:00"))]
    Assert.Equal<seq<SensorItem>>(expected, actual)

Возможно, вы читали, что тексты должны быть не только позитивным (функция делает то, что нужно), но и негативными (функция не делает того, чего не нужно). Наш четвёртый тест показывает, что функция не удаляет точки, если метки времени расположены в корректном возрастающем порядке.

Внезапный подводный камень

Предложенный алгоритм неплохо работал до апреля 2019 года. В один прекрасный день он стал возвращать гигантские отрицательные результаты. Заглянув в сырые данные мы обнаружили странные даты.

№	Широта	Долгота	Метка времени
1	55,6700525	37,4681227	2019-04-17 11:07:26
2	55,6700947601348	37,4682662263513	1999-09-01 11:07:32
3	55,6700855400413	37,46821526438	1999-09-01 11:07:42
4	55,6699361	37,4682669	2019-04-17 11:07:41
…	…	…	…
721	55,7126607466489	37,4804087541997	1999-09-01 11:55:19
722	55,7126877782866	37,4804472271353	1999-09-01 11:55:21

Видите, что происходит? Время неожиданно уходит из 2017-го года в 1999-й и мы получаем несколько точек подряд из прошлого. Метка времени у этих точек продолжает возрастать, поэтому наш алгоритм выбрасывает вторую точку, но оставляет третью.

Причина произошедшенго оказалась нетривиальной. Нам «повезло», мы наткнулись на смену эпох, заложенную в стандарте GPS. Началом времён он считает полночь с 5 на 6 января 1980 года. Дата и время GPS хранятся в двух целых числах: сначала порядковый номер недели, на который отведено 10 бит, затем количество секунд, прошедших с начала недели.

Нетрудно заметить, что счётчик недель переполняется каждые 7168 дней (7×1024), или один раз в 19,62 лет. Апрель 2019-го — время очередного переполнения. Устройства, сделанные не совсем по стандарту, могут отбрасывать дату в прошлое.

Чтобы исправить ситуацию, я применил два способа. Один из них основан на специальной обработке рискованных дат, и не очень интересен. Второй касается нашей функции. Её необходимо переписать так, чтобы она удаляла все метки из прошлого.

Для начала напишем тест для проверки новой ошибки.

[<Fact>]
let ``removeZeroOrNegativeTimespans - with two negative timespans - removes both points`` () =
    let source = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                  SensorItem(1.0, 1.0, 0.0, 0.0, DateTimeOffset.Parse("2018-11-07T16:38:16+03:00"));
                  SensorItem(2.0, 2.0, 0.0, 0.0, DateTimeOffset.Parse("2018-11-07T16:38:17+03:00"));
                  SensorItem(3.0, 3.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:18+03:00"))]

    let actual = removeZeroOrNegativeTimespans source

    let expected = [SensorItem(0.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                    SensorItem(3.0, 3.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:18+03:00"))]
    Assert.Equal<seq<SensorItem>>(expected, actual)

Мы добавили в список две точки из прошлого подряд, и ожидаем, что наш методы удалит обе.

Мы уже не можем воспользоваться встроенной функцией List.pairwise, нам нужно написать фильтрацию самостоятельно.

let removeZeroOrNegativeTimespans points =
    let rec filter (p1: SensorItem) points =
        match points with
        | (p2: SensorItem)::tail -> if p2.Timestamp - p1.Timestamp > TimeSpan.Zero
                                    then p2::filter p2 tail
                                    else filter p1 tail
        | _ -> points

    match points with
    | p1::tail -> p1::filter p1 tail
    | _ -> points

В этом коде мы написали внутреннюю рекурсивную функцию filter. Мы постоянно передаём начальную точку для сравнения. Если вторая точка удовлетворяет фильтру, мы используем её вместо начальной, а если нет, продолжаем использовать начальную.

После такой модификации кода у нас проходит и новый тест, и четыре предыдущих. Это означает, что мы не внесли ошибок.