No Image

Фильтр калмана для акселерометра

СОДЕРЖАНИЕ
0 просмотров
22 января 2020

Фильтрация данных гироскопа с помощью фильтра Калмана #1

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

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

Еще нужно сказать о том, что в интернете мириады статей по теме, но по отдельности они бесполезны =( Мне пришлось изрядно покопаться, прежде чем я нашел решение для вроде-бы часто всплывающей задачи фильтрации данных гироскопа. Могу порекомендовать труды [1] и [2], совокупное восприятие которых помогло мне написать что-то работоспособное. Стоит отметить, что обе работы содержат ошибки или неточности в обозначениях, но раз уж я смог разобраться, значит они совсем незначительны.

Сложности

Фильтр Калмана внешне обманчиво просто выглядит: реально 5 уравнений и всё. Ну да, пусть уравнения матричные, но мы же большие уже, матрицы могем давно. Но:

  1. мне непонятен смысл матриц . Нет, конечно, я прочитал, что матрицы такие-то отвечают за такие-то неопределенности, матрица P – степень уверенности в показаниях, матрица K – матрица коээфициентов усиления ошибок и прочее, но в единую картину это как-то не складывается
  2. не совсем понятно как нужно формулировать задачу, чтобы решать ее фильтром Калмана.

В процессе поиска информации по этому самому фильтру, наткнулся на замечательное (http://habrahabr.ru/blogs/augmented_reality/118192/):

Так вот, этот фильтр, «Калмановский», он зародился в радиолокации, в обработке радиолокационных сигналов, потому что там, вы посылаете волну, она к вам обратно приходит и вы по времени задержки измеряете расстояние, а по эффекту Допплера, который «ви-и-и-у», измеряете скорость. Почти как у нас, только у нас углы, но то же самое, т.е. есть величина и производная величины, замерянные в один момент времени. И дальше там надо максимально эффективно отфильтровать шум, «Калман» придуман для этого. У меня мама занималась всю свою жизнь вторичной обработкой радиолокационных сигналов. Я, уже когда со всем этим разобрался, реализовал этот complimentary filter, спросил у нее:
— А как вы фильтровали? Ты разбираешься в фильтре Кальмана? Можешь мне его объяснить?
— Ой, да, у нас там отдельный институт в Жуковском или еще где-то работал над этими фильтрами Калмана, все защитили кучу диссертаций…
— А как вы подбирали параметры?
— Ну так, чисто экспериментально, на глазок, кое-как…
— Хм, интересно. А другие фильтры, попроще, не пробовали применять?
И тут она мне выдала:
— Ну да, мы под конец использовали простейший «альфа-бета» фильтр, он работает примерно также, а вычислительно намного проще.
Там еще советские вычислительные машины стояли, и этот фильтр Калмана был очень вычислительно дорогой.

Попытка взлететь

После всех этих бугагашечек и зачитывания матриц вслух, в надежде на просветление, мною была предпринята попытка, обложившись распечатками статей, реализовать хоть что-нибудь. Для начала я попробовал совсем одномерный случай – в последовательности рандомных точек, пляшущих около определенного значения, увидеть сигнал. Я воспользовался процедурой, описаной в [2] в разделе 4.3 (стр. 30) и у меня получилось, что вызвало искривление лица на манер оО. (Результаты я не сохранил, поэтому переходим ко второй попытке)

Попытка взлететь №2

Для второй попытки я плотно раскурил доку [1] и первым делом составил все матрицы. В самой доке приведен пример с гироскопом, на выходе которого – напряжение. Будем считать, что у нас на выходе просто угловая скорость в радианах в секунду. На реальном железе я пока не пробовал, но говорят, что у реального гироскопа амплитуда отклонений в состоянии покоя составляет порядка 4 рад/с, из этого и будем исходить:

В моем случае никакого акселерометра нет, есть только гироскоп. Ввиду того, что интересует нас угловая скорость (назовем omega) и угол – отклонение от начального положения (phi), размерность задачи получается 2, отсюда и размеры всех матриц.

Гироскоп я решил моделировать следующим образом:

где: i – время (на самом деле – просто счетчик цикла).

Ну а далее собственно сам код рекуррентного фильтра (Калмана), цель которого заключается в обновлении априорных и апостериорных параметров фильтра, что бы это ни значило:

В итоге, получается следующий код:

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

Рис.1 estimate – оценка омеги, real – значения, снимаемые с "гироскопа", mean – среднее, относительно которого пляшет real.

Рис.2 estimate – оценка фи, real – фи, полученное путем интегрирования омеги (mean с предыдущего графика).

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

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

Читайте также:  Яркость экрана телевизора какая лучше

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

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

Подробней о реализации и о возможностях этого метода, советую прочитать здесь и вот здесь. Целью же этой статьи показать, как можно реализовать в bascom-avr фильтр Калмана, в упрощенном варианте. Очень упрощенном. Здесь не будет матриц и многоэтажных уравнений, способных нанести нормальному человеку травму мозга. А будет всего одна формула, которая выводится из всей горы матриц, при условии если мы пренебрежем расчетом управляющего воздействия. Формула в конечном итоге имеет следующий вид:

где – результирующее значение текущего вычисления,

– коэффициент стабилизации,

– исходное значение текущего измерения,

– результирующее значение предыдущего вычисления.

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

Это все теория, теперь перейдем к практике. Для испытания я взял аналоговый акселерометр ADXL335 и подключил его к микроконтроллеру ATMega8, считывание данных идет через АЦП.

Тестовый код в Bascom-AVR

$regfile = "m8def.dat" ‘микроконтроллер ATmega8
$crystal = 8000000 ‘частота работы 8МГц
$baud = 9600 ‘скорость передачи 9600 бод
$hwstack = 32
$swstack = 32
$framesize = 32

‘конфигурируем АЦП
Config Adc = Single , Prescaler = 128 , Reference = Internal

Dim R As Word ‘переменная R в которую будем записывать показания с АЦП

declare sub kalman ‘объявляем подпрограмму фильтрации

‘переменные для фильтра
dim Mn as single ‘результирующее значение
dim An as Single ‘исходное значение
dim Mn1 as Single ‘результат вычисления в предыдущей интерации
dim k as single ‘коэффициент стабилизации
k = 0 . 1 ‘устанавливаем коэффициент

R = Getadc ( 0 ) ‘считывание данных с акселерометра

An = R
call Kalman ‘вызываем подпрограмму фильтрации

Print R ; ";" ; round ( Mn ) ‘отправляем данные в терминал

‘подпрограмма фильтрации
sub Kalman
Mn = k * An
An = 1 – k
Mn1 = Mn1 * An
Mn = Mn + Mn1
Mn1 = Mn
end sub

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

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

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

–>Категория : Полезная информация | –>Добавлено : 10.04.2016

–>Просмотров : 16400 | –>Комментарии : 25 | –>Теги : акселерометр, Калман, Фильтр | –>Рейтинг : 4.6 / 8
–>Всего комментариев : 25

Сделал все тоже самое о чем говорит RDW. Взял код из примера, добавил номер шага перед значением после фильтра. Подал сначала максимальное значение на вход, и как только значение стало максимальным-подал 0 и посмотрел за сколько опустится до 0. Так что exersizze прав, увеличение и спад значений происходит за одинаковое число шагов. RDW, ЧЯДНТ?

Динамику значения я проверял просто:

– подал на вход максимальное значение (некоторое N), начал считать через сколько шагов оно его достигло;
– потом резко уменьшил до минимума, так же начал считать шаги.

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

Вопрос по статье, попробовал данный алгоритм и чуть почитал статьи из ссылок выше, возникли вопросы:

1. фильтр в реальности странно как-то работает, при уменьшении входного значения, данные на выходе очень медленно меняются (долго падают), но при увеличении входных данных, динамика идет в разы быстрее. К = 0.2 Непонятно, почему так, как убрать эту неравномерность?
2. в статьях куда больше переменных и формулы посложнее, вы сами корректировали вычисления? Уверенны в них?

KoSS_89, попробовал повторить и задал коэффициент константой, все работает. Но у меня версия сейчас стоит 2.0.7.8 может в этом дело.

radiomag , я бы назвал этот опыт построения сверлилки не удачным, хотя неудачный опыт – тоже опыт. Но описывать неудачную конструкцию не вижу смысла, только когда доведу до ума. Хотя склоняюсь все же за ручную подачу, как и у всех нормальных станков:) Вот, если не видели есть такая конструкция, которую можно напечатать на принтере http://www.thingiverse.com/thing:766712 и ее обзор https://www.youtube.com/watch?v=PNLtDmbVrr8

Зачётная сверлилка.
Почему бы не опубликовать описание девайса в отдельном топике? Вообще то ручное управление, имхо, "рулит" (более желательно, как говорится).

P.S.
За основной материал – тоже спасибо. Очень познавательно.

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

Ну это распространенная ошибка – пробовать использовать передачу как направляющую.

The English version available here .

Предыстория

Как-то раз пришла ко мне очень крутая задача — повысить точность позиционирования и подсчет пройденной дистанции по GPS координатам для приложения водителя. Заданные вопросы прояснили следующее: погрешность показаний разных GPS приемников может быть очень существенной. На точность GPS точки могут влиять расположенные рядом высокие здания, деревья, разные погодные явления. Так же влияет расположение спутников и т.п. Этой точности достаточно для определения собственного местонахождения, однако если использовать поток геоданных для вычисления расстояния и стоимости поездки для службы такси, то ошибки накапливаются, и получаемый результат оказывается значительно хуже ожидаемого. На бюджетных смартфонах ошибка может составить более 400%, что ведёт к разного рода неприятным ситуациям и недовольству клиента.

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

Гугление показало, что один из самых надежных способов повысить точность позиционирования и подсчет дистанции — составить векторную карту дорог города и проецировать GPS координаты на ближайшие дороги, как делается здесь. Однако этот метод можно применить, если существует актуальная и надежная база координат дорог. Ее создание — нетривиальная задача, требующая задействования значительных ресурсов для каждого города, поэтому отпадает. Можно воспользоваться API от Google, но там есть ограничение на количество запросов в день + не всем водителям есть возможность установить google play сервисы.

Постановка задачи

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

  • Убрать проблему того, что пройденный маршрут постепенно увеличивается, когда машина фактически стоит на месте. Это происходит из-за того, что GPS координаты приходят с погрешностью, и на карте выглядит как “звезды”.
  • Отфильтровать резкие “скачки” в точку, удаленную от реального маршрута на значительное расстояние (до 500 метров)
  • Восстановить маршрут при кратковременной (

30–60 секунд) потере связи с GPS.

При этом алгоритм не должен израсходовать батарею за 3 минуты работы и всю доступную память. Желательно, чтоб он вообще не накапливал координаты, а обрабатывал несколько предыдущих и текущие.

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

Возник такой вопрос: что, кроме GPS координат, может дать информацию о местоположении объекта? Можно использовать:

  1. wi-fi точки, но в нашем городе свободных wi-fi точек мало.
  2. Можно использовать 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 и фильтр Маджвика (см. описание ниже). Лучшие они по нескольким причинам. Главные — они просты в использовании и для реализации не нужно писать очень много кода 🙂 При этом они не шумят и работают очень быстро.

Виртуальные датчики Andro >Разработчики 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 для каждого направления. Почти ничего не изменится, только размеры матриц уменьшатся.

Комментировать
0 просмотров
Комментариев нет, будьте первым кто его оставит

Это интересно
Adblock detector