На главную

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

Фильтрация по скорости

Есть два метода удаления некорректных точек, основанные на измерении скорости. Первый отбрасывает точки, в которых скорость слишком большая, он называется устранением всплесков скорости (Removing Outline Speed Values).

Второй удаляет точки со слишком низкой скоростью, близкой к нулю, он называется устранением дрейфа нулевой скорости (Replacing Zero Speed Drift).

Для применения обоих методов нам необходимо вычислять скорость. Сейчас мы разберёмся, как это сделать.

Сферическая теорема косинусов

Построим на сфере треугольник с углами $A$, $B$, $C$ и сторонами $a$, $b$, $c$.

Сферическая теорема косинусов

Сферическая теорема косинусов утверждает, что

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

Из сферической теоремы косинусов выводится формула длины ортодромии — кратчайшего расстояния между точками на сфере. Построим треугольник из наших двух точек и северного полюса. $C$ будет означать разницу долгот, то есть $\lambda_2 - \lambda_1$.

Сферическая теорема косинусов, альтернативная запись

Из-за того, что широта точек считается от экватора, а стороны строятся от полюса, получаем, что $a = \pi/2 - \phi_2$ и $b = \pi/2 - \phi_1$, где $\phi_1$ и $\phi_2$ — широты точек.

Так как $\sin (\pi/2 - \alpha) = \cos \alpha$, и наоборот, получаем формулу длины ортодромии:

Формула гаверсинусов

Формула длины ортодромии, основанная на сферической теореме синусов, хорошо работает на больших расстояниях. На расстояниях порядка километра её значением будет $\cos c$, близкий к 0,99999999. У чисел с плавающей точкой при вычислении арккосинуса возникнет большая погрешность.

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

Версинус

На самом деле нам нужен даже не сам версинус, а его половина, half of versed sine, или гаверсинус.

Запишем сферическую теорему косинусов.

В правой части прибавим и вычтем $\sin a \sin b$.

Вспоминаем формулу косинуса разности $\cos (\beta - \alpha) = \cos \alpha \cos \beta + \sin \alpha \sin \beta$. Сокращаем правую часть формулы и параллельно выносим за скобки $-\sin a \sin b$.

Теперь сделаем небольшое волшебство. Вычтем обе части равенства из 1 и затем разделим на 2, это позволит нам упростить формулу.

Поскольку $\frac{1 - cos \alpha}{2} = \text{hav} \alpha$, мы можем переписать формулу через гаверсинус.

Это и есть формула гаверсинусов. Приведём её к виду, удобному для вычисления через широту и долготу.

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

Теперь, зная гаверсинус дуги, соединяющей точки, найдём её длину. Вспоминаем, что гаверсинус можно выразить через синус.

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

Осталось последнее. Чтобы вычислить длину дуги в километрах, умножим длину в радианах на радиус. В нашем случае нужен радиус Земли, которая, к сожалению, не шар, а эллипсоид.

Если мы представим, что Земля это шар, точность наших вычислений составит 0,5%, что для расстояния в метр составляет полсантиметра. Такой точности нам достаточно. Что взять в качестве радиуса Земли. Например, среднее арифметическое между радиусом на экваторе на экваторе (6378,137км) и на полюсе (6356,752км).

let distance latitude1 longitude1 latitude2 longitude2 =
    let radian x = x * Math.PI/180.0

    let hav x = sin(x/2.0) ** 2.0
    
    let earthEquatorialRadius = 6378.137
    let earthPolarRadius = 6356.752
    let averageEarthRadius = (earthEquatorialRadius + earthPolarRadius)/2.0

    let φ1 = radian latitude1
    let λ1 = radian longitude1

    let φ2 = radian latitude2
    let λ2 = radian longitude2

    let h = hav (φ2 - φ1) + cos φ1 * cos φ2 * hav (λ2 - λ1)

    2.0 * asin (sqrt h) * averageEarthRadius

По традиции, координаты мы получаем в градусах, а в формуле применяем радианы, поэтому нам потребуется функция для перевода градусов в радианы.

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

Давайте убедимся, что он работает. Напишем тесты.

Расстояние от Москвы до Санкт-Петербурга

Расстояние от Москвы до Санкт-Петербурга составляет 635км, или, точнее, 634,37км. От центра до центра. Координаты центров (55,753960; 37,620393) (Москва) и (59,9386300; 30,3141300) (Санкт-Петербург). Данные получены из открытых источников, то есть из Google.

Точность расчётов 0,5% или 0,005. Это означает, что расстояние, вычисленное нами по формуле должно отличаться от 634,37 не больше, чем на 634,37 × 0,005.

Запишем эту информацию в виде теста:

[<Fact>]
let ``distance - between Moscow and Saint Petersburg - approximately equals 630km`` () =
   let MoscowLatitude = 55.753960
   let MoscowLongitude = 37.620393
   let SaintPetersburgLatitude = 59.9386300
   let SaintPetersburtLongitude = 30.3141300

   let actual = distance MoscowLatitude MoscowLongitude SaintPetersburgLatitude SaintPetersburtLongitude

   let expected = 634.37
   let epsilon = 0.005
   Assert.True(abs(actual - expected) < expected * epsilon)

Длина одного градуса на экваторе

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

Единица метр равна одной десятимиллионой длины Парижского меридиана от Северного полюса до экватора. Длина целого меридиана составляет ровно 40000 километров.

Из-за приплюснутой формы Земли длина экватора чуть больше, около 40076 километров. С точностью до целых длина одного градуса дуги на экваторе равна 40000/360 или 111 километров.

[<Fact>]
let ``distance - signle grade at equator - approximately eqauls 111km`` () =
    let startLatitude = 0.0
    let startLongitude = 0.0
    let endLatitude = 0.0
    let endLongitude = 1.0

    let actual = distance startLatitude startLongitude endLatitude endLongitude

    let expected = 40000.0/360.0
    Assert.Equal(expected, actual, 0)

В утверждении Assert.Equal последний параметр, а именно 0, задаёт количество знаков после запятой, для которых необходимо проверять равенство. 0 означает, что дробные части просто отбрасываются, и на самом деле мы хотим убедиться, что целая часть равна 111.

Вычисление скорости

Зная, как вычислить расстояние, мы можем вычислить скорость.

let velocity (p1: SensorItem) (p2: SensorItem) =
    let Δtime = (p2.Timestamp - p1.Timestamp).TotalHours
    let Δdistance = distance p1.Latitude p1.Longitude p2.Latitude p2.Longitude

    Δdistance/Δtime

Функция получилась совсем простой. Единственное, что нам пришлось сделать — указать тип параметров. F# выводит типы, но не всегда может это сделать.

Тест для проверки также окажется простым. Мы воспользуемся рассуждениями из нашего второго теста для функции distance. Если временные метки отстоят друг от друга на час, а координаты обрузуют на экваторе дугу в один градус, то скорость движения составит 111 км/час.

[<Fact>]
let ``velocity at single grade per hour at equator approximately equals 111km per hour`` () =
    let startSensorItem = new SensorItem(0.0, 0.0, 0.0, 0.0, new DateTimeOffset(2019, 4, 26, 11, 00, 00, TimeSpan.Zero))
    let endSensorItem = new SensorItem(0.0, 1.0, 0.0, 0.0, new DateTimeOffset(2019, 4, 26, 12, 00, 00, TimeSpan.Zero))

    let actual = velocity startSensorItem endSensorItem

    let expected = 111.0
    Assert.Equal(expected, actual, 0)

Устранение всплесков скорости (Removing Outline Speed Values)

Мы собирали реальные данные по трекам. В одном случае стоимость поездки составила почти девять тысяч рублей вместо ожидаемых трёхсот. Оказалось, что автомобиль заехал в туннель, а по выезде восстанавливал поключение к спутникам GPS. Первая координата, полученная после восстановления, оказалась из Санкт-Петербурга, а не из Москвы.

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

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

let removeOutlineSpeedValues hiLimit points =
    let isOutlineSpeed p1 p2 =
        let velocity = velocity p1 p2

        velocity > hiLimit

    let rec filter p1 points =
        match points with
        | p2::tail -> if isOutlineSpeed p1 p2
                        then filter p1 tail
                        else p2::filter p2 tail
        | _ -> points

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

Для увеличения читаемости я добавил внутреннюю функцию isOutlineSpeed. Функция filter, как и в случае с removeZeroOrNegativeTimespans, реализует особую фильтрацию, благодаря которой мы можем отбрасывать несколько неправильных точек, идущих подряд, а не только первую из них.

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

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

let oneDegreeOfMeridianInKm = 111.32
    

[<Fact>]
let ``removeOutlineSpeedValues with outline speed removes point`` () =
    let source = [SensorItem(45.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(46.1, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T17:38:14+03:00"));
                  SensorItem(47.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T18:38:16+03:00"))]

    let actual = removeOutlineSpeedValues oneDegreeOfMeridianInKm source

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


[<Fact>]
let ``removeOutlineSpeedValues without outline speed returns same list`` () =
    let source = [SensorItem(45.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(46.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T17:38:14+03:00"));
                  SensorItem(47.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T18:38:16+03:00"))]

    let actual = removeOutlineSpeedValues oneDegreeOfMeridianInKm source

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

Замена дрейфа нулевой скорости нулём (Replacing Zero Speed Drift)

Нулевая скорость должна быть нулевой, поэтому, пока автомобиль стоит на одном месте, датчик GPS должен возвращать одни и те же координаты. Логично?

Не совсем. Из-за погрешностей измерения, датчик GPS может давать близкие, но разные координаты, даже если автомобиль остановился. Скорость этих микроперемещений невысока и составляет 100-200 метров в час. Небольшое, но постоянное её изменение называется дрейфом нулевой скорости.

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

Впрочем, эти точки можно просто отбрасывать, осуществляя неявное прореживание данных. Мы будем делать именно так, поэтому наш метод будет называться remove, а не replace.

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

Из-за этих важных дателей, функция filter будет отличаться от предыдущих вариантов.

let removeZeroSpeedDrift loLimit points =
    let isZeroDriftSpeed p1 p2 =
        let velocity = velocity p1 p2

        velocity < loLimit

    let rec filter p1 points =
        match points with
        | [] -> [p1]
        | [p2] -> if isZeroDriftSpeed p1 p2
                  then [p2]
                  else p1::[p2]
        | p2::tail -> if isZeroDriftSpeed p1 p2
                        then filter p2 tail
                        else p1::filter p2 tail

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

Если в треке три точки с дрейфом, функция должна удалить вторую.

[<Fact>]
let ``removeZeroSpeedDrift with very small drift removes drift points`` () =
    let source = [SensorItem(45.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(46.95, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T17:38:14+03:00"));
                  SensorItem(47.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T18:38:16+03:00"))]

    let actual = removeZeroSpeedDrift (oneDegreeOfMeridianInKm/10.0) source

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

Если точек всего две, вторую удалять не нужно.

[<Fact>]
let ``removeZeroSpeedDrift with very small drift keeps two end points`` () =
    let source = [SensorItem(45.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(45.05, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T17:38:14+03:00"))]

    let actual = removeZeroSpeedDrift (oneDegreeOfMeridianInKm/10.0) source

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

Наконец, если дрейфа нет совсем, список должен остаться неизменным.

[<Fact>]
let ``removeZeroSpeedDrift without drift returns same list`` () =
    let source = [SensorItem(45.00, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(45.15, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T17:38:14+03:00"));
                  SensorItem(47.00, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T18:38:16+03:00"))]

    let actual = removeZeroSpeedDrift (oneDegreeOfMeridianInKm/10.0) source

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