Создание собственной системы стабилизации
Где там “изюминка” с хорошим магом?
Идея алгоритма такая (как я понял в меру знания языка):
- рожаем кватернион акселерометра (с нужными хитрыми ограничениями для избежания сингулярностей)
- берём вектор магнитометра и вращаем его полученным ранее хитрым кватернионом акселя.
- теперь рожаем кватернион магнитометра (тоже с хитрыми ограничениями от авторов…)
- рожаем конечный кватернион (Yaw) перемножая кватернион акселя (1.) и кватернион мага (3.)
типа основной “замес” в том, что кватернион магнитометра - получаем сначала как функцию от кватерниона акселя а потом при перемножении их “убиваются все сингулярности” и - вот оно якобы счастье… (графики, аплодисменты и т.д. )))
Вообще говоря похоже на:
У меня сделано просто - вектор магнитного поля (в системе отсчета ЛА) вращается в систему отсчета “Земля”, от него отбрасывается Z, вращается обратно в систему
но, не уверен, может и есть что то другое…
Вот тогда и обсудим результаты, глядишь чего и получится…(уж больно хорошие результаты пророчат авторы статьи) Там, кстати, во второй части (пока не смотрел подробно) ещё и про влияние негравитационых ускорений акселя есть решение, через динамический коэфициент усиления…
Да ничего там нового нет, все тот же комплементарник только в замороченной форме кватернионов (со всеми вытекающими математическими тонкостями через которые придется продираться). Динамический коэффициент на акселе по анализу длины вектора гравитации - идея старая, но не рабочая, хороша только для тестовых отчетов, но в реальности его нужно применять очень акуратно, особенно если система постоянно в движении (я так и не получил заметных улучшений).
- рожаем кватернион акселерометра (с нужными хитрыми ограничениями для избежания сингулярностей) 2. берём вектор магнитометра и вращаем его полученным ранее хитрым кватернионом акселя. 3. теперь рожаем кватернион магнитометра (тоже с хитрыми ограничениями от авторов…)
Вот именно что все хитрости нужны только для того, чтобы применить математику кватернионов к этой задаче. Тогда как в форме DCM все очень тривиально и просто считается.
чтобы применить математику кватернионов к этой задаче.
Наверно люди неспроста потратили силы на это, может и результат всёж получше будет чем у DCM (?),
Да ничего там нового нет, все тот же комплементарник
Ну да, согласен (в принципе они этого и не скрывают), но меня больше заинтересовала часть именно про магнитометр и аксель…
Тогда как в форме DCM все очень тривиально и просто считается.
Посоветуйте источник, где посмотреть, если не трудно…
Собсно, кратко они пишут, что при резких изменениях магнитного поля, частота оценки биаса гиры по Z становится меньше. При этом если скорость изменения магнитного поля низкая, то AHS будет работать некорректно, и соответсвенно курс будет неверен.
либо вы пропустили самое главное, либо опустили это как очевидное 😃
но основная мысль в этом абзаце
One powerful feature when it comes to heading estimation is Active Heading stabilization (AHS). AHS uses the magnetometers to estimate the gyro bias, but it does not reference the heading.
Тут идея в том что они НЕ корректируют азимут по магнетометру (как это сделано в open source системах, которые смотрел), а корректируют биас гир!
И это очень похоже на то, о чем говорил visasta пару годков назад. Что мол ему пофигу например на силовые наводки на компас и что он его использует лишь для коррекции…
В общем идея такова 😃 Берем дифференциал мага по Z, и сравниваем его с угловой скоростью по Z. Если отличие не больше определенного порога (скажем 10%), то корректируем биас гиры, если порог выше, то значит маг врет и тогда в сад его, либо в э том случае уменьшаем коэф. коррекции биаса гиры (с увеличением % отличия между угловой скоростью гир по Z и диф. мага по Z).
А вот тут они меня запутали 😃
When AHS is applied to a filter profile that uses the magnetic field as a reference, the magnetic field will no longer be used as a reference. Heading output will be heading tracking instead of referenced heading.
Пишут что маг все же используется для азимута, но при этом НЕ используется когда, когда работает AHS (т.е. биас гир коректируется по магу)
UPD:
Собсно, кратко они пишут, что при резких изменениях магнитного поля, частота оценки биаса гиры по Z становится меньше.
When the magnetic field is disturbed, AHS will still function. The frequency though of the updates of the gyro bias will be lower, as AHS is triggered only when the
magnetic field is stable.
и тут речь скорее не про резкие изменения магнитного поля, а про некоректные изменения, т.е. когда диффернциал. маг поля сильно не сходится с угловой скоростью гиры…
Наверно люди неспроста потратили силы на это, может и результат всёж получше будет чем у DCM (?),
просто другой подход. Есть матрицы поворота, есть кватернионы которые некотрые вещи решают проще (интерполяция угла например) но это лишь математика, а логика та же - комплементарный фильтр. Хотя, возможно я ошибаюсь и где внутри формул скрыты существенные отличия. Просто мне сложно понять все эти выкладки с кватернионами до последнего символа, тогда как DCM я понимаю полностью.
В общем идея такова Берем дифференциал мага по Z, и сравниваем его с угловой скоростью по Z. Если отличие не больше определенного порога (скажем 10%), то корректируем биас гиры, если порог выше, то значит маг врет и тогда в сад его, либо в э том случае уменьшаем коэф. коррекции биаса гиры (с увеличением % отличия между угловой скоростью гир по Z и диф. мага по Z).
Да, видимо так и происходит, но вся соль в деталях реализации. Например, поворачиваем сенсор вблизи металлического предмета, и ДУС видит одну скорость а компасс другую (т.к. вектор идет по эллипсоиду и в зависимости от ориентации к полуосям будет разная скорость). Или, объект медленно движется посупательно в неравномерном магнитном поле - вектор поля крутится, а гироскопа нет. При этом разница может вполне укладываться в 10%.
Да, видимо так и происходит, но вся соль в деталях реализации. Например, поворачиваем сенсор вблизи металлического предмета, и ДУС видит одну скорость а компасс другую (т.к. вектор идет по эллипсоиду и в зависимости от ориентации к полуосям будет разная скорость). Или, объект медленно движется посупательно в неравномерном магнитном поле - вектор поля крутится, а гироскопа нет. При этом разница может вполне укладываться в 10%.
все верно тут дьявол в деталях 😃
важно верно найти признаки/события глюков мага + вкл/выкл лимиты настроить… но даже если все будет работать, то нет 100% гарантии что система не выпрыгнет из штанов (из стабилизации) в определенный момент…
для стабильности, к этому механизму корекции биас гиры, можно добавить корекцию вектора азимута по гпс, когда система в движении…
- рожаем кватернион акселерометра (с нужными хитрыми ограничениями для избежания сингулярностей)
- берём вектор магнитометра и вращаем его полученным ранее хитрым кватернионом акселя.
- теперь рожаем кватернион магнитометра (тоже с хитрыми ограничениями от авторов…)
- рожаем конечный кватернион (Yaw) перемножая кватернион акселя (1.) и кватернион мага (3.)
У меня так самая первая АХРС работала, при больших вибрациях получаются большие нелинейности, из-за высокой вероятности попадания в зону неопределенности.
типа основной “замес” в том, что кватернион магнитометра - получаем сначала как функцию от кватерниона акселя а потом при перемножении их “убиваются все сингулярности” и - вот оно якобы счастье… (графики, аплодисменты и т.д. )))
Не вчитался как тут, у меня в первом алго формировался кватернион из произведения (поворота) кватерниона акселя на кватернион мага. Болтало его страшно. Потом он, уменьшенный на коэф. фильтра, складывался с кватернионом АХРС(угловым интегралом ДУС), после кватернион АХРС нормировался.
они НЕ корректируют азимут по магнетометру (как это сделано в open source системах, которые смотрел), а корректируют биас гир!
У меня так работают все алго, начиная со 2й версии (сентябрь 2013). Начальное смещение дусов высчитывается при арминге перед полётом.
Динамический коэффициент на акселе по анализу длины вектора гравитации - идея старая, но не рабочая, хороша только для тестовых отчетов, но в реальности его нужно применять очень акуратно, особенно если система постоянно в движении (я так и не получил заметных улучшений).
Так точно, просто срезать и выбрасывать лишний аксель нельзя, нужно правильно распределять разницу между векторами акселя и мнимого вектора гравитации между коррекцией горизонта и интегралом линейного перемещения (скорость , расстояние ИНС).
важно верно найти признаки/события глюков мага + вкл/выкл лимиты настроить… но даже если все будет работать, то нет 100% гарантии что система не выпрыгнет из штанов (из стабилизации) в определенный момент…
Тут всё просто - нужно ограничить вектор мага и отбросить координату Z, кватернион доворота будет (mx,my,0,0). Алго может “выскочить из штанов” только при деградации итогового кватерниона АХРС стремящемуся к (0,0,0,0), чтоб так не получилось нужно ограничивать угловые дельты за отсчёт до МаксУгловаяСкорость*dt (на случай получения глючного отсчёта от ДУС), также ограничивать макс.значения смещения и макс. скорость изменения смещения за отсчёт.
для стабильности, к этому механизму корекции биас гиры, можно добавить корекцию вектора азимута по гпс, когда система в движении…
А есть мысли как это сделать? У мну в моделях работает, а в природе нет(((
Я вот ссылочку выше давал, может кто попробует сделать реализацию на Си ?
Вот из моего первого проекта (в файле imu.cpp).
#ifdef ACC_COMP_MODE /******* аксель + компас ********/
if (vAcc.z < 0.0f)
{
qAcc.set(0,0,1,0);
/* находим кватернион положения по акселю */
q.set(-vAcc.x,vAcc.y,vAcc.z, 0);
/* приведение вектору гравитации */
qAcc *= q;
qAcc.normalize();
qAcc.w += 1.0f;
q.set(1,0,0,0);
qAcc *= q;
} else {
qAcc.set(0,0,-1,0);
/* находим кватернион положения по акселю */
q.set(vAcc.x,vAcc.y,vAcc.z, 0);
/* приведение вектору гравитации */
qAcc *= q;
qAcc.normalize();
qAcc.w += 1.0f;
}
/* приведение к азимуту */
// q.set(0,0,(qIMU.w*qIMU.z+qIMU.x*qIMU.y),(1-2*(qIMU.y*qIMU.y+qIMU.z*qIMU.z)));
// qAcc *= q;
/* определяем длину дуги между G и ИНС */
qAcc.normalize();
float delta = inner_product(qIMU, qAcc);
if (abs(1 - AccMag) < 0.05f) {
q = (qIMU * (1.0f-omega))+(qAcc*delta*omega);
if ((q.w!=0)||(q.x!=0)||(q.y!=0)||(q.z!=0)) qIMU = q;
}
У меня так работают все алго, начиная со 2й версии (сентябрь 2013). Начальное смещение дусов высчитывается при арминге перед полётом.
так я не говорил про начальную калибровку биасов гирр и это самом собой что они высчитываются до арма (или с подачей питания как в вие)… А речь шла про калибровку биаса гиры Z оси по дифф. магнетометра в полете, если значение дифф. мага валидное… так же из источника выше излагается идея о том что не нужно выполнять коррекцию результируещего Z вектора (т.е. результат интегрировния гир) по магу… у тебя так? точности гир хватает что бы не уплывал интеграл гиры по Z? Из моей практики не корректируемый интеграл долго не проживет…
А есть мысли как это сделать? У мну в моделях работает, а в природе нет(((
на сколько помню в арду сделано это давно, но я не разбирался… у Константина в iNav это вроде тоже есть…
github.com/iNavFlight/inav
идея о том что не нужно выполнять коррекцию результируещего 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 - он использует компасс и в условиях наводок.