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

Фильтр Калмана

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

Чтобы не запутаться, будем рассуждать последовательно.

Модель

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

Чтобы его выровнять, нам нужно что-то, что позволит исправлять ошибки датчика. В методе Калмана это что-то — математическая модель.

Рассмотрим утрированно простой вариант движения автомобиля — представим, что он передвигается в одномерном пространстве.

Автомобиль едет по прямой дороге. Время от времени мы определяем его положение и скорость. На шаге $i$ координата автомобиля равна $x_i$, а скорость равна $u_i$. На шаге $i + 1$, через время $\Delta t$, новая координата будет равна $x_{i+1}$.

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

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

Индекс $i$ у $\xi$ показывает, что погрешность в каждой точке разная. Действительно ли она случайна? Мы не знаем. Наверное мы могли бы сделать модель сложнее, и вычислять погрешность с большей точностью, но у нас нет задачи строить сложные модели.

На практике не важно, действительно ли погрешность случайна или она только похожа на случайную — мы в любом случае можем применить к ней математический аппарат.

Вспоминая теорию вероятностей

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

Взгляните на иллюстрацию. Я свёл две случайные величины, которые объединяет одна характеристика. Вы видите, что значения во всех испытаниях сосредоточены вокруг единого центра. Вычислив среднее арифметическое всех величин, узнаем значение, которое ожидаем обычно. Оно так и называется — математическое ожидание — и обозначается буквой $E$ — первой в слове Expectation.

На иллюстрации математическое ожидание $E \xi$ у обеих величины равно 2.

Вторая характеристика случайной величины — разброс. В левой половине графика значения находятся чуть дальше от центра, а в правой — чуть ближе. Разброс показывает, как далеко случайная величина $\xi$ может отстоять от центра $E \xi$. Проблема в том, что разница $\xi - E \xi$ бывает как отрицательной, так и положительной. Чтобы всегда получать положительные значения, возведём её в квадрат $(\xi - E \xi)^2$.

Однако, в выборке много случайных величин. Какую из них взять, чтобы оценить разброс? На рисунке мы видим, что максимальная разница для двух выборок равна 1, в то время как «на глазок» левая выборка шире правой.

Чтобы точнее оценивать разброс, вместо максимальной разницы надо брать среднюю, то есть $E(\xi - E \xi)^2$.

Эта мера разброса называется дисперсией.

Наша погрешность измеряется в метрах, а дисперсия — в метрах-в-квадрате. Это не всегда удобно, поэтому помимо дисперсии в теории вероятностей оперируют стандартным отклонением.

Стандартное отклонение в левой выборке равно 1, а в правой — 0,6. В отличие от отдельных случайных величин, математическое ожидание, дисперсия и стандартное отклонение вполне предсказуемы. Если мы возьмём сто случайных чисел, посчитаем среднее, а потом возьмём ещё сто случайных чисел, и снова посчитаем среднее, эти два средних будут равны друг другу с высокой степенью точности.

Но вернёмся к погрешности модели. Чему может быть равно её математическое ожидание? Модель иногда даёт перекос в большую сторону, иногда в меньшую, но в среднем точно оценивает пройденное расстояние. Это значит, что среднее арифметическое погрешности должно быть равно нулю, $E \xi = 0$.

Если это так, то дисперсия погрешности равна

Датчик

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

$x^S_i$ означает не возвещение в степень $S$, а величину $x_i$ помеченую буквой $S$, первой буквой слова Sensor. Так мы обозначим координату, полученную с датчика.

Погрешность датчика может быть и положительной, и отрицательной. В откалиброванном датчике математическое ожидание погрешности будет равно нулю, $E \eta = 0$, а дисперсия

Рекурсивный метод

Фильтр Калмана работает рекурсивно. Предположим, что нам известна координата $x^A_i$, вычисленная на предыдущем шаге алгоритма, и мы хотим вычислить следующую координату $x^A_{i+1}$. Буква $A$ — первая в слове Appoximation, то есть приближение.

Нам известны скорость $u^S_i$, которую датчик GPS определил на предыдущем шаге, и текущая координата, полученная с датчика сейчас $x^S_{i+1}$.

Новое приближение будет находиться где-то между координатой датчика $x^S_{i+1}$ и координатой, вычисленной по модели, т. е. $x^A_i + u^S_i \Delta t$.

Чтобы найти это между, воспользуемся простой линейной интерполяцией.

Коэффициент $K$ определяет относительные «веса» датчика и модели. При $K = 1$ координата определяется только датчиком, а при $K = 0$ — только моделью.

Ошибкой предсказания назовём разницу между реальной координатой $x_{i+1}$ и предсказанной $x^A_{i+1}$.

Так как $x^A_{i+1} = K x^S_{i+1} + (1 - K)(x^A_i + u^S_i \Delta t)$,

Поскольку $x^S_i = x_i + \eta_i$ (и $x^S_{i+1} = x_{i+1} + \eta_{i+1}$), то

Вспоминаем, что $x_{i+1} = x_i + u^S_i \Delta t + \xi_i$, тогда

Вынесем $x_i + u^S_i \Delta t + \xi_i$ за скобки, получим

Видим, что множитель $(1 - K)$ дважды встречается в выражении, и, следовательно, его можно вынести. При этом у нас сократится член $u^S_i \Delta t$

Зная, что $e_{i+1} = x_{i+1} - x^A_{i+1}$, то есть $e_i = x_i - x^A_i$, сокращаем запись во вторых скобках

Магия

Мы хотим минимизировать ошибку $e_{i+1}$. Как это сделать? Первый ответ, который приходит в голову — никак. Ошибка — случайная величина, значения которой мы не знаем.

Тупик.

Математики, оказавшись в тупике, смотрят на задачу с непривычных точек зрения. Да, мы не знаем, чему равна $e_{i+1}$, но предположим, что мы много раз поставили эксперимент с автомобилем. Тогда мы сможем посчитать математическое ожидание ошибки $E e_{i+1}$ — оно постоянно, и его можно минимизировать.

Звучит интересно. Попробуем заменить все случайные величины их ожиданиями

Как минимизировать такую ошибку? Непонятно. Идея Калмана кажется совершенно парадоксальной — мы должны вычислять не ожидание ошибки, а квадрат ожидания $E e^2_{i+1}$.

Не похоже на простое решение, не так ли?

Проверка

Раскрываем скобки и ужасаемся

Мы вспоминаем, что наш датчик хорошо откалиброван, и, следовательно, в среднем даёт ошибку ноль, то есть $E \eta_{i+1} = 0$. Следовательно, всё произведение, куда входит этот множитель, тоже равно нулю

Возводим $E e_i + E \xi_i$ в квадрат

Модель в среднем также даёт ошибку ноль, $E \xi_i = 0$

Чему равно значение $E \xi^2_i$? Кажется странным, что в каждой точке трека модель имеет разную погрешность. Действительно, ожидания погрешности должны быть равны во всех точках $E \xi^2_i = E \xi^2$. Провернём небольшой трюк, чуть усложнив формулу и запишем её как $E \xi^2 = E(\xi - 0)^2$.

Вспоминаем, что $E \xi = 0$. Подставляем ожидание вместо нуля, получаем $E(\xi - E \xi)^2$. Это формула расчёта дисперсии $\sigma^2_{\xi}$.

Те же соображения касаются $E \eta^2_{i+1}$. Следовательно, формула целиком приобретает вид

Последнее чудо

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

Чтобы немного упростить формулы, я введу переменные $a$ и $b$

и

Наше выражение примет вид

Раскроем скобки

И сгруппируем члены выражения относильно $K$

Чем отличается $E e^2_{i+1}$ от $E e_{i+1}$? Тем, что имеет квадратичную зависимость от $K$, и, значит параболический график.

«Рога» параболы направлены вверх, если коэффициент при $K^2$ больше $0$. Коэффициент $a + b$ раскрывается в \(E e^2_i + \sigma^2\_{\xi} + \sigma^2_{\eta}\), то есть в сумму трёх вадратов. Он может быть равен $0$ только в идеальном мире, где модель и датчик дают результаты вообще без погрешности.

В реальном мире его значение всегда больше $0$, следовательно, парабола направлена вверх и имеет минимум. Это значит, что, выбрав правильное значение $K$, мы можем минимизировать квадрат ожидания ошибки.

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

или

откуда

Подставим $K$ в формулу $E e^2_{i+1} = (a + b) K^2 - 2a K + a$, получим

Приведём все слагаемые к единому знаменателю $(a + b)$

Раскроем скобки и сократим $a^2$

От математики к программе

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

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

Для работы алгоритма нужен квадрат ожидания на предыдущем шаге \(E^2_i\), дисперсия погрешности модели \(\sigma^2_{\xi}\) и дисперсия погрешности датчика \(\sigma^2_{\eta}\).

Где найти все эти значения? Квадрат ожидания ошибки на первом шаге $E^2_1$ может быть любым, потому что фильтр Калмана быстро приведёт его к минимуму, но мы можем взять любое разумное значение, например, разброс погрешности датчика, то есть дисперсию его погрешности.

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

Нам придётся подбирать дисперсии вручную. К счастью, я уже сделал это для нас.

Зная $E e^2_i$ мы можем вычислить коэффицент Калмана $K$

А, зная $K$ и показания датчика в точке $i + 1$ мы можем вычислить приближение

Приближение вычисляем также рекурсивно. Это значит, что для начала рассчётов нам нужно приближение в самое первой точке $x^A_1$. Разумным значением будет самая первая координата, полученная с датчика.

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

Двумерная плоскость

Самое время вспомнить, что автомобиль едет по поверхности Земли, и мы должны учитывать две координаты. Это нетрудно сделать.

Вместо скалярной скорости $u^s_i$ используем проекции вектора скорости на оси ${u^s_x}_i$ и ${u^s_y}_i$. О том, как их получить, мы поговорим ниже.

Программирование

На каждой итерации мы получем предыдущие вычисленные координаты и квадрат ожидания ошибки. Кроме того, для вычислений нам нужны дисперсия погрешности модели ($\sigma^2_{\xi}$), которую в программе мы назовём ξVariance, и дисперсия погрешности датчика ($\sigma^2_{\eta}$), известная под именем ηVariance.

F#, как и прочие языки в .NET, прекрасно работает с Unicode и разрешает греческие буквы в названиях переменных.

let iterateKalman ξVariance ηVariance (latitude, longitude, error) (p: SensorItem) =
    let a = error + ξVariance
    let b = ηVariance
    let K = a/(a + b)
    let nextError = (a * b)/(a + b)

    . . .

    let nextLatitude = K * p.Latitude + (1.0 - K) * (latitude + latitudeVelocity * Δt)
    let nextLongitude = K * p.Longitude + (1.0 - K) * (longitude + longitudeVelocity * Δt)

    (nextLatitude, nextLongitude, nextError)

Функция iterateKalman выполняет вычисления в рамках одной итерации. Она берёт следующую точку (это параметр p типа SensorItem), и по формулам, которые мы расписали выше, вычисляет следующие координаты и следующий квадрат ожидания ошибки.

Обратите внимание, что переменные latitude, longitude и error мы получаем и возвращаем, объединив в один кортеж (tuple).

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

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

let smoothByKalman σξ ση (points: SensorItem list) =
    let ξVariance = σξ ** 2.0
    let ηVariance = ση ** 2.0

    match points with
    | [] -> []
    | p1::ps -> let first = (p1.Latitude, p1.Longitude, p1.Timestamp, ηVariance)
                let smoothed = ps
                            |> List.scan (iterateKalman ξVariance ηVariance) first
                            |> List.map (fun (latitude, longitude, _) ->
                                         SensorItem(latitude, longitude, 0.0, 0.0, DateTimeOffset.Zero))

                SensofItem(p1.Latitude, p1.Longitude, 0.0., 0.0., DateTimeOffset.Zero)::smoothed

С помощью метода List.map мы превращаем список кортежей обратно в список SensorItem для дальнейшей обработки. Это всё.

Я, когда писал, думал, что кода получится значительно больше. Функциональное программирование и правда иногда напоминает магию.

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

Учитываем Speed и Heading

Вектор скорости нам известен по свойствам Speed и Heading. Первое — скалярная скорость в метрах в секунду, второе — направление вектора.

На двумерной плоскости нулевой угол совпадает с осью $0x$, и углы растут против часовой стрелки. Стандартные формулы рассчёта проекций, работающие через sin и cos, рассчитаны именно на такое положение дел.

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

Как это сделать?

На левом рисунке видим исходную систему: ноль наверху, направление по часовой стрелке. Отразим её по горизонтали (рисунок справа), чтобы изменить направление. Затем повернём на 90° по часовой стрелке.

Соответствующий код на F# выглядит так:

let cartesianAngleFromHeading =
    let flipHorizontal angle = 360.0 - angle
    let rotateClockwise90 angle = angle - 90.0
        
    flipHorizontal >> rotateClockwise90

Мы описываем две простых функции — flipHorizontal для отражения по горизонтали и rotateClockwise90 для поворота и возвращаем комбинацию (>>) этих функций.

(flipHorizontal >> rotateClockwise90) angle

Означает то же самое, что и

rotateClockwise90 (flipHorizontal angle)

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

Разберёмся, как это работает. F#, как и OCaml, позволяет программисту определять операторы. Оператор >>, который называется прямой композицией функций, определён как

let (>>) f g x = g (f x)

Когда мы пишем f >> g x, код перобразуется в g (f x), то есть в функцию g, которая применяется к результату вызова f с параметром x.

А если мы пишем f >> g, происходит магия каррирования, о которой я писал выше. Если вы не указали последний параметр функции, возникает новая функция с одним недостающим параметром.

Переведём градусы в радианы

let angle = radian (cartesianAngleFromHeading heading)

А также переведём метры в секунду в километры в час

let kilometersPerHour metersPerSecond = 3.6 * metersPerSecond
let velocity = kilometersPerHour speed

Вычислим проекции вектора скорости на параллель и мередиан

let velocityLongitude = velocity * cos angle
let velocityLatitude = velocity * sin angle

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

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

let kilometersPerLongitudeGrade = distance latitude longitude latitude (longitude + 1.0)
let kilometersPerLatitudeGrade = distance latitude longitude (latitude + 1.0) longitude

(velocityLatitude / kilometersPerLatitudeGrade, velocityLongitude / kilometersPerLongitudeGrade)

Функция целиком выглядит так:

let project latitude longitude speed heading =
    let cartesianAngleFromHeading =
        let flipHorizontal angle = 360.0 - angle
        let rotateClockwise90 angle = (270.0 + angle) % 360.0
        
        flipHorizontal >> rotateClockwise90

    let angle = radian (cartesianAngleFromHeading heading)

    let kilometersPerHour metersPerSecond = 3.6 * metersPerSecond
    let velocity = kilometersPerHour speed

    let velocityLongitude = velocity * cos angle
    let velocityLatitude = velocity * sin angle

    let kilometersPerLongitudeGrade = distance latitude longitude latitude (longitude + 1.0)
    let kilometersPerLatitudeGrade = distance latitude longitude (latitude + 1.0) longitude

    (velocityLatitude / kilometersPerLatitudeGrade, velocityLongitude / kilometersPerLongitudeGrade)

Теперь давайте допишем iterateKalman, чтобы учитывать скорость. Для вычисления скорости нам потребуется $\Delta t$, которую мы сможем вычислять, если вместе с координатами будем передавать метку времени.

let iterateKalman ξVariance ηVariance (latitude, longitude, timestamp: DateTimeOffset, error) (p: SensorItem) =
    let a = error + ξVariance
    let b = ηVariance
    let K = a/(a + b)
    let nextError = (a * b)/(a + b)

    let Δt = (p.Timestamp - timestamp).TotalHours
    let (latitudeVelocity, longitudeVelocity) = project latitude longitude p.Speed p.Heading

    let nextLatitude = K * p.Latitude + (1.0 - K) * (latitude + latitudeVelocity * Δt)
    let nextLongitude = K * p.Longitude + (1.0 - K) * (longitude + longitudeVelocity * Δt)

    (nextLatitude, nextLongitude, p.Timestamp, nextError)

Переменную timestamp надо добавить и в основную функцию smoothByKalman:

let smoothByKalman σξ ση (points: SensorItem list) =
    let ξVariance = σξ ** 2.0
    let ηVariance = ση ** 2.0

    match points with
    | [] -> []
    | p1::ps -> let first = (p1.Latitude, p1.Longitude, p1.Timestamp, ηVariance)
                let smoothed = ps
                            |> List.scan (iterateKalman ξVariance ηVariance) first
                            |> List.map (fun (latitude, longitude, _) ->
                                         SensorItem(latitude, longitude, 0.0, 0.0, DateTimeOffset.Zero))

                SensofItem(p1.Latitude, p1.Longitude, 0.0., 0.0., DateTimeOffset.Zero)::smoothed

Тестирование

Я обязательно перепишу этот раздел. Конечно, у меня есть тесты для фильтра Калмана, но они неподдерживаемые.

[<Fact>]
let ``smoothBySimplifiedKalman - with points - filters coordinates`` () =
    let source = [SensorItem(45.0, 0.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:14+03:00"));
                  SensorItem(45.5, 0.5, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:15+03:00"));
                  SensorItem(45.0, 1.0, 0.0, 0.0, DateTimeOffset.Parse("2018-12-07T16:38:16+03:00"))]

    let actual = List.toArray (smoothBySimplifiedKalman 1.0 1.0 source)

    Assert.Equal(45.1111111111111, actual.[1].Latitude, 1)
    Assert.Equal(0.111111111111111, actual.[1].Longitude, 1)
    Assert.Equal(45.0836111111111, actual.[2].Latitude, 1)
    Assert.Equal(0.331111111111111, actual.[2].Longitude, 1)

Из этого теста не очевидно, почему в Assert.Equal указаны те константы, которые указаны. Я взял их из работающего кода. Вместо такой магии лучше тестировать отдельные функции, такие как iterateKalman и cartesianAngleFromHeading.

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