Создание собственной системы стабилизации
this offset learning algorithm is inspired by this paper from Bill Premerlani
это уже яйца мамонта, еще на апм1 коптера небыло, а аглоритм был в коде самолета
это уже яйца мамонта, еще на апм1 коптера небыло, а аглоритм был в коде самолета
Так и фильтру Калмана тоже уже пол-века 😁
Да, запустил всё ж я всю эту систему (изрядно измучив себе мозг),
И как оказалось, с ошибками ))…, а в действительности - крутая штука (уж лучше Магвика точно), и вот тому подтверждение:
всё работает как на видео, надо ещё адаптивный коэфициент попробовать… короче рекомендую…
бездоказательное видео имхо. тот же самый фильтр Махони даст такие же результаты, если коррекцию по акселерометру сделать большой, а по магнитометру маленькой.Сконцентрировались на неактульных проблемах в виде слишком больших угловых ускорениях, которых не бывыает в реальных ЛА, а если уж так хочется, то эта проблема легко решается правильной частотй опроса и фильтрацией (в том же MPU надо очень быстро крутануть, чтобы достичь такого же эффекта как на видео). В видео умышленно не показано долгое линейное или центростремительное ускорение, потому что их фильтр бы съехал, ввиду слишком сильной коррекции акселерометра. Коррекцию магнитометра только оси Z - уже миллион и один раз используется везде. И вообще ощущение, что специально сравнивают с плохо настроенными для данного (не актулаьного) случая другими алгоритмами.
тот же самый фильтр Махони даст такие же результаты, если коррекцию по акселерометру сделать большой, а по магнитометру маленькой
У него (Махони) всего две настройки - Kp и Ki (вторая, как правило, не используется), а тут есть и пороги для коррекции “биасов” и раздельный Kp для акселя и мага, так что свободы уж точно больше…
не показано долгое линейное или центростремительное ускорение,
надо проверить как динамический коэф. усиления работает, а там видно будет…
Если оставить только Kp то весь фильтр выродится в обычный альфа-бета комплиментарник, так что Ki используется. Kp надо естественно раздельный для акселя и для мага, тут иначе нельзя. Несколько сообщений назад Алексей Москаленко писал
Да ничего там нового нет, все тот же комплементарник только в замороченной форме кватернионов (со всеми вытекающими математическими тонкостями через которые придется продираться). Динамический коэффициент на акселе по анализу длины вектора гравитации - идея старая, но не рабочая, хороша только для тестовых отчетов, но в реальности его нужно применять очень акуратно, особенно если система постоянно в движении (я так и не получил заметных улучшений).
Тут я с ним согласен, это обычный комплиментарник, динамический коэффициент не рабочая идея.
У него (Махони) всего две настройки - Kp и Ki (вторая, как правило, не используется), а тут есть и пороги для коррекции “биасов” и раздельный Kp для акселя и мага, так что свободы уж точно больше…
Ki у Махони как раз для коррекции байасов гироскопа, если его грамотно использовать все работает идеально даже в условиях температурного дрейфа гироскопа.
надо проверить как динамический коэф. усиления работает, а там видно будет…
Динамический Kp ИМХО идея дохлая, пробовал, заметных улучшений нет.
Динамический Kp ИМХО идея дохлая, пробовал, заметных улучшений нет.
Не уверен, тут весьма важны правильные критерии этого самого подбора Kp.
всё работает как на видео
неправильный на видео кальман
в ардукоптере 3.3 и старше если подносишь магнит то компас исключается из решения задачи и не влияет на ориентацию
тут весьма важны правильные критерии
Согласен полностью …, например в данном случае авторы берут текущую длину магнитного вектора (?) и сравнивают ее с длиной вектора гравитации (?)… типа так: error=(m_vect - g_Kvect)/g_Kvect и по этой ошибке уже изменяют Kp…
У меня непонятки полные пока на этот счет, g_Kvect у них = 9.8 м/сек указан (константа), ну и как можно (а главное зачем) сравнивать несравнимые вещи ?
Что это за критерий такой, не пойму, может кто просвятит ?
Разобрался в некоторой степени со знаменитым фильтром Калмана.
Я бы выделил несколько ключевых “фокусов” алгоритма.
Магия Калмана заключается в нахождении нескольких матриц ковариации (они завуалированы в выражениях стандартного Калмана).
Первая считает ковариацию( меру похожести или линейной зависимости между двумя величинами) между переменными вектора состояния. А это приводит к очень интересной магии, а именно, если задать некоторые зависимости(можно довольно сложные) между переменными, фильтр их статистически обнаружит и использует для своих корыстных целей (минимизации квадрата ошибки вектора состояния). Простой пример - Вектор состояния (X) состоит из позиции и скорости, матрица преобразования (A) преобразует вектор состояния таким образом - скорость сейчас равна скорости в прошлый момент времени, а позиция равна позиции в прошлый момент времени ПЛЮС скорость. Наблюдая только позицию можно предсказать скорость!!! то есть мы нигде не писали, что скорость - это дифференциал по времени, фильтр выяснил это СТАТИСТИЧЕСКИ. В перспективе фильтр может оценить очень сложные аналитические зависимости, тем самым делая работу за разработчика. Главное связать переменные. То есть фильтр может оценивать как наблюдаемые переменные, так и ненаблюдаемые!!
Вторая матрица ковариации оценивает зависимость между вектором состояния и показаниями датчика . По сути она определяет коэффициент Калмана. Но у нее есть одно неочевидное свойство - отбраковывать левые показания! то есть если датчик показывает какие-то левые данные, ковариация между ними и вектором состояния уменьшится и коэффициент соответственно тоже уменьшится.
В итоге имеем то, что у нас есть автоматически адаптивный коэффициент влияния и неявные зависимости между переменными.
Последнее свойство кстати используется для определения параметров (parametr estimation), а это наталкивает на определенные мысли о адаптивном управлении, где система управляет объектом, определяя его параметры динамически, соответственно подстраиваясь под изменения (Dual estimation Kalman filter).
Теперь самое интересное - почти всегда вместо матриц A и H используются нелинейные уравнения, что приводит к тому что их приходится линеаризовывать каждый раз в окрестности данного вектора состояния, чтобы определить искомые оценки ковариаций и мат. ожиданий. Линеаризация обычно происходит разложенением в ряд Тейлора, по просту дифференцированием. Это порождает кучу проблем (не все можно дифференцировать, точность не всегда достаточно) которые приводят к неустойчивости фильтра, вследствие заниженной оценки шума. Так вот придумали новый метод оценки ковариации и мат ожидания вектора состояния с нелинейными уравнениями, называется UT (Uncsented transform). Кратко - мы просто берем несколько точек в окрестности нашей оценки и прогоняем через нелинейные уравнения, находим ковариацию и мат ожидание. Метод очень крутой, используется в UKF(Uncsented Kalman Filter) устойчивость выше чем у EKF.
Собственно, скоро выложу результаты пробы UKF, результаты и выводы мне показались довольно интересными, своеобразными.
P.S. Если где то ошибся - поправляйте и дополняйте, тема очень интересная
Разобрался в некоторой степени со знаменитым фильтром Калмана.
Здорово! Может нам объединить усилия над проектом INAV?
Собственно, скоро выложу результаты пробы
А замысел то какой (касаемо нашей темы) ? в смысле - что наблюдаем , что оцениваем, что регулируем, или чего хотим добиться.?
Разобрался в некоторой степени со знаменитым фильтром Калмана.
А по каким материалам или учебникам разбирались?
Здорово! Может нам объединить усилия над проектом INAV?
Что за проект? На какой стадии? Какие цели проекта? Над чем в данный момент работаете?
А замысел то какой (касаемо нашей темы) ? в смысле - что наблюдаем , что оцениваем, что регулируем, или чего хотим добиться.?
Все то же, получение наиболее точных данных координат коптера из искаженных, зашумленных показаний датчиков.
Простой пример - Вектор состояния (X) состоит из позиции и скорости, матрица преобразования (A) преобразует вектор состояния таким образом - скорость сейчас равна скорости в прошлый момент времени, а позиция равна позиции в прошлый момент времени ПЛЮС скорость. Наблюдая только позицию можно предсказать скорость!!!
помнится - там получается всего навсего интеграл от прошлой скорости - рассчётная позиция - матрица А - предположение (предсказание)… магия не в этом месте…
А по каким материалам или учебникам разбирались?
советую искать не научные публикации, а лекционные материалы для студентов, презентации в основном. После понимания основ, можно почитать публикации касательно конкретных применений, сформировать картину как оно на самом деле. Далее почитать уже конкретно о EKF, UKF. Параллельно изучая код с конкретными реализациями. Ну и попробывать самому. Сразу скажу изучать придется буржуйскую литературу. (На хабре не стоит читать имхо)
Вообщем то у меня сформировался примерно такой список по мере осваивания:
- ru.wikipedia.org/wiki/Фильтр_Калмана
- Учебный курс на Udacity.com: Artificial Intelligence for Robotics
- Introduction to Estimation and the Kalman Filter Hugh Durrant-Whyte Australian Centre for Field Robotics The University of Sydney - там прямо все по полочкам разложили.
- Касательно UKF - The Unscented Kalman Filter Barry Sherlock University of North Carolina - все очень подробно.
- we.easyelectronics.ru/…/sigma-tochechnyy-filtr-kal…
6)Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models Rudolph van der Merwe & Eric Wan - конкретные применения UKF - Проект AutoQuad - очень все по-умному сделано, основательный проработанный подход. Открытый код. Собственно код я взял оттуда, само ядро фильтра универсальное, вам как разработчикам нужно только проинициализоровать, задать матрицы шумов, написать функции f(x), h(x).
- и всякий раз гуглить, что то непонятно - гуглить. гуглить, гулить и еще раз гуглить. Тут надо определить границу, где можно самому сделать работу, а где взять готовое и не изобретать велосипед, ведь все уже до нас изобрели, надо только поискать (ИМХО).
помнится - там получается всего навсего интеграл от прошлой скорости - рассчётная позиция - матрица А - предположение (предсказание)… магия не в этом месте…
Я вас не понял вообще.
Я вас не понял вообще.
Матрица экстраполяции - по сути дела большой интеграл - там интегрируется всё, что только можно - т.е. найти углы из угловых скоростей, скорости из ускорений, путь из скорости и т.д. …
далее мы получаем новые данные и их и по ним корректируем рассчётные данные, а так же исходя из разницы рассчётных и полученных данных - корректируем коэффициенты для следующей итерации…
магия не в ней я ж четко написал:
Магия Калмана заключается в нахождении нескольких матриц ковариации
Магия заключается в матрице ковариации между переменными вектора состояния Pxx, а не в матрице A( другое обозначение F) (вы говорите именно про A). Именно матрица ковариации может оценить ненаблюдаемую скорость из наблюдаемой позиции.
Магия заключается в матрице ковариации между переменными вектора состояния Pxx, а не в матрице A( другое обозначение F)
ну нет там никакой магии - посмотрите из чего она состоит - единицы по главной диагонали и дельта т - выше неё - и вы её умножаете на ваш вектор из прошлых показаний…
ваши слова:
Простой пример - Вектор состояния (X) состоит из позиции и скорости, матрица преобразования (A) преобразует вектор состояния таким образом - скорость сейчас равна скорости в прошлый момент времени, а позиция равна позиции в прошлый момент времени ПЛЮС скорость. Наблюдая только позицию можно предсказать скорость!!!
да не спорю, скорость можно найти но не дифференцируя путь, а интегрируя ускорение…
магия заключается именно в подборе коэффициентов усиления - а вот тут всё плохо…
вот где самое неудобное место:
Нет, вы меня не поняли, у нас нет ускорения, в моем примере у нас есть только две переменные - позиция и скорость. Позицию мы измеряем датчиком - это наблюдаемая переменная, а скорость - ненаблюдаемая, фильтр оценивает ее основываясь на матрицу ковариации.
ну нет там никакой магии - посмотрите из чего она состоит - единицы по главной диагонали и дельта т - выше неё - и вы её умножаете на ваш вектор из прошлых показаний…
это не та матрица.
Цитата из вики:
\textbf{P}_{k|k} — апостериорная ковариационная матрица ошибок, задающая оценку точности полученной оценки вектора состояния и включающая в себя оценку дисперсий погрешности вычисленного состояния и ковариации, показывающие выявленные взаимосвязи между параметрами состояния системы
ru.wikipedia.org/wiki/Фильтр_Калмана
в матрице ковариации - по диагонали содержаться дисперсии самих переменных вектора состояния, а в нижнем и верхнем треуголнике (матрица симметрична) ковариации(попросту коэффициенты похожести, влияния) одной переменной вектора состояния на другую.
Коэффициент калмана подбирается в зависимости от матрицы ковариации. По простому - чем больше одна величина похожу на другую, тем больший коэффициент наблюдаемая переменная будет оказывать на ненаблюдаемую.