Фильтрация треков GPS на F#, Часть II
Самый эффективный способ разработки
Наверняка вы читали статью «Серебрянной пули нет» или, по крайней мере, слышали о ней. Фредерик Брукс постулирует, что не существует ни технологии, ни управленческого метода, которые позволят поднять производительность программистов в десять раз.
В юбилейном издании «Мифического человеко-месяца» Брукс уточняет, что один способ всё-таки есть.
Возможно, он ускорит работу не в десять раз, но всё же. Этот способ — повторное использование кода.
Наращивание программ с помощью больших блоков высокого уровня, созданных когда-то раньше или кем-то другим, помогает избежать целых уровней сложности.
Повторное использование — Святой Грааль программирования.
Мы разработаем два метода стабилизации, для которых нужно вычислять скорость автомобиля. Попробуем повысить нашу эффективность, и используем код вычисления повторно.
Сферическая теорема косинусов
Земля очень похожа на сферу. Настолько похожа, что с высокой степенью точности мы можем считать расстояние между точками на Земле равным расстоянию между точками на шаре. Вспомним, что каждая точка трека даёт нам координаты — широту и долготу, а также метку времени.
Возьмём две соседних точки. Вычтя время получения первой точки из времени второй, мы получим приращение времени $\Delta t$. Определив расстояние между точками, мы получим приращение расстояния $\Delta s$. Средняя скорость на участке от первой до второй точки будет равна $\frac{\Delta s}{\Delta t}$.
Если вычисление приращения времени проблемы не представляет, то с расстоянием всё сложнее. В сферической геометрии для этого используют формулу гаверсинусов, но почему именно её?
Давайте разберёмся.
Чтобы вычислить расстояние на сфере, можно использовать сферическую теорему косинусов. Две точки на сфере можно связать разными дугами. Значит ли это, что расстояний между точками может быть несколько? Нет. В качестве расстояния на сфере принимают длину кратчайшей дуги. Без доказательства примем теорему, которая утверждает, что кратчайшая дуга принадлежит большому кругу, то есть кругу, который проходит через две точки за сфере, и центр которого совпадает с центром сферы.
Построим на сфере треугольник с углами $A$, $B$, $C$ и сторонами $a$, $b$, $c$.
Сферическая теорема косинусов утверждает, что
\[\cos c = \cos a \cos b + \sin a \sin b \cos C\]Её доказательство вы найдёте в википедии.
Нетрудно сообразить, что такое $\cos C$, но по поводу $\cos a$ или $\cos c$ могут возникнуть вопросы. $a$ и $c$ — это не углы, а дуги, как мы можем посчитать для них косинус?
Оказывается, это нетрудно. Длина дуги $l$ с радиусом $r$, вырезанная углом $\alpha$ радиан, равна $\alpha r$. Если мы возьмём сферу с радиусом $r = 1$, длина дуги будет равна $\alpha$. Например, длина дуги, вырезанной углом $\pi/4$ в круге с радиусом один метр, будет равна $\pi/4$ метра.
Используя единичную сферу (сферу, радиус которой равен единице), мы можем упростить формулы, указывая длину стороны сферического треугольника вместо длины, делёной на радиус.
Из сферической теоремы косинусов выводится формула длины ортодромии — кратчайшего расстояния между точками на сфере. Построим треугольник из наших двух точек и северного полюса. $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 (\pi/2 - \alpha) = \sin \alpha$, получаем формулу длины ортодромии:
\[\cos c = \sin \phi_1 \sin \phi_2 + \cos \phi_1 \cos \phi_2 \cos (\lambda_2 - \lambda_1)\]Формула гаверсинусов
Эта формула хорошо работает на больших расстояниях. На расстояниях порядка километра её значением будет $\cos c$, который очень близок к 0,99999999. У чисел с плавающей точкой при вычислении арккосинуса от такого числа возникнет большая погрешность.
Для того, чтобы корректно работать с небольшими расстояниями, заменим косинусы на синусы. Для наших целей нужна специальная версия синуса, так называемый синус-версус или версинус. Её название означает обращённый синус. На рисунке вы видите зависимость между синусом, косинусом (сопряжённым синусом) и версинусом (обращённым синусом).
На самом деле нам нужен даже не сам версинус, а его половина, half of versed sine, или гаверсинус.
\[\text{hav} \alpha = \sin^2 \frac{\alpha}{2} = \frac{1 - \cos \alpha}{2}\]Запишем сферическую теорему косинусов.
\[\cos c = \cos a \cos b + \sin a \sin b \cos C\]В правой части прибавим и вычтем $\sin a \sin b$.
\[\cos c = \cos a \cos b + \sin a \sin b - \sin a \sin b + \sin a \sin b \cos C\]Вспомним формулу косинуса разности $\cos (\beta - \alpha) = \cos \alpha \cos \beta + \sin \alpha \sin \beta$. Сократим правую часть формулы и параллельно вынесем за скобки $-\sin a \sin b$.
\[\cos c = \cos (b - a) + \sin a \sin b (1 - \cos C)\]Теперь небольшое волшебство. Вычтем обе части равенства из 1 и затем разделим на 2, это позволит нам упростить формулу.
\[\frac{1 - \cos c}{2} = \frac{1 - \cos (b - a)}{2} + \sin a \sin b \frac{1 - \cos C}{2}\]Поскольку $\frac{1 - cos \alpha}{2} = \text{hav} \alpha$, мы можем переписать формулу через гаверсинус.
\[\text{hav} c = \text{hav} (b - a) + \sin a \sin b \text{hav} C\]Это и есть формула гаверсинусов. Приведём её к виду, удобному для вычисления через широту и долготу.
\[\text{hav} c = \text{hav} (\phi_2 - \phi_1) + \cos \phi_1 \cos \phi_2 \text{hav} (\lambda_2 - \lambda_1)\]Синусы в правой части превратились в косинусы, потому что широта считается от экватора, а в формуле углы строятся из северного полюса. То же самое преобразование мы делали в формуле, основанной на теореме косинусов.
Теперь, зная гаверсинус дуги, соединяющей точки, найдём её длину. Вспоминаем, что гаверсинус можно выразить через синус.
\[\text{hav} \alpha = \sin^2 \frac{\alpha}{2}\]Чтобы вычислить длину дуги в радианах, мы должны применить к результату обратные функции в обратном порядке. Возведению в квардрат соответствует извлечение квадратного корня, синусу — арксинус, а делению на 2 — умножение на 2.
\[c = 2 \arcsin \sqrt{\text{hav} (\phi_1 - \phi_2) + \cos \phi_1 \cos \phi_2 \text{hav} (\lambda_2 - \lambda_1)}\]Осталось последнее. Чтобы вычислить длину дуги в километрах, умножим длину в радианах на радиус. В нашем случае нужен радиус Земли, которая, к сожалению, не шар, а эллипсоид.
Если мы представим, что Земля это шар, точность наших вычислений составит 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 Maps.
Точность расчётов 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 километров. С точностью до целых длина одного градуса дуги на экваторе равна 40076/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 = 40076.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)
Вспомним поездку, стоимость которой составила девять тысяч рублей. «Наценка» возникла из-за точки с некорректными координатами.
Мы могли бы отбрасывать точки с очень большими расстояниями между ними, но этот способ не очень хорошо работает, поскольку не учитывает разницы между метками времени. Получив четыре некорректные точки подряд и пятую корректную, мы можем отбросить пятую только потому, что она уже слишком далеко.
Опираясь не на расстояние, а на скорость, мы такой ошибки не допустим.
Надо учитывать время между координатами. Наш способ фильтрации заключается в том, что мы вычисляем скорость между начальной и последующими точками, и выбрасываем те из них, скорость для которых оказываетя больше порогового значения.
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)
Результаты
Мы научились вычислять расстояние между двумя точками, заданными широтой и долготой, а также скорость перемещения. Мы используем вычисления скорости для стабилизации трека, выбрасывая участки, скорость на которых неправдоподобно высока или неправдоподобно мала.
Мы покрыли код тестами, благодаря чему снизили вероятность ошибок.