Снижаем погрешность GPS на Android с помощью фильтра Калмана и акселерометра
Краткое и простое описание проекта:
Предыстория
Как-то раз пришла ко мне очень крутая задача — повысить точность позиционирования и подсчет пройденной дистанции по GPS координатам для приложения водителя. Заданные вопросы прояснили следующее: погрешность показаний разных GPS приемников может быть очень существенной. На точность GPS точки могут влиять расположенные рядом высокие здания, деревья, разные погодные явления. Так же влияет расположение спутников и т.п. Этой точности достаточно для определения собственного местонахождения, однако если использовать поток геоданных для вычисления расстояния и стоимости поездки для службы такси, то ошибки накапливаются, и получаемый результат оказывается значительно хуже ожидаемого. На бюджетных смартфонах ошибка может составить более 400%, что ведёт к разного рода неприятным ситуациям и недовольству клиента.
Сначала нужно было решить эту проблему на Android-смартфонах, так как из-за невысокой стоимости, их использует подавляющее большинство водителей такси. А затем нужно реализовать какой-нибудь сервис, который будет обрабатывать маршруты, присланные сторонними разработчиками.
Гугление показало, что один из самых надежных способов повысить точность позиционирования и подсчет дистанции — составить векторную карту дорог города и проецировать GPS координаты на ближайшие дороги, как делается здесь. Однако этот метод можно применить, если существует актуальная и надежная база координат дорог. Ее создание — нетривиальная задача, требующая задействования значительных ресурсов для каждого города, поэтому отпадает. Можно воспользоваться API от Google, но там есть ограничение на количество запросов в день + не всем водителям есть возможность установить google play сервисы.
Постановка задачи
Отсюда следует, что необходимо максимально улучшить показания GPS координат программно. Целью является реализация такого алгоритма на смартфоне, который решал бы следующие задачи:
- Убрать проблему того, что пройденный маршрут постепенно увеличивается, когда машина фактически стоит на месте. Это происходит из-за того, что GPS координаты приходят с погрешностью, и на карте выглядит как “звезды”.
- Отфильтровать резкие “скачки” в точку, удаленную от реального маршрута на значительное расстояние (до 500 метров)
- Восстановить маршрут при кратковременной (~30–60 секунд) потере связи с GPS.
При этом алгоритм не должен израсходовать батарею за 3 минуты работы и всю доступную память. Желательно, чтоб он вообще не накапливал координаты, а обрабатывал несколько предыдущих и текущие.
Очередной поиск показал, что самым распространенным решением проблем позиционирования и шума является использование фильтра Калмана (см. подробное описание ниже). О нем есть огромное количество статей в интернете, целая куча книг и разных научных работ. В этой статье не будет подробностей о том, как и почему он работает, хотя бы потому, что нам нужно конкретное решение задачи, а не теория.
Возник такой вопрос: что, кроме GPS координат, может дать информацию о местоположении объекта? Можно использовать:
- wi-fi точки, но в нашем городе свободных wi-fi точек мало.
- Можно использовать GSM-вышки, но нужно составить их карту.
И всё это выглядит как магия, потому что очень большой точности от этих источников не добиться, погрешность может быть до 500 метров. Если бы можно было как-то узнать о том, как именно движется объект, т.е. в какую сторону и с какой скоростью, это прояснило бы ситуацию.
Первый же запрос выдал информацию о том, что современные (и не очень) смартфоны оснащены набором датчиков, среди которых почти всегда есть акселерометр, гироскоп и магнитометр. Гироскоп стали добавлять в относительно новые телефоны, но это и не очень критично. То есть, в теории у нас есть еще один источник данных, который может помочь в определении местоположения объекта. Главное разобраться, как использовать эти датчики для получения информации о том, в какую сторону направлен вектор ускорения.
Акселерометр
Акселерометр это прибор, измеряющий проекции кажущегося ускорения . Это очень простое устройство, которое, грубо говоря, всегда знает где земля (если это не состояние свободного падения, конечно). Представим грузик с известной нам массой m, закрепленный между двумя пружинами. При ускорении в оси пружин грузик отклонится и по этому отклонению можно судить о силе, действующей на грузик. В микросхемах похожий механизм, только вместо грузика используется конденсатор.
Акселерометр можно использовать для определения ускорения тела и для определения угла наклона (в качестве инклинометра). Данные акселерометра довольно сложно интерпретировать. Допустим он показывает ускорение по оси X. С чем это связано? Тело может двигаться с ускорением вдоль оси X, может быть наклонено и это проекция g на ось X. В этом простейшем случае мы можем посмотреть на значение ускорения вдоль оси Z, но обычно ситуация сложнее. Но главная проблема в том, что акселерометр показывает только относительные ускорения, которые никак нельзя связать с картой и GPS координатами.
Еще одна проблема — акселерометры “шумят”. Есть приложение Acceleration explorer, с помощью которого можно очень хорошо увидеть насколько сильно шумит акселерометр и как с этим можно бороться. Можно использовать метод скользящего окна, фильтр низких частот, медианный фильтр или еще какой-нибудь хитрый алгоритм, чтобы уменьшить шум датчика. Но это может приводить к ошибкам и медленной реакции фильтра.
Можно, опять же, использовать фильтр Калмана, но для этого нужно знать дисперсию ошибки. Во встраиваемых системах можно узнать, что за акселерометр используется и в его техническом описании найти это значение, но при использовании смартфона необходимо самим подсчитать дисперсию, добавив шаг калибрации в приложение или просто подобрать такое значение, при котором большинство телефонов будут выдавать более-менее корректный результат.
Гироскоп
Гироскоп — это устройство, способное реагировать на изменение углов ориентации тела, на котором оно установлено, относительно инерциальной системы отсчета.
Зная начальное положение устройства в пространстве можно найти текущее положение, проинтегрировав показания гироскопа. Это очень точный и быстрый датчик, однако при интегрировании растёт ошибка и показания постепенно “уплывают”. Для борьбы с этим явлением используется sensor fusion, описанный далее.
Магнитометр
Магнитометр — это датчик, измеряющий характеристики магнитного поля. Простейшим подобным прибором является обыкновенный компас. Грубо говоря этот датчик всегда “знает” где север.
Однако всё не так однозначно, как может показаться на первый взгляд. Во-первых, магнитометр будет реагировать на любой сильный источник магнитного поля. Во-вторых, существует еще такое понятие, как “магнитное отклонение” — угол между “магнитным” Севером и “истинным” Севером (направлением вдоль меридиана на север). Национальное агентство геопространственной разведки (NGA) предоставляет исходные коды приложения, написанного на C, для определения значения магнитного отклонения на основе WMM (world magnetic model). Так же предоставляется файл с данными, который обновляется каждые 5 лет. Но об этом можно особенно не задумываться, потому что в Android есть специальный метод. Если это не андроид — можно воспользоваться готовым кодом отсюда https://www.ngdc.noaa.gov/geomag/WMM/soft.shtml.
Определение ориентации телефона в пространстве
Итак, у нас есть 3 датчика, с помощью которых мы можем однозначно определить положение телефона в пространстве. Можно выразить его с помощью матрицы поворота. Так же у нас есть вектор ускорения.
Для того, чтобы получить ускорение в “абсолютной” системе координат (связанной с Землёй) необходимо просто умножить инвертированную матрицу поворота на вектор полученного ускорения. Так происходит вот почему. Допустим у нас уже есть матрица поворота R, которую мы получили, воспользовавшись одним из алгоритмов AHRS (см. описание ниже). Предположим наше устройство никуда не движется. Тогда вектор ускорения A примет вид[Ax Ay Az].T() (T() значит транспонированый). При умножении R*[0 0 1].T() мы должны получить вектор A. Кстати это очень хорошая проверка того, что AHRS работает правильно. [0 0 1].T() — это вектор ускорений устройства, находящегося в покое, в “абсолютной” системе координат. Значит должен быть способ получить этот вектор из вектора A, используя матрицу R. Из курса линейной алгебры вспоминаем, что нельзя одну матрицу разделить на другую. Вместо этого можно умножить матрицу на инвертированную. Отсюда следует, что [0 0 1] = Rinv*A. Для нахождения инвертированной матрицы и всех этих умножений лучше всего использовать openGL.
В результате получится вектор из 3х элементов: ускорение на восток, на север и вверх.
Есть много методов для определения положения устройства в пространстве на основе данных от акселерометра, магнитометра и гироскопа. Часть из них можно посмотреть в приложении по ссылке ниже:
Из них всех было выбрано 2 лучших — виртуальный сенсор ROTATION_VECTOR и фильтр Маджвика (см. описание ниже). Лучшие они по нескольким причинам. Главные — они просты в использовании и для реализации не нужно писать очень много кода :) При этом они не шумят и работают очень быстро.
Виртуальные датчики Android
Разработчики Android проделали огромную работу для улучшения показаний датчиков и нам не нужно изучать и реализовывать все хитрые алгоритмы sensor fusion и AHRS. Для определения ускорения можно воспользоваться датчиком LINEAR_ACCELEROMETER, который выдает ускорения без учета силы гравитации. А для определения ориентации в пространстве можно (и нужно) использовать ROTATION_VECTOR.
Лучший результат показывает ROTATION_VECTOR, использующий фильтр Калмана. Здесь можно посмотреть на файл Fusion.cpp и немного повосхищаться.
Фильтр Маджвика
Фильтр Маджвика — это ПО с открытым исходным кодом, рассчитанное, в первую очередь, на низкую вычислительную мощность целевой системы. В качестве входных данных он использует показания акселерометра, гироскопа и (опционально) магнитометра. На выходе получается кватернион, описывающий положение устройства в пространстве. Он работает действительно быстро и почти не тратит ресурсы (в документации утверждается, что используется 160 операций сложения, 172 умножения, 5 делений и 5 извлечений квадратного корня), но есть проблема в определении параметров этого фильтра. Один из них — частота, с которой поступают данные с датчиков. Во встраиваемых системах можно очень точно её определить, но в Android ситуация другая. Используя Android можно указать только минимальную частоту опроса, в то время как реальня частота может быть гораздо выше. Впринципе это можно обойти, подсчитывая эту частоту каждый раз, когда приходят данные со всех 3х датчиков. Второй параметр — коэффициент усиления. Его нужно подбирать для каждого устройства индивидуально, чего нельзя сделать, когда даже телефоны одной модели могут давать разный результат. Так же этому фильтру нужно некоторое время (около 5–7 секунд, зависит от коэффициента усиления) на инициализацию/стабилизацию. Из плюсов стоит отметить, что его легко перенести на любую платформу.
Фильтр Калмана
Подготовительная работа закончена. К текущему моменту у нас есть вектор ускорения в “абсолютной” системе координат и можно приступать к реализации и использованию фильтра Калмана.
Фильтр Калмана — это эффективный рекурсивный фильтр, оценивающий вектор состояния динамической системы, используя ряд неполных и зашумленных измерений. Назван в честь Рудольфа Калмана.
Реализация фильтра сама по себе не очень сложна. Нужно определить и реализовать несколько операций с матрицами и просто следовать формулам из википедии. Есть множество готовых библиотек (даже в openCV есть этот фильтр https://docs.opencv.org/trunk/dd/d6a/classcv_1_1KalmanFilter.html), но можно реализовать что-то своё, чтоб лучше понимать, как он работает. Главная задача — определить вектор состояния системы, матрицу перехода, управляющий вектор и прочие компоненты фильтра Калмана. Одна из самых сложных задач — определить ковариационные матрицы шума процесса и шума измерений.
Матрицы в текущем решении основываются на законе равномерного движения, только в матричной форме (т.к. у нас 2 направления — восток и север). В такой форме довольно легко расширить фильтр, чтоб он учитывал и ускорение вверх, но пока в этом нет необходимости. А можно наоборот разбить фильтр на 2 или 3 для каждого направления. Почти ничего не изменится, только размеры матриц уменьшатся.
Параметры фильтра.
Вектор состояния системы
Здесь X и Y — координаты, а X’ и Y’ — скорости.
Матрица эволюции системы
Матрица управления
Вектор управления
Это наши проекции ускорений в абсолютной системе координат. Впринципе можно добавить ускорения в состояние системы, а матрицу и вектор управления сделать 0. Но так будет больше похоже на формулу закона равноускоренного движения.
Матрица измерений
Это матрица, помогающая привести все измерения к единой системе. К примеру, мы же не можем напрямую приравнять вольты к градусам цельсия, это не будет иметь смысла. Матрица H как раз является связующим звеном между вольтами и градусами цельсия. В нашем случае она равна единичной матрице, потому что никаких преобразований делать не нужно.
Здесь есть варианты вида этой матрицы. Если GPS-приемник предоставляет информацию о скорости (основываясь на эффекте Допплера), то эта матрица будет единичной. Если же GPS-приемник предоставляет только координаты, то матрица примет такой вид:
Второй вариант показывает лучший результат при кратковременной потере GPS сигнала, потому что на вектор состояния не влияет последняя полученная от приёмника скорость.
Ковариационная матрица шума измерений
Если GPS приемник не предоставляет информацию о скорости объекта. В противном случае:
Все эти значения можно получить разными способами. Можно распарсить NMEA сообщение и использовать компонент HDOP, можно использовать класс Location и метод getAccuracy(). Чтобы получить скорость в нашей системе координат необходимо получить направление. Это либо компонент course в NMEA сообщении, либо bearing из объекта Location. Здесь применяется допущение (хотя на самом деле вроде так и есть), что скорость X’ никак не влияет на координату X, потому что получаются они разными способами. Так же координата Y и скорость Y’ никак не влияют на скорости и координаты X.
Ковариационная матрица шума процесса
При формировании матрицы было сделано допущение, что ошибка акселерометра по оси X никак не влияет на ошибку по оси Y.
Для того, чтоб фильтр Калмана работал, нужно было все данные привести к единой системе координат. За основу взята следующая статья : https://en.wikipedia.org/wiki/Great-circle_distance. Смысл в том, что все координаты были переведены в расстояния от точки с координатами 0, 0.
GeoHash
GeoHash — алгоритм для преобразования 2 координат вида 37.571309, 55.767190 (долгота, широта) в 1 строку. Он похож на алгоритм бинарного поиска и работает очень быстро, почти не тратя вычислительные ресурсы.
В данном проекте используется для решения 2х проблем. Во-первых, необходимо как-то объединить находящиеся рядом точки, чтоб снизить поток избыточной информации. Для этого можно выбрать какой-нибудь радиус и объединять все точки, попадающие в окружность с этим радиусом. Но как выбрать радиус и центр этой окружности? Вычисление расстояния в сферических координатах — дорогостоящая операция, поэтому была использована GeoHash функция, позволяющая очень быстро определить относятся ли точки к одной области или нет. Эта функция выдает хэш в виде строки, длина которой определяется пользователем и влияет на точность кодирования. Чем больше длина хэша, тем меньше область и больше точность координат в одной области. Максимальная возможная длина геохэш-строки — 12 символов. Очень хороший результат для нашей задачи показывает длина хэша 7 или 8 символов.
Ещё одно применение этой функции — определение и фильтрация “скачков”. Мы будем считать, что если GPS приемник выдал подряд больше 3 (задается пользователем) координат с одним хэшем, то эта точка правильная и её нужно учитывать. Если же меньше — то, вероятно, это какое-то случайное значение, которое не нужно учитывать.
Как видно из приведенных примеров GeoHash фильтр позволяет сильно сократить количество обрабатываемых точек. Это может быть критичным, если координаты должен обрабатывать сервер или если планируется сохранять маршруты в базе данных (к примеру).
Результат работы, вывод и дальнейшие улучшения
В результате был написан Android модуль и библиотека на C, реализующие все вышеперечисленный фильтры. Визуально тракетории стали более сглаженными и исчезло накручивание дистанции при отсутствии движения. Как следствие — улучшился подсчет пройденной дистанции.
Для подсчета дистанции использовались 2 алгоритма. Один из них — алгоритм Винченти. Он используется внутри метода distanceBetween() класса Location. Второй метод взят отсюда: https://en.wikipedia.org/wiki/Great-circle_distance (раздел с haversine формулой). Тесты показывают, что результат почти не отличается при том, что второй вариант значительно менее ресурсоемок. Но есть подозрение, что на больших дистанциях погрешность будет больше. В пределах города второй алгоритм показывает такой же результат, как и первый.
В дальнейшем планируется реализовать восстановление траектории при кратковременной потере связи с GPS и максимально сократить использование GPS. На данном этапе можно восстановить траекторию при потере сигнала до 30–50 секунд. Дальше ошибка измерений акселерометра становится слишком большой.
Используйте и развивайте
Ссылка на репозиторий:
Для предложений по улучшению используйте issues. Для помощи и консультации по внедрению библиотеки так же создавайте задачи в репозитории.
Данное решение распространяется под лицензией MIT. Используйте и не забывайте ставить копирайты с предупреждениями :)
Список источников информации
https://www.youtube.com/watch?v=C7JQ7Rpwn2k&t=1856s — крутейший доклад о sensor fusion и не только.
http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ — всё, что связано с фильтром Маджвика, включая реализацию.
https://alexanderpacha.files.wordpress.com/2017/05/masterthesis-pacha.pdf — очень интересная статья, включающая подробное описание кватернионов, методы определения ориентации телефона и многое другое.
http://scottlobdell.me/2017/01/gps-accelerometer-sensor-fusion-kalman-filter-practical-walkthrough/ — отличное описание фильтра и того, как его можно использовать. Однако пример, который прилагается к предложенной версии алгоритма у меня не показал тех результатов, которые демонстрируются в видео. Очень хорошо объясняется то, как нужно использовать AHRS и IMU.
https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4570372/ — здесь пример того, как получить кватернионы с помощью акселерометра, магнитометра и как их объединить с помощью комплементарного фильтра.
http://campar.in.tum.de/Chair/KalmanFilter и https://en.wikipedia.org/wiki/Kalman_filter . Очень хорошо дополняют друг друга.
The English version below link:
