Установка и подключение приемника GPS компас к ArduPilot квадрокоптера

Установка и подключение приемника GPS компас к ArduPilot квадрокоптера Вертолеты

Hard iron

Даю справку. Интенсивность магнитного поля земли сильно зависит от земных координат, в которых оно измеряется. Например, в Кейп Тауне (Южная Африка) поле составляет около 0.256 Гс (Гаусс), а в Нью-Йорке в два раза больше — 0.52 Гс. В целом по планете, интенсивность магнитного поля варьируется в диапазоне от 0.25 Гс до 0.65 Гс.

Для сравнения, поле обычного магнитика на холодильник составляет 50 Гс, — это в сто раз больше чем магнитное поле в Нью Йорке!! Понятно, что чуткий магнитометр может легко запутаться, если рядом с ним возникнет один из таких магнитов. На квадрокоптере, конечно, таких магнитиков нет, но зато есть куда более мощные редкоземельные магниты вентильных двигателей, а еще электронные цепи контроллера, провода питания и аккумуляторная батарея.

Такие источники паразитного магнитного поля называют Hard Iron. Воздействуя на магнитометр, они придают некоторое смещение измеряемым значениям. Посмотрим, имеются ли Hard Iron искажения у нашей сферы. Проекция точек сферы на плоскость XY, выглядит следующим образом:

Видно, что облако точек имеет некоторое заметное смещение по оси Y влево. По оси Z смещение практически отсутствует. Ликвидировать такое искажение очень просто: достаточно увеличить или уменьшить получаемые от прибора значения на величину смещения. Например, калибровка Hard Iron для оси Y будет иметь вид:

Ycal_hard = Y — Ybias

где Ycal_hard — калиброванное значение;Y — исходное значение;Ybias — величина смещения.

Смотрите про коптеры:  Узнайте о дронах FPV с помощью подробной расшифровки

Чтобы вычислить Ybias нам потребуется зафиксировать максимальное и минимальное значение Y, а затем воспользоваться простым выражением:

Ybias = (Ymin-Ymax)/2 — Ymin

где Ybias — искомая величина смещения;Ymin — минимальное значение оси Y;Ymax — максимальное значение оси Y.

Soft iron

В отличие от Hard Iron, искажение типа Soft носит куда более коварный характер. Опять же, проследим этот вид воздействия на собранных ранее данных. Для этого, обратим внимание на то, что шар на картинке сверху, и не шар вовсе. Его проекция на ось YZ немного сплющена сверху, и слегка повернута против часовой стрелки.

Исправить ситуацию со сплющенностью поможет умножение показаний датчика на некоторый множитель:

Ycal_soft = Y * Yscale

где Ycal_hard — калиброванное значение;Y — исходное значение;Yscale — коэффициент масштабирования.

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

Yscale = (Amax-Amin)/(Ymax-Ymin)

где Yscale — искомый коэффициент искажения по оси Y;Amax — максимальное значение на некоторой оси;Amin — минимальное значение на некоторой оси;Ymax — максимальное значение на оси Y;Ymin — минимальное значение на оси Y.

Другая проблема, из-за которой сфера оказалась повернутой, устраняется чуть сложнее. Однако, вклад такого искажения в общую ошибку измерения достаточно мал, и мы не будем подробно расписывать способ его «ручного» нивелирования.

Автоматическая калибровка

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

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

В интернете можно найти несколько программ, пригодных для этого. Например, MagCal, или еще одна — Magneto. В отличие от MagCal, в Magneto рассчитанные параметры выводятся в готовом к использованию виде, без необходимости дополнительных преобразований. Именно этой программой мы и воспользуемся. Главная и единственная форма программы выглядит следующим образом:

В поле «Raw magnetic measurements» выбираем файл с исходными данными. В поле «Norm of Magnetic or Gravitational field» вводим величину магнитного поля Земли в точке нашей дислокации. Учитывая, что этот параметр никак не влияет на угол отклонения стрелки нашего виртуального компаса, я поставил значение 1090, что соответствует значению 1 Гаусс.

Затем жмем кнопку Calibrate и получаем:

  1. значения смещения по всем трем осям: Combined bias (b);
  2. и матрицу масштаба и ортогонализации: Correction for combined scale factors, misalignments and soft iron (A-1).

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

Vcal = A-1 * (V — Vbias)

где Vcal — вектор калиброванных значение магнитометра для трех осей;A-1 — матрица масштаба и ортогонализации;Vbias — вектор смещения по трем осям.

Безопасность на квадрокоптере: калибруйте компас и придерживайте рукой

Посадка в руку - приручение

Месяц назад мы приобрели квадрокоптер DJI Phantom 3 Advanced. Аппарат доставляет массу удовольствия всей семье, особенно, когда после полёта возвращаешься домой и просматриваешь записи с высоты на большом экране. Но самые сильные эмоции мы испытали, когда дрон терял управление.

Вообще, при покупке такого аккуратненького, красивенького и технологичного вертолётика руки чешутся запустить его ввысь немедленно. Я не отношу себя к самым безбашенным экспериментаторам, но всё, что мне поначалу хотелось узнать из руководства по эксплуатации — это как запустить двигатели и на что влияют рычажки на пульте. Так как до этого имелся небольшой опыт управления маленьким дроном, похожим на Syma, то уж с Фантомом, казалось, проблем управления быть не должно.

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

Интересную мысль высказал в своём интервью один дизайнер из нашей компании. Суть в следующем: все эти гаджеты и виджеты вокруг нас формируют отличную от натуральной действительность, по-нашему — дополненную реальность. Если взрослые люди ещё как-то осознают, где и что, то детишки приходят в этот мир, наполненный дронами, планшетами и прочей техникой, ощущая его, наверное, вполне себе целостно. Конечно, я успел помечтать о сооружении квадрокоптера своими руками. К своему творению у меня вряд ли осталось бы такое детское любопытство, какое возникает к сложной китайской поделке. Фантом от DJI возник в моей жизни весьма спонтанно, словно доселе невиданное физическое явление, которое хотелось бы не только использовать по назначению, но и познавать как бы изнутри. Следует сразу подчеркнуть, что килограммовая машина с мощными пропеллерами действительно опасна со всех сторон. Однако опасность, как известно, резко снижается в процессе освоения аппарата, особенно, освоения на грани безопасности.

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

Кто-то скажет, что цена летающего гаджета сама по себе ограничивает фантазию на эксперименты, но, даже если этот «кто-то» в большинстве, то дронов, убитых в первом же полёте, всё равно уже не вернуть. И вообще, разве какие-то там презренные деньги могут охладить пыл настоящего русского естествоиспытателя? Тем более, уже потраченные деньги.

Меня лично остановили чисто технические препятствия: батарейка в коробке была заряжена всего на 20%, и приложение DJI Go почему-то не показывало видео с камеры на моём престарелом смартфоне. Пока разбирался с видео, батарейка села ещё больше. Так что с первым полётом Фантом у меня выше полутора метров от земли не оторвался.

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

Так вот, прежде чем запускать новый квадрокоптер в опасные трёхмерные путешествия, я настоятельно рекомендую не мануалы читать, а посмотреть на Ютюбе видео падений и других неприятных ситуаций с мультикоптерами. Я-то просто собирался глянуть, как люди летают, точнее, пилотируют дронов, но глаз сам зацепился за картинки с подписями вида «phantom crash». После таких роликов инструкции уже не читаются, а как бы перечитываются в поисках важных сведений. Поэтому к вечеру я был спокоен и уравновешен. Летать, как полагается, начал в режиме новичка. А когда отключил его, поначалу значительно ограничивал высоту и дальность полёта.

Более всего впечатлили видео, показывающие, как Фантом уносится в неизведанное, подхваченный сильным ветром. Есть в них какая-то тревога и безнадёжность, которая не чувствуется в подборке забавных падений и столкновений. Успокаивает только одна мысль: «Если видео выложили, значит, дрон с бортовой флэшкой таки нашёлся». Но снова тревожит другая: «А что если нашёл его кто-то другой? А что если выжила только флэшка?» Честно говоря, мы тоже уже запускали при значительных скоростях воздушных потоков, но дрон, отметим, противостоит им весьма уверенно. Трудно представить, какие ветра имели место дуть в приведённых видео. Там, небось, оператора самого сносило, а он ещё дроном пытался управлять. Хотя, ещё раз, верховые воздушные течения могут сильно отличаться от низовых.

Просматривая видео с бьющимися и сдуваемыми квадрокоптерами, я заметил однажды простую рекомендацию: калибруйте компас перед полётом — это позволит Фантому без проблем вернуться к месту взлёта. Действительно, возврат «домой» является чуть ли не самой важной функцией летательного аппарата. Потому что при подъёме на 150 метров над землёй даже в чистом дневном небе его уже непросто разглядеть. А если не знать примерный сектор телесного угла, то и просто не разглядеть. Если же дрон взлетает на 250 метров и по горизонтали удаляется на 500, то визуально его контролировать нет никакой возможности. В незнакомом однообразном ландшафте по видео с его камеры тоже не сориентируешься. Подозреваю, на заре коптеростроения не один аппарат потерялся в небе таким образом. А нам повезло. Умные китайцы встроили такой функционал, что квадрокоптер возвращается домой не только тогда, когда его просят, но и в том случае, если теряет сигнал с пульта. Кстати, в городе потеря сигнала происходит регулярно.

Но вернёмся к калибровке. Вышли мы однажды во двор собственного жилищного комплекса и откалибровали компас. Таблица состояния квадрокоптера исчезла с экрана планшета, запустились двигатели, и коптер взмыл в небо. Сантиметров на шестьдесят где-то… Сразу после взлёта его повело куда-то вбок, в дерево. Оператор сумел корректно отреагировать и увести машинку от столкновения. Но сумасшедший дрон продолжил хулиганство: сначала чуть не повыкосил оператора вместе со зрителями, а затем благополучно врезался таки в другое дерево и упал вверх ногами на траву. Снова нужно отдать должное китайцам: винты не сломались, и вообще, всё осталось в целости, если не считать пары царапин на борту.

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

Но не совсем обычно. С руки. На мой взгляд, полезно знать, что DJI Phantom 3 без проблем запускается, если держать его одной рукой за ногу. Может пригодиться при запуске на каком-нибудь поле с высокой травой. В конце видео под этой статьёй показано, как осуществлять таким образом приземление, то есть приручение. Ну и просто полезно знать тяжесть и силу квадрокоптера на всякий случай. При первом запуске с руки я просто поддал газу, чтобы вывесить дрона в воздухе и, не разжимая полностью пальцев, убедился, что его никуда не сносит.

Кроме того, после первого удара о дерево мы немедленно приобрели защитные рамки для пропеллеров. Они, конечно, делают конструкцию громоздкой и непригодной к быстрой разборке, но так спокойнее. Между прочим, на том же Ютюбе есть такие видео, где квадрокоптеры внезапно устремляются в стену или в людей без всякой видимой причины. С нами такого не случалось, но во время съёмок около высоких зданий аппарат действительно иногда непредсказуемо болтает. Трындит, как прожжённая торговка… Подозреваю, дело тут в сложных воздушных завихрениях, которые локально могут быть очень стремительны (яркий пример — торнадо). Так что пусть лучше он будет в защите.

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

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

Обн. Образовалось много интересных комментариев, за которые я очень благодарен. Конечно, статья и не претендовала на истину в последней инстанции, но лично на мой вкус ценности своей из-за критики не потеряла. Насчёт калибровки компаса и, вообще, аккуратного взлёта есть обширный комментарий опытного коптеровода fly4546. Мне кажется, это чересчур, но для справки интересно. На днях сам взлетал с автомобильной стоянки на крыше здания: с пола Фантом взлетать отказывался, т.к. в окружении такого количества железа компасу сносило крышу, зато с руки спокойно взлетел и вёл себя абсолютно предсказуемо. Думаю, мозги у Фантомов всё лучше, поэтому великая доля осторожности от прежних поколений теряет актуальность.

Быть или не быть?

Немного рассуждений на форуме RCDesign (начало) по поводу делать compassmot или нет.

Александр Старостин:

Интересно, тут коптерный гуру jabram не советует использовать compassmot. Точнее советует использовать его только для оценки влияния магнитного поля на компас, а затем выключать использование. И работать над уменьшением наводок (перекладывать, связывать провода и т.п.).

Вячеслав Перминов:

Возможно он отчасти прав.
Я не знаю точно, как оно работает. У меня такое предположение. Компасмот запоминает влияние потребляемого тока на компас в момент проведения процедуры и потом корректирует данные компаса в зависимости от нагрузки. Если при реальной эксплуатации элекртомагнитная картина поменяется (акк другой стороной вставили, разъем уложили не так как было) то есть вероятность неправильной корректировки компаса.

Алексей Козин:

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

Вячеслав Перминов:

Задумка с компасмотом верная. Источник отклонения у нас вращается вместе с аппаратом. Каждый мотор учитывать не обязательно. Достаточно знать на сколько и куда они суммарно отклонят компас. Если разъем на акке жесткий (кажись у фантома 2 такой), и конфигурацию не трогать — то должен нормально работать.
Стороны света не при чем. Если наводка относительно компаса постоянная, то ее можно вычесть(или прибавить) от показаний компаса. Хуже всего, если наводка полярность поменяет (напр. провода у акка перевернули). Тогда компасмот будет удваивать погрешность.

Update 22.11.2023: Вопрос целесообразности выполнения процедуры.

Если вы нашли ошибку на странице, то нажмите Shift Enter или нажмите здесь, чтобы уведомить нас.

Вкладка cli

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

Затем нужно сообщить эти диапазоны прошивке. Делается это следующими командами:

rxrange 0 1050 1950 rxrange 1 990 1992 rxrange 2 990 2023 rxrange 3 1070 1980

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

set p_pitch = 55 set i_pitch = 40 set d_pitch = 15 set p_roll = 55 set i_roll = 40 set d_roll = 15 set p_yaw = 90 set i_yaw = 45 set d_yaw = 20 set p_level = 20 set i_level = 15 set d_level = 75 set gyro_lpf = 256hz set tpa_rate = 10 set tpa_breakpoint = 1650 set looptime = 1000 set gyro_sync = on set gyro_sync_denom = 8

Опытным путём (то есть, в поле) надо определить газ висения и задать как значение переменной nav_mc_hover_thr. Оно будет использовано в режиме ALTHOLD. Если фактический газ висения будет отличается от заданного, то при активации режима удержания высоты, квадрокоптер будет сильно просаживатся или подпрыгивать.

Чтобы квадрокоптер армился в любом положении (даже вверх ногами), надо прописать:

set small_angle = 180

В iNAV много внимания уделено всевозможной защите. Выше уже упоминалась блокировка запуска моторов при неоткалиброванных датчиках. Кроме этого, есть переменные inav_gps_min_sats и nav_extra_arming_safety. Первая отвечает за минимальное количество спутников GPS, которые надо «поймать» для арминга, вторая разрешает/запрещает арминг при полном отсутствии сингала GPS.

Собственно, всё, можно сохранять. Вот полный дамп моих настроек:

Источник

Влияние наклона магнитометра на вычисляемое направление

На очереди проблема номер два. В начале статьи мы уже попробовали вычислить угол между севером и стрелкой компаса. Для этого годится простая формула:

H = atan(Y/X)

где H — угол отклонения стрелки компаса от северного направления;X,Y — калиброванные значения магнитометра.

Представим теперь, что мы фиксируем ось X строго по направлению к северу, и начинаем вращать датчик вокруг этой оси (придаем крен). Получается, что проекция поля на ось X остается неизменной, а вот проекция на Y меняется. Согласно формуле, стрелка компаса будет показывать либо на северо-запад, либо на северо-восток, в зависимости от того, в какую сторону делаем крен. Это и есть, заявленная в начале статьи, вторая проблема электронного компаса.

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

Vcal2 = Ry*Rx*Vcal

где Vcal — магнитный вектор, очищенный от Hard и Soft искажений;Rx и Ry — матрицы поворота вокруг осей X и Y;Vcal2 — магнитный вектор, очищенный от влияния крена и тангажа.

Пригодная для программы контроллера формула будет иметь вид:

Xcal2 = Xcal*cos(pitch) Ycal*sin(roll)*sin(pitch) Zcal*cos(roll)*sin(pitch) Ycal2 = Ycal*cos(roll) — Zcal*sin(roll) H = atan2( -Ycal2, Xcal2 )

где roll и pitch — наклоны вокруг осей X и Y;Xcal,Ycal,Zcal — вектор магнитометра (Vcal);Ycal2, Ycal2 — калиброванные значения магнитометра (Zcal2 не считаем — он нам не пригодится);H — угол между севером и стрелкой компаса.

Компас самолета. магнитный компас самолета.

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

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

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

Компас самолета

В середине котелка размещена колонка, на которой крепится картушка. Картушка – это комплекс соединенных магнитов, которые направлены один к одному одноименно заряженным полюсом. В большинстве случаев картушки авиационных компасов состоят из двух горизонтальных и двух вертикальных магнитов. Магниты должны быть расположены с высокой степенью точности, поскольку малейший сдвиг может привести к отклонению показателей от истинных. Верхние пары магнитов имеют значительно больший магнитный момент, нежели нижние, в соотношении 15 CGSm к 12 CGSm. В итоге суммарный момент должен быть не ниже чем 54-56 CGSm. От правильного подбора магнитов и их размеров зависит качество работы компаса. На конце картушки устанавливается стрелка, которая и указывает на сторону горизонта, она служит для ориентировки в полетной карте. Общая магнитная система рассчитывается на 200 часов работы двигателей. Внутри котелка нанесена курсовая черта, которая используется в качестве индекса при отсчете курса.

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

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

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

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

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

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

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

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

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

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

Магнитное наклонение

На самом деле, последний рисунок может показаться немного странным. Почему будучи в горизонтальном состоянии, датчик показывает почти максимальное значение по оси Z?? Ситуация повторяется если мы наклоним прибор, например, осью X вниз — опять получим максимальное значение (левый круг). Получается, что на датчик постоянно действует поле направленное сквозь датчик вниз к поверхности земли!

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

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

Теперь же перейдем, непосредственно к проблемам.

Опции темы

Временно отключен, я на INAV не летал, но в BetaFlight привык всё проверить дома.. а как это сделать с GPS не представляю..

В inav без компаса и gps не будет файлсейва. А что вы собрались проверять дома?

Хотел проверить RXLOSS как и писал выше. Странно то что в настройках Failsafe присутствую и Drop и Landing, но они не работают. И то что на скинах выше я выкладывал это так же нормально?

Тоесть AUX каналы начиная с 6ого при потери сигнала устанавливаются кто на что горазд. а ведь у меня на них будет назначено многое.

Тоесть, если что-то случилось с GPS, например сразу после взлёта, проверяем RTH, а он не сработал, и к примеру RXLOSS, то всё коптер просто улетает в космос? Я правильно понял?

Хотел проверить RXLOSS как и писал выше. Странно то что в настройках Failsafe присутствую и Drop и Landing, но они не работают. И то что на скинах выше я выкладывал это так же нормально?

https://www.youtube.com/watch?v=6uZNbwl1EU4

Тоесть AUX каналы начиная с 6ого при потери сигнала устанавливаются кто на что горазд. а ведь у меня на них будет назначено многое.

Тоесть, если что-то случилось с GPS, например сразу после взлёта, проверяем RTH, а он не сработал, и к примеру RXLOSS, то всё коптер просто улетает в космос? Я правильно понял?

Фильтр калмана: разбор навигационной системы бпла исходный код

В статье я бы хотел объяснить принципиальную разницу между Фильтром Калмана (ФК) и классическими фильтрами, кратко рассмотреть преимущество выбранного ФК поделиться опытом использования данного ФК во встраиваемой системе квадрокоптера для навигации на основе инерциального и ГНСС датчиков и поделится исходным кодом с демкой для самостоятельного изучения.

Всем привет! Меня зовут Илья, я работаю программистом-инженером в компании “SID Logic”, мы занимаемся разработкой электроники и сопутствующего ПО для различных применений, мои области интересов касаются систем навигации (оптические, инерциальные, ГНСС, UWB, GSM/LTE), систем управления (ПИД, MRAC, MPC) преимущественно для задач робототехники, беспилотников. Одну из важных тем в навигации занимает тема Фильтра Калмана.

На Хабре фильтру Калману (ФК) посвящено уже много статей, но я пока не встречал объяснения (возможно, плохо смотрел?:))принципиального преимущества его использования перед, например, альфа-бета, различными комбинациями фильтров низкой частоты (ФНЧ), фильтров высокой частоты (ФВЧ), интеграторами, дифференциаторами и т.д… Однако, даже если вы знали, то о чем я буду дальше говорить, возможно, вам будет полезен код, которым я пользуюсь уже несколько лет, на основе которого было проверено множество гипотез, подтверждено их применение.

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

Для начала нужно рассмотреть такую формулировку задачи. Абстрактной задачи.

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

Установка и подключение приемника GPS компас к ArduPilot квадрокоптера

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

Сшивка панорамы
Сшивка панорамы

В обоих примерах вы можете регулировать переменные (крутилки и смещения фото) и смотреть, как они влияют на результат, и вы можете оценить, как далеко вы от цели или близко (близка ли установленная температура к желаемой, сильно ли видно границу стыка двух фотографий). То есть у вас есть функция прямой зависимости (температура = f(крутилки) и визуальная сходимость стыка = f(смещение 2 фото)), но нет обратной зависимости.

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

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

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

ФК помогает решать задачи:

Итак, основная мысль данной статьи, на которой я хотел бы акцентировать внимание:  

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

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

Будем работать с более применительным к реальности видом ФК – то есть нелинейным, так как большинство систем имеют нелинейные взаимосвязи.

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

Все нелинейные виды ФК так или иначе используют линеаризацию. Распространены 2 вида линеаризации ФК:

EKF и UKF линеаризации
EKF и UKF линеаризации

В инженерном контексте для нас можно выделить следующие особенности:

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

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

Согласно целям статьи, понизить порог вхождения в тему, дать универсальный алгоритм для быстрого прототипирования и проверки гипотез в условиях слабой вычислительной базы и малого кол-ва памяти лучшим выбором будет являться второй вид ФК – UKF.

Помимо UKF, существуют следующие вариации сигма-точечного фильтра Калмана: CDKF (central difference Kalman filter – фильтр Калмана центральной разности), SRUKF (square-root unscented Kalman filter – ансцентный фильтр Калмана квадратного корня), SRCDKF (square-root central difference Kalman filter – фильтр Калмана квадратного корня центральной разности) и их итеративные модификации IUKF, ICDKF, ISRUKF, ISRCDKF, которые рассматриваются в работах [1, 2].

В инженерном контексте задачи оптимальным выбором я считаю т.н. Square Root Central Difference Kalman Filter (SRCDKF). Выбор сигма точек осуществляется согласно центральной разности (в отрицательное направление от линеаризуемой точки и в положительное направление) пропорционально матрице квадратного корня ковариации. При этом не используется разложение Холецкого для матрицы ковариации, которое может быть не стабильным в условиях с присутствием одновременно малых и больших значений в матрице.  

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

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

Исходный код ядра можно скачать:

Ядро SRCDKF

С Теорией пока всё. Теперь практика.

Пример, иллюстрирующий суть ФК

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

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

Установка и подключение приемника GPS компас к ArduPilot квадрокоптера

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

Раз уж я сказал, что задача из реального мира, а реальный мир трехмерный, то мы будем рассматривать математическую модель описания вращения и положения в трехмерном пространстве. Для описания вращения в трехмерном пространстве воспользуемся кватернионом, можно сказать стандартом для таких случаев. А для описания положения – обычным 3-х мерным вектором.

Q=[q_w q_x q_y q_z]^T\ \ P=[p_x p_y p_z]^T

Кто работал с дешевыми MEMS IMU знает о необходимости калибровки для нивелирования масштабных искажений, межосевых взаимосвязей, а также смещений (biases).

Так выглядят данные с акселерометра, при вращении по всем осям. Центр эллипсоида смещен на некоторый вектор (bias). Сам эллипсоид масштабно искажен.
Так выглядят данные с акселерометра, при вращении по всем осям. Центр эллипсоида смещен на некоторый вектор (bias). Сам эллипсоид масштабно искажен.

Упрощенная, но достаточная для нашей точности математическая модель (без масштабных и межосевых коэффициентов) акселерометра и ДУСа одинаковая и имеет вид:

A_{raw}=A_{true} A_{bias}\W_{raw}=W_{true} W_{bias}

Где A_{raw}  W_{raw}– 3-х мерный вектор данных непосредственно с сенсора (ускорения и угловой скорости соответственно), A_{true}  W_{true} – истинный 3-х мерный вектор данных, A_{bias}  W_{bias}– 3-х мерный вектор смещения (bias).

Модель IMU у нас есть.

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

Для более-менее достаточной для нашей точности математической модели нужно 3 параметра для описания смещения (bias) акселерометра (в СК IMU), 3 параметра для описания смещения (bias) датчика угловой скорости (в СК IMU), 3 параметра описания положения (3-х мерный вектор в глобальной СК), 4 параметра для описания кватерниона ориентации ( кватернион, выражающий вектор из СК коптера в глобальную СК), дополнительно включим 3-х мерный вектор скорости (в глобальной СК). Итого 16 параметров. Это называется вектор состояния системы, так как он описывает полное состояние (в контексте нашей задачи) нашего квадрокоптера в конкретный момент времени, то есть какое он имеет положение, ориентацию, скорость и т.д.

X=[A_{bias}^T  W_{bias}^T  P^T  Q^T  V^T]^TA_{bias}=[a_{bx}  a_{by}  a_{bz}]^T\W_{bias}=[w_{bx}  w_{by}  w_{bz}]^T\Q=[q_w q_x q_y q_z]^T\ \ P=[p_x p_y p_z]^T\V=[v_x  v_y  v_z]^TУстановка и подключение приемника GPS компас к ArduPilot квадрокоптера

Эволюция системы описывается тем, как будет меняться вектор состояния во времени, то есть зависимость нынешнего момента времени от прошлого, еще этот этап называется прогнозом. (t) – текущий момент времени, (t-1) – прошлый момент времени.

P(t)=P(t-1) V(t-1)dt

Здесь просто интегрируется по времени положение по скорости dt– шаг дискретизации.

A_{temp}=Q(t-1)*(A_{raw}-A_{bias}(t-1))\V(t)=V(t-1) A_{temp}dt S_{v_{vel}}

A_{temp}– временный вектор ускорений, состоящий из A_{raw}– вектора непосредственно из акселерометра A_{bias}(t-1)– смещение акселерометра в прошлый прогноз, * – операция вращения вектора кватернионом, в данном случае выражает локальный вектора ускорения в глобальную СК. Q(t-1) – кватернион в прошлый прогноз.

S_{v_{vel}} – вектор стандартных отклонений шумов прогноза для линейной скорости.

W_{temp}=W_{raw}-W_{bias}(t-1) S_{w_{rate}}dt\Q(t)=Q(t-1)otimes textrm{exp}(W_{temp}dt)

W_{temp}– вектор угловой скорости, состоящий из W_{raw}– вектора данных непосредственно с ДУСа, W_{bias}(t-1)– рассчитанный на предыдущем шаге вектор смещения, S_{w_{bias}}– вектор стандартных отклонений шумов прогноза для угловой скорости.

otimes– символ, обозначающий перемножение кватернионов, exp()– это функция перевода 3-х мерного вектора вращения, находящегося в касательной плоскости относительно сферы кватерниона в пространство кватерниона, подробнее распишу попозже.

A_{bias}(t) = A_{bias}(t-1) S_{a_{bias}}dt\W_{bias}(t) = W_{bias}(t-1) S_{w_{bias}}dt

Смещения IMU не зависят от других переменных в векторе состояния, поэтому в нынешнем моменте времени они почти равны прошлым значениям. Почти, потому что все-таки они немного меняются со временем (температура может меняться), нужно чтобы ФК понимал, что это не константы и имел некоторую неопределенность в значении, для этого есть два параметра S_{a_{bias}}  S_{g_{bias}} – которые являются векторами стандартных отклонений шумов прогноза для смещений IMU.

Код этапа прогноза
void TimeUpdate(float *in, float *noise, float *out, float *u, float dt, int n) {
	float tmp[3], acc[3];
	float rate[3];
	float mat3x3[3 * 3];
	float q[4];
	int i;
	float qRate[4], qRes[4];

	// assume out == in
	out = in;

	for (i = 0; i < n; i  ) 
	{
		// create rot matrix from current quat
		q[0] = in[UKF_STATE_Q1*n   i];
		q[1] = in[UKF_STATE_Q2*n   i];
		q[2] = in[UKF_STATE_Q3*n   i];
		q[3] = in[UKF_STATE_Q4*n   i];
		quatToMatrix(mat3x3, q, 1);

		// pos
		out[UKF_STATE_POSX*n   i] = in[UKF_STATE_POSX*n   i]   in[UKF_STATE_VELX*n   i] * dt;
		out[UKF_STATE_POSY*n   i] = in[UKF_STATE_POSY*n   i]   in[UKF_STATE_VELY*n   i] * dt;
		out[UKF_STATE_POSZ*n   i] = in[UKF_STATE_POSZ*n   i]   in[UKF_STATE_VELZ*n   i] * dt;

		// acc
		tmp[0] = u[0]   in[UKF_STATE_ACC_BIAS_X*n   i];
		tmp[1] = u[1]   in[UKF_STATE_ACC_BIAS_Y*n   i];
		tmp[2] = u[2]   in[UKF_STATE_ACC_BIAS_Z*n   i];

		// rotate acc to world frame
		rotateVecByMatrix(acc, tmp, mat3x3);
		acc[1] -= GRAVITY;


		out[UKF_STATE_VELX*n   i] = in[UKF_STATE_VELX*n   i]   acc[0] * dt   noise[UKF_V_NOISE_VEL_X*n   i];
		out[UKF_STATE_VELY*n   i] = in[UKF_STATE_VELY*n   i]   acc[1] * dt   noise[UKF_V_NOISE_VEL_Y*n   i];
		out[UKF_STATE_VELZ*n   i] = in[UKF_STATE_VELZ*n   i]   acc[2] * dt   noise[UKF_V_NOISE_VEL_Z*n   i];


		// rate = rate   bias   noise
		rate[0] = (u[3]   in[UKF_STATE_GYO_BIAS_X*n   i]   noise[UKF_V_NOISE_RATE_X*n   i]) * dt;
		rate[1] = (u[4]   in[UKF_STATE_GYO_BIAS_Y*n   i]   noise[UKF_V_NOISE_RATE_Y*n   i]) * dt;
		rate[2] = (u[5]   in[UKF_STATE_GYO_BIAS_Z*n   i]   noise[UKF_V_NOISE_RATE_Z*n   i]) * dt;

		// rotate quat
		exp_mapQuat(rate, qRate);
		quatMultiply(qRes, q, qRate);

		out[UKF_STATE_Q1*n   i] = qRes[0];
		out[UKF_STATE_Q2*n   i] = qRes[1];
		out[UKF_STATE_Q3*n   i] = qRes[2];
		out[UKF_STATE_Q4*n   i] = qRes[3];

		// acc bias
		out[UKF_STATE_ACC_BIAS_X*n   i] = in[UKF_STATE_ACC_BIAS_X*n   i]   noise[UKF_V_NOISE_ACC_BIAS_X*n   i] * dt;
		out[UKF_STATE_ACC_BIAS_Y*n   i] = in[UKF_STATE_ACC_BIAS_Y*n   i]   noise[UKF_V_NOISE_ACC_BIAS_Y*n   i] * dt;
		out[UKF_STATE_ACC_BIAS_Z*n   i] = in[UKF_STATE_ACC_BIAS_Z*n   i]   noise[UKF_V_NOISE_ACC_BIAS_Z*n   i] * dt;

		// gbias
		out[UKF_STATE_GYO_BIAS_X*n   i] = in[UKF_STATE_GYO_BIAS_X*n   i]   noise[UKF_V_NOISE_GYO_BIAS_X*n   i] * dt;
		out[UKF_STATE_GYO_BIAS_Y*n   i] = in[UKF_STATE_GYO_BIAS_Y*n   i]   noise[UKF_V_NOISE_GYO_BIAS_Y*n   i] * dt;
		out[UKF_STATE_GYO_BIAS_Z*n   i] = in[UKF_STATE_GYO_BIAS_Z*n   i]   noise[UKF_V_NOISE_GYO_BIAS_Z*n   i] * dt;
	}
}

Пару слов о векторах стандартных отклонений шумов прогноза. Чем больше будут эти значения шумов, тем меньше доверие прогнозу, неопределенность соответствующей переменной вектора состояния будет расти с каждой итерацией прогноза и этап коррекции будет вносить существенную корректировку в них. То есть эта переменная будет подвержена большой волатильности. ФК будет пользоваться данной “крутилкой” больше, чем теми, у которых малое значение стандартного отклонения шума.

Также стоит отметить, что в отличии от других видов ФК модель шума (как процесса, так и датчика) может быть не аддитивной, и данный вид ФК сможет учесть и нелинейную природу шума. Это преимущество обеспечено способом линеаризации с помощью сигма-точек, которые пропускаются в том, числе и через модель шума.

По поводу textrm{exp()}. Кватернион – это гиперпараметризированная модель вращения, так как имеет 4 параметра в пространстве вращения с 3 степенями свободы. Так как полноценно описать вращение 3 параметрами не получается (Gimbal lock), приходится прибегать к таким математическим конструкциям, как кватернионы. Для перехода из 4-х мерного пространства в 3-х мерное пространство и обратно есть специальные операции textrm{exp()}  textrm{log()}. Это можно представить как сферу (кватернион) и касательную плоскость (3-х мерный вектор вращения).

Установка и подключение приемника GPS компас к ArduPilot квадрокоптера

Отсюда же проистекает проблема, которую я описывал выше (ФК не умеет работать с параметрами, у которых есть ограничения), у кватерниона должна быть единичная длина. В таком случае можно пойти на небольшую хитрость и после каждой итерации прогноза нормализовывать кватернион. Для нашей мат. модели такой способ сгодится, однако для более нелинейных систем, придется использовать специальную арифметику и операции с кватернионами внутри ФК. Подробнее про касательные пространства и фильтрацию с кватернионами в [3, 4].

В нашем случае есть только 2 датчика IMU и GNSS. IMU – используется непосредственно в модели прогноза как априорные значения и не моделируются.

GNSS моделируется так:

P_{gnss}=Q*P_{loc} P S_{p}

Q – кватернион из вектора состояния, P_{loc}– вектор из IMU в GNSS в СК IMU, P– вектор позиции из вектора состояния, S_p– вектор стандартного отклонения шума GNSS.

Чем выше S_p тем меньше доверие GNSS, данную величину можно варьировать в зависимости от качества данных с датчика.

Код, модели GNSS

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

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

Итак, для демонстрации данного ФК был написана демка на Unity.

Кратко расскажу подробности симулятора:

Сам ФК в Unity используется как динамическая библиотека, для WebGL – сырые исходники. Коптер не управляется по физическим законам, используются просто силы и моменты для имитации полета коптера в скрипте Control.cs. Для имитации IMU и GNSS созданы 2 скрипта IMUModel.cs и GNSSModel.cs. Обертка и взаимодействие с Unity ФК реализована в KalmanFilter.cs. Для графиков использовался открытый Asset.

Для физики используется внутренний движок PhysX. Для демонстрации 60 Гц – достаточно.

Вот видео демки и графики вектора состояния.

Вектор смещений ДУСа
Вектор смещений ДУСа
Вектор смещений акселерометра
Вектор смещений акселерометра
График углов Эйлера
График углов Эйлера
Вектор положения
Вектор положения
Вектор скоростей
Вектор скоростей

Пояснение по графикам: Estimate – это оценка ФК, Reference – это фактическая величина, Uncertainty – дисперсия из матрицы ковариации *100.

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

Демку, что на видео можно скачать из репа:

Исходный код демки

А также можно поиграть интерактивно.

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

В случае применения ФК и данной мат. модели, такого не будет: при наличии движения с помощью датчика GNSS будет происходить разделение сырого вектора ускорения из акселерометра на линейную часть и на вектор ускорения свободного падения, из-за чего углы тангажа и крена будут максимально точными, а угол курса не будет уплывать. К тому же мы получаем еще и более точные векторы положения и скорости. Данную особенность приметил не только я:

В этом и заключается преимущество ФК – нахождении неявных полезных обратных зависимостей между вектором состояния и датчиками.

Вот график и видео иллюстрирующие коррекцию курсового угла, даже при начальном неправильном значении в 45 градусов.

Угол курса с начальной ошибкой в 45 градусов
Угол курса с начальной ошибкой в 45 градусов

В интерактивной демке, много чего нет, с чем можно поиграть непосредственно в Unity Edior, а именно: различные значения bias IMU, их изменения во времени, различные величины стандартных отклонений шумов прогноза и датчика GNSS, задержки между датчиками, неверные начальные условия и т.д. Так что советую скачать исходник и непосредственно в Unity Editor поиграть с различными условиями.

Самое полезное, это то, что тот же самый C код, который работает в демке (kalman lib), можно целиком без переделок запустить на микроконтроллере, на реальном железе в реальных условиях.

Вот например коптер, который я собрал, который летает почти с таким же ФК:

Суть ФК можно расширить на оптимизацию не только текущего вектора состояния в конкретный момент времени, но и оптимизацию, например, целого набора состояний, включенных в разные моменты времени (Sliding-Window Optimization, bundle adjustment/graph optimization). Такой подход требует гораздо большей вычислительной мощности и реализуется другими методами, однако гораздо более точен и стабилен.

За рамками статьи остались такие темы, как:

Если вам будет интересно можно продолжить цикл статей по практическому применению ФК.

Список литературы:

  1. Van Der Merwe, R. Sigma-point Kalman filters for probabilistic inference in dynamic state-space models

  2. Chaabane, M. Nonlinear State and Parameter Estimation Using Iterated Sigma Point Kalman Filter: Comparative Studies

  3. Christoph Hertzberga, Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds

  4. Joan Sol`, Quaternion kinematics for the error-state Kalman filter

Оцените статью
Радиокоптер.ру
Добавить комментарий