Создание собственной системы стабилизации
идея о том что не нужно выполнять коррекцию результируещего Z вектора (т.е. результат интегрировния гир) по магу… у тебя так?
У меня именно так, итоговый кватернион только нормируется после интегрирования. Кватернион содержит предполагаемую (мнимую) ориентацию, по этой ориентации маг разворачивается к мировой СК. Z отбрасывается, Х- косинус,У-синус; У/Х -удельное (нормированное по косинусу) рассогласование Немного не так, весь вектор нормируется до единицы, У - берется как рассогласование мнимого азимута относительно текущего вектора мага, если Х <0, то знак У меняется. Это рассогласование расщепляется на локальные оси и добавляется к смещению соответствующих осей ДУСа. Вот так у меня.
Из моей практики не корректируемый интеграл долго не проживет…
При плавных эволюциях живет вполне прилично (>1 мин), в покое вообще до 10 мин.
При плавных эволюциях живет вполне прилично (>1 мин), в покое вообще до 10 мин.
В демо xsense (
) очень хорошая стабильность, 1.2 градуса за 17 минут при движении и умеренных вибрациях робота-пылесоса.
У меня именно так, итоговый кватернион только нормируется после интегрирования. Кватернион содержит предполагаемую (мнимую) ориентацию, по этой ориентации маг разворачивается к мировой СК. Z отбрасывается, Х- косинус,У-синус; У/Х -удельное (нормированное по косинусу) рассогласование Немного не так, весь вектор нормируется до единицы, У - берется как рассогласование мнимого азимута относительно текущего вектора мага, если Х <0, то знак У меняется. Это рассогласование расщепляется на локальные оси и добавляется к смещению соответствующих осей ДУСа. Вот так у меня.
Саша, это все те же помидоры, лишь вид сбоку 😃
До тех пор пока мы берем разность между расчетным вектором (по гирам) и абсолютным вектором мага и выполняем:
- либо корекцию расчетного/конечного вектора
- либо вносим коррекцию прямиком в биас гир
это будет все тот же компл. фильтр, просто разных типов/порядков… в твоей реализации в покое, после всех возмущений, расчетный вектор приползет к абсолютному…
Ребята же из xsens для коррекции похоже берут диф. мага и теряют абсолютность, т.е. привязку к мировой СК… но тут конечно и недостатки есть 😃
В демо xsense очень хорошая стабильность, 1.2 градуса за 17 минут при движении и умеренных вибрациях робота-пылесоса.
но ты заметь что у них без коррекции ошибка всего 47 град за 17 мин, и при таких то условиях! там явно сенсоры почище будут наших mpu-шек 😃
попробовал вчера отрубить коррекцию по магу, так за минуту уплыл на 30-40 град и это в покое на столе 😃
тут конечно еще целочисленная математика виной…
Простите, но не очень понимаю, что вы подразумеваете под словом “биас”. То есть вы имеете ввиду, что они корректируют угловую скорость гиры через угловую скорость магнитометра? То есть отличие кроется именно в том, что они не не ищут разность между виртуальным вектором севера (проинтегрированные скорости гир) и абсолютным значением севера с магнитометра? Но мне кажется, что корректировка скорости гир через дифференцируемый по времени вектор магнитометра как раз таки даст уйти виртуальному северу, просто медленно… По моему они имели ввиду, что коррекция идет привычным образом, просто при резких изменениях данных магнитометра, ЧАСТОТА коррекции снижается
это будет все тот же компл. фильтр, просто разных типов/порядков… в твоей реализации в покое, после всех возмущений, расчетный вектор приползет к абсолютному…
Подтверждаю, всё именно так.
Ребята же из xsens для коррекции похоже берут диф. мага и теряют абсолютность, т.е. привязку к мировой СК… но тут конечно и недостатки есть
Тут мне лично вообще ничего не понятно… Маг датчик абсолютный, как можно использовать его дифференциал? Для принятия решения о качестве данных? Дык собственно, если уменьшить коэффициент влияния мага на итоговую ориентацию, но оставить выше средней ошибки ДУС и интегратора - всё будет работать четко. Конечно если мат.ожидание разброса данных мага будет стремиться к истинному азимуту места…
Если же использовать дифф. данные мага, то в итоговом повороте будет ДВЕ ошибки интеграции - ДУСа и интеграл шума мага. И чем тогда компенсировать набегающую ошибку?
Вот немного ПО теме -> fixik-papus.livejournal.com/32228.html
Тут мне лично вообще ничего не понятно… Маг датчик абсолютный, как можно использовать его дифференциал?
Люди пытаются, экспериментируют, выкладывают результаты… почему бы и нет ? Попытка - не пытка…
Только моё мнение такое - всеми этими ухищрениями типа “коррекция биаса” “смешивание/подмешивание” жиру не натопишь, есть у бюджетных сенсоров свой предел точности и некоторая степень корявости, как ни крути/не усложняй алгоритмы, эта корявость в одном месте сглаживается а в другом вылазит, - ходьба по кругу, или иными словами:
слепой помогает хромому и слушает немого чтоб улучшить жизнь в целом )))
За столько лет экспериментов с схемой гира/аксель/маг/gps(ну еще сонар), пора бы ИМХО найти уже золотую середину в мат. обеспечении, и успокоиться… ,
а хотите бОльшего - другие датчики надо применять…
Вот немного ПО теме -> fixik-papus.livejournal.com/32228.html
Отличный пример! Описывает как раз все сложности с настройкой системы в комплексе: хотя теория точна и понятна, тот же Калман - разжеван сотню раз, а вот правильно настроить все веса и пороги и учесть все возможные косяки с датчиками - это требует огромной отладочной работы и дара предвидения (чем собственно и и занимаются программисты).
Тут мне лично вообще ничего не понятно… Маг датчик абсолютный, как можно использовать его дифференциал? Для принятия решения о качестве данных? Дык собственно, если уменьшить коэффициент влияния мага на итоговую ориентацию, но оставить выше средней ошибки ДУС и интегратора - всё будет работать четко. Конечно если мат.ожидание разброса данных мага будет стремиться к истинному азимуту места…
Если же использовать дифф. данные мага, то в итоговом повороте будет ДВЕ ошибки интеграции - ДУСа и интеграл шума мага. И чем тогда компенсировать набегающую ошибку?
понимаю твои возмущения! 😃
но иначе откуда у них в алгоритме на выходе (после 17 мин) погрешность в 1.2 и 47 град?
Соот-но провязки и корекции по абсолютному вектору нет, иначе был бы ноль в погрешности на выходе…
Да ничего там нового нет, все тот же комплементарник только в замороченной форме кватернионов
Да, запустил всё ж я всю эту систему (изрядно измучив себе мозг), не вдаваясь в детали, имею сказать - основная фишка этого метода в раздельности обсчета курса и тангажа/крена… особой выгоды ИМХО от сего подхода не нашел…
но иначе откуда у них в алгоритме на выходе (после 17 мин) погрешность в 1.2 и 47 град? Соот-но провязки и корекции по абсолютному вектору нет, иначе был бы ноль в погрешности на выходе…
Если дифференциировать магнетометр - там такие шумы вылезут, что ничего полезного не извлечь. При хорошо откалиброванном гироскопе коррекция идет на десятые а то и сотые доли единицы отсчета (при 2000 град.сек / 16 бит) - именно эту ошибку и должен извлечь алгоритм. Можно попробовать взять разницу показаний гирика с дифференциалом мага, и отсечь шумы НЧ-фильтром очень низкой частоты (с постоянной времени 10 сек и более) и может быть, высокого порядка. А результат применять только при вращениях выше некоторой скорости. Тогда останется только очень медленная ошибка магнетометра (например, очень медленно подносим магнит - магнитное поле крутится, а объект нет). Но об этом говрят и сами Invesense:
“There is one known situation where AHS will have a detrimental effect on the performance. When the
magnetic field is changing very slowly (e.g. when an object is rotating slowly, less than e.g. 0.5 deg/s),
AHS might estimate the gyro bias incorrectly”
Само собой диф. мага очень шумный и его надо фильтровать.
А результат применять только при вращениях выше некоторой скорости. Тогда останется только очень медленная ошибка магнетометра (например, очень медленно подносим магнит - магнитное поле крутится, а объект нет).
Результат (фильтрованная разница показаний гирика с дифференциалом мага) применять к биасу только при вращениях выше некоторой скорости? Почему? Ведь результат уже сильно фильтрован и текущее вращение изменяет его с большой задержкой.
Общался кстати с Алексеем Козиным. Он говорил что в АРДУ 3.3 сделали подобную вещь “подносишь магнит или крутишь компас относительно апм - сразу выявлает и показывает индикатором что екф исключил компас. убираешь магнит и все встает на свои места”
Может кто разбирался уже? Это явно не магия калмана, а дополнение к мат. модели…
“подносишь магнит или крутишь компас относительно апм - сразу выявлает и показывает индикатором что екф исключил компас. убираешь магнит и все встает на свои места”
Ага, только у меня и без магнита ЕКФ постоянно компасс исключает в Ардупилоте 😃 Намудрили там что-то с проверками и порогами. А фишка AHS - он использует компасс и в условиях наводок.
в АРДУ 3.3 сделали подобную вещь “подносишь магнит или крутишь компас относительно апм - сразу выявлает и показывает индикатором что екф исключил компас. убираешь магнит и все встает на свои места”
без магнита ЕКФ постоянно компасс исключает в Ардупилоте Намудрили там что-то с проверками и порогами.
Сергей Липницкий накопал кой чего по этому алгоритму
там типа самообучения вкручено отдельно от EKF …
/*
* this offset learning algorithm is inspired by this paper from Bill Premerlani
*
* …googlecode.com/…/MagnetometerOffsetNullingRevisit…
*
* The base algorithm works well, but is quite sensitive to
* noise. After long discussions with Bill, the following changes were
* made:
*
* 1) we keep a history buffer that effectively divides the mag
* vectors into a set of N streams. The algorithm is run on the
* streams separately
*
* 2) within each stream we only calculate a change when the mag
* vector has changed by a significant amount.
*
* This gives us the property that we learn quickly if there is no
* noise, but still learn correctly (and slowly) in the face of lots of
* noise.
*/
тут не совсем понял
А результат применять только при вращениях выше некоторой скорости. Тогда останется только очень медленная ошибка магнетометра (например, очень медленно подносим магнит - магнитное поле крутится, а объект нет).
Результат (фильтрованная разница показаний гирика с дифференциалом мага) применять к биасу только при вращениях выше некоторой скорости? Почему? Ведь результат уже сильно фильтрован и текущее вращение изменяет его с большой задержкой.
Сергей Липницкий накопал кой чего по этому алгоритму
Да, кстати я делал подобный алгоритм для компасса, только с использованием гироскопа это надежнее - если гироскоп крутит вектор по сфере, а компасс по смещенной сфере, то при каждом атомарном повороте можно нехитрыми операцими вычислить направление коррекции, и уменьшать ошибку смещения сфер до нуля. Работает и находит оффсеты довльно быстро, НО ТОЛЬКО при вращениях по разным осям. Т.е. если взлетели блинчиком и летаем, не поворачивая коптер, то не сработает. И в контроллере подвеса я его не использую, но вообще штука интересная и “про запас” код оставил.
Результат (фильтрованная разница показаний гирика с дифференциалом мага) применять к биасу только при вращениях выше некоторой скорости? Почему? Ведь результат уже сильно фильтрован и текущее вращение изменяет его с большой задержкой.
Насчет фильтрации: задержка будет существенная, но уже на ошибку (т.е. разность скоростей двух датчиков), а для нее не важно, когда мы ее применим, сразу или через 5 секунд, т.к. биас постоянен во времени или меняется достаточно медленно (например, при нагреве ДУС).
А насчет порога на скорость, тут нужно экспеременировать. Я думаю, что нужно рассмотреть все случаи, и порогов будет несколько:
- Гироскоп не крутится, компасс не крутится - скорее всего, система в покое, можно корректировать bias
- Гироскоп не крутится, компасс крутится - что-то не так, возможно система движется прямолинейно в неоднородном поле. Коррекцию не делаем.
- Гироскоп крутится, компасс крутится, разница после фильтрации в пределах допуска - применяем коррекцию.
this offset learning algorithm is inspired by this paper from Bill Premerlani
это уже яйца мамонта, еще на апм1 коптера небыло, а аглоритм был в коде самолета
это уже яйца мамонта, еще на апм1 коптера небыло, а аглоритм был в коде самолета
Так и фильтру Калмана тоже уже пол-века 😁
Да, запустил всё ж я всю эту систему (изрядно измучив себе мозг),
И как оказалось, с ошибками ))…, а в действительности - крутая штука (уж лучше Магвика точно), и вот тому подтверждение:
всё работает как на видео, надо ещё адаптивный коэфициент попробовать… короче рекомендую…
бездоказательное видео имхо. тот же самый фильтр Махони даст такие же результаты, если коррекцию по акселерометру сделать большой, а по магнитометру маленькой.Сконцентрировались на неактульных проблемах в виде слишком больших угловых ускорениях, которых не бывыает в реальных ЛА, а если уж так хочется, то эта проблема легко решается правильной частотй опроса и фильтрацией (в том же MPU надо очень быстро крутануть, чтобы достичь такого же эффекта как на видео). В видео умышленно не показано долгое линейное или центростремительное ускорение, потому что их фильтр бы съехал, ввиду слишком сильной коррекции акселерометра. Коррекцию магнитометра только оси Z - уже миллион и один раз используется везде. И вообще ощущение, что специально сравнивают с плохо настроенными для данного (не актулаьного) случая другими алгоритмами.
тот же самый фильтр Махони даст такие же результаты, если коррекцию по акселерометру сделать большой, а по магнитометру маленькой
У него (Махони) всего две настройки - Kp и Ki (вторая, как правило, не используется), а тут есть и пороги для коррекции “биасов” и раздельный Kp для акселя и мага, так что свободы уж точно больше…
не показано долгое линейное или центростремительное ускорение,
надо проверить как динамический коэф. усиления работает, а там видно будет…
Если оставить только Kp то весь фильтр выродится в обычный альфа-бета комплиментарник, так что Ki используется. Kp надо естественно раздельный для акселя и для мага, тут иначе нельзя. Несколько сообщений назад Алексей Москаленко писал
Да ничего там нового нет, все тот же комплементарник только в замороченной форме кватернионов (со всеми вытекающими математическими тонкостями через которые придется продираться). Динамический коэффициент на акселе по анализу длины вектора гравитации - идея старая, но не рабочая, хороша только для тестовых отчетов, но в реальности его нужно применять очень акуратно, особенно если система постоянно в движении (я так и не получил заметных улучшений).
Тут я с ним согласен, это обычный комплиментарник, динамический коэффициент не рабочая идея.