flybrain. передатчик + приемник + автопилот. powered by stm32
Пока буду реализовывать EKF, чтобы
Готовый EKF для Кортекса можно взять у OpenPilotа. Советую так-же посмотреть Мардж.
вот от сюда что ли?
link
Я не пойму что там за язык. matlb что ли?
В любом случае, у меня уже заработал EKF. Ща исследовать буду.
вот от сюда что ли? link
Нет, я имел в виду Си-шные исходники. Открытые и скачиваемые через Git: wiki.openpilot.org/display/…/Building+on+Windows
В любом случае, у меня уже заработал EKF
Тогда - удачи!
Но даже сейчас Roll и Pitch вполне себе хорошо правду показывают, если без фанатичных перегрузок платформу механически трясти
не покажете?
Нет, я имел в виду Си-шные исходники.
Ой! Как там все мутно 😦
Реализация на кватернионах с непонятной функцией предсказания. Я пока кватернионы не реализовал, сейчас чисто на углах Эйлера работает, и работает отменно. Может потом этим займусь, сейчас визуализацию сравнения EKF и DCM делаю.
не покажете?
Покажу. Как раз пишу софтину, чтобы два уровня горизонта на экране монитора сравнить. Если сегодня закончу, дома засниму видео ролик, покажу.
Если сегодня закончу, дома засниму видео ролик, покажу.
Жду посмотреть.
вот первое видео моего EKF работает только гира и только аксель. Время одного цикла EKF занимает примерно 50мкс Вот для сравнения алгоритм DCM
Любопытно. Не ожидал такой разницы от ЕКF, без комплексирования с GPS/компас.
Есть еще алгоритм Мардж: varesano.net/…/initial-implementation-9-domdof-mar…
Предлагаю определиться с терминологией. “алгоритм DCM” - что под этим подразумевается? DCM в общем виде, как и кватернионы - описание поворота. Разницы по принципу применения- нет, и тот и другой метод описывает поворот, только вычислительные затраты немного отличаются, и точность при одинаковой разрядности, но в нашем случае точности метода не принципиальна- неидеальность датчиков сильно “перевешивает”.
Следует , вероятно говорить о методе коррекции гироскопов.
В случае " EKF" также, вероятно, имеет место какое- либо описание поворота на основе интегрирования угловых скоростей, и какой-то метод коррекции, адаптивный в зависимости от величины угловых скоростей и ускорений и предыдущего состояния системы.
“Алгоритм DCM” также имеет метод описания поворота ( очевидно, DCM) и метод коррекции. Обычно в качестве корректирующего метода применяют ПИ регулятор.
Опишите, что именно вы используете в разных случаях (если представляете, как работают примененные вами алгоритмы). Тогда можно что- либо посоветовать, да и остальным участникам будет , надеюсь, понятнее.
По поведению платформы в случае “алгоритма DCM” общем виде видно, что при околонулевых угловых скоростях имеет место колебательный затухающий процесс. То есть система коррекции имеет колебательный характер (причем декремент затухания слишком мал), а желательно иметь апериодический. Также кажется, что при движении вправо- влево платы происходит “зашкаливание” акселерометров.
примерно 50мкс
Так мало не может быть.
20кгц екстендед калмана не бывает, если только на гигагерцовых процах.
20кгц екстендед калмана не бывает
50мкс это время затрачиваемое процом на одну итерацию по кальману.
А частота расчета равно 400Hz, то есть 1 в 1, то что сейчас гира выдает
аксель выдает на частоте 50Гц
работает только гира и только аксель. Время одного цикла EKF занимает примерно 50мкс
Не пойму почему картинка не дрейфует вокруг вертикальной оси ? (магнитометр ведь не подключен)
Или она “отключена” и остались (отслеживаются) только два угла ?
В EKF две переменных и шесть наблюдаемых, так?
В случае " EKF" также, вероятно
Не, там не так все просто. Там есть матрица предсказывающего физического поведения модели на следующем шаге исходя из предыдущей историии, есть матрица предсказания ошибки, которая так же в процессе накопления статистики обучается. Процесс обучения четко заметен в самом конце, когда после нереальных расколбасов по всем осям, я резко ставлю плату в горизонт.
Оно встает с ошибкой градусов на 15, потом медленно ползет к нулю. Это происходит потому, что у алгоритм тупеет в этот момент так как обученная матрица ошибок вываливается за границы предсказаний физической модели, а гироскоп показывает нули по всем осям. Алгоритм ошалело пытается понять что происходит, а информации ноль. Соответственно необходимо время для переобучения, что и видно. А если в этот момент слегка покачать платформу (даже едва заметно), оно тут же приходит в себя, потому что поперла различная инфа с гироскопа.
Алгоритм DCM
Вот здесь, все так. Есть просто матрица вращения и алгоритм принятия решения корректироваться по акселю или нет. А если да, то насколько верить.
DCM у меня тупо из мегапирата содран. И вообщем-то, если использовать без фанатизма для спокойного полета блинчиком, вполне себе работает. Видео это подтверждает.
Или она “отключена”
да, я специально отключил. Какой в ней смысл, если компас отключен. Есть там небольшой дрейф, вполне ожидаемый. Как научусь компас калибровать, так включу.
В EKF две переменных и шесть наблюдаемых, так
сейчас да, скоро добавлю компас, будет 9 параметов. Хотя, даже сейчас уже практически все шоколадно и с этим уже можно дальше двигаться.
Насчет времени итерации я имел ввиду, что судя по посту в тему мегапирата на дцм уходит 19 мкс, а на калмана всего в 2.2 раза больше. Нормальный фильтр калмана гораздо больших вычислительных ресурсов требует чем дцм и точно не в 2 раза.
Как научусь компас калибровать, так включу.
Для начала можно так: (max-min)/2 - это ноль, так по каждой оси.
И оси выровнять друг с другом - подбираем k_x, так, чтобы d_x=k_x*(max_x-min_x) равны для всех осей (т.е. d_x=d_y=d_z).
фильтр калмана гораздо больших вычислительных ресурсов требует чем дцм и точно не в 2 раза.
Он тут пока очень маленький, 2 оси. С маг-ом время увеличится вдвое (примерно квадрат от кол-ва переменных).
Что не в два раза - это точно… Хотя, например можно пересчитывать ковариционную матрицу ( " обучать" 😃 как важно сказал Алекс,) с частой отсчетов по акселерометрам? быстрее-то информация не приходит, угловые ускорения у нас не так велики.
Также , по времени надо помнить что М4 имеет аппаратный арифметический блок для операций с плавающей точкой, и сильно шустрее в этом применении.
Также, есть еще один способ комплексироания данных - комплементарный фильтр (CF) (описан у Махони, кстати)…
На Ютубе есть замечательное видео под бодрую музыку сравнения калмана, расширенного калмана, и CF.
Хотя, например можно пересчитывать ковариционную матрицу
Верно, например на 10 предикторов 1 корректирующий шаг выполнить.
комплементарный фильтр (CF)
Кстати почти те-же результаты дает что и екф, но раз в 10 быстрее выполняется. Для ардуины идеален.
Что не в два раза - это точно…
Почему? Как считать?
Было 2 оси, матрица F 2х2 =4, будет 3оси 3x3=9, время подсчетов увеличится в 9/4=2 раза.
ПРИМЕРНО, без учета усложнения модели.
Сорри, похоже это был ответ не мне 😃, ну пусть остается, как пояснение к подсчетам.
В неэкстремальных условиях, проведенные эксперименты показали полностью идентичные углы во все моменты времени.
Почему? Как считать?
дсм против калмана сравниваем
TO: Flash_rahЕсли хотите, чтобы я отвечал на личные сообщения, то разрешите их у себя в профиле. Мне ваши личные настройки не позволяют отвечать вам.
19 мкс, а на калмана всего в 2.2
Ну и что? Кальман у меня оптимизирован, а DCM у Олега на темплейтах. Там очень много передачи параметров не по ссылкам а копированием происходит. Не факт, что это не занимает львиную долю времени в тех 18мкс.
Для начала можно так:
Ноль действительно очень далеко от того, где он должен быть. Поэтому чтобы пока не замарачиваться, я просто не использую. Найти середину - это вариант, но я думал выполнить те дейсвия, который а апноутах написаны, хотя может это и лишнее. Наверно сделаю по быстрому так, как ты предлагаешь. Для тестов сойдет, а там посмотрим.
С маг-ом время увеличится вдвое
Ну не факт. Думаю его только как корректирующий фактор ввести в дополнение к гире. Тогда только на 1/3 должно увеличится. И я планирую магнитометр на 30Гц пускать. Уж больно эта штука внешним воздействиям подвержена, не было бы хуже.
и сильно шустрее в этом применении.
вот тут все правда. Я пробовал компилять без FPU инструкций. Скорость упала больше чем в 10 раз. Нет, уж нафиг…
Также, есть еще один способ комплексироания данных
Ну, если кто реализацию выложит готовую, я готов добавить и сравнить и доложить о результатах. Но сейчас мне бы хотелось дальше двигаться.
Еще вот думаю частоту акселя поднять до 400Hz и посмотреть что получится.
Верно, например на 10 предикторов 1 корректирующий шаг выполнить.
Ну оно сейчас так и получается. Или, если быть более точным, то дрейф акселя фильтруется FIR фильтром, который подстраивается акселем, но аксель работает на частоте 50Hz, соответственно имеем побочную адаптивную FIR фильтрацию дополнительно, неявно наложенную на гиру. Причем за счет отрицательной обратной связи они на самом деле друг на друга влияют
Было 2 оси
Сейчас реально считаются 3 оси по полной программе, я только на визуалке вырубил.
полностью идентичные углы во все моменты времени
Тут да, согласен. Оба варианта вполне себе рабочие. Конечно калман более четкий и существенно меньше подвержен вибрациям и паразитным ускорениям. Но, опять же наша цель 3D пилотаж или полет в плоскости горизонта с минимальными кренами?
Сейчас с кальманом осталось по сути 2 технические проблемы решить.
- Как задавать начальное референсное значение платформы. Засада в том, что при подаче питания оно не знает где у него верх, где право, лево, вперед, назад. Поэтому, куда гиру при старте повело, туда оно ведет положительный отсчет углов. Даже вот не знаю, наверно принудительно не нули при старте в углы писать, а небольшие смещения в доли градусов, тогда оно сообразит плюс или минус изначально. Ну а для самолета как? Вот подали мы питание, как там пользователь платы разместил? Да хрен его знает. Значит надо как то дать пользователю на старте объяснить где “перед”, а где “назад” ну и лево право за одно. То есть вариант plug-and-fly как-то не очень просматривается.
- Отловить сингулярность, которая возникает скорее всего при переходе от матрицы к углам эйлера. Деление на ноль, или тангенс какой в 90 градусов упирается. Либо я чего-то не учел, либо оно так и должно быть. Но надо куда-то IFов на эту тему воткнуть.
Как задавать начальное референсное значение платформы
А в чем проблема?
Начальные углы по акселю задавайте. Гиры - по нулям.