Фильтр Калмана: разбор навигационной системы БПЛА исходный код / Хабр

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Согласно целям статьи, понизить порог вхождения в тему, дать универсальный алгоритм для быстрого прототипирования и проверки гипотез в условиях слабой вычислительной базы и малого кол-ва памяти лучшим выбором будет являться второй вид ФК – 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), в нашем примере нету магнитометра и барометра, хотя это не принципиально.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Смещения IMU не зависят от других переменных в векторе состояния, поэтому в нынешнем моменте времени они почти равны прошлым значениям. Почти, потому что все-таки они немного меняются со временем (температура может меняться), нужно чтобы ФК понимал, что это не константы и имел некоторую неопределенность в значении, для этого есть два параметра – которые являются векторами стандартных отклонений шумов прогноза для смещений 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;
	}
}

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

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

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

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

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

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

– кватернион из вектора состояния, – вектор из IMU в GNSS в СК IMU, – вектор позиции из вектора состояния, – вектор стандартного отклонения шума 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 градусов.

В интерактивной демке, много чего нет, с чем можно поиграть непосредственно в 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