Создание собственной системы стабилизации
Все правильно вы сказали, я не прав, не хватает еще одной вещи, надо иметь какой то датчик, который реагирует, когда поворачивается в сторону оператора, тогда можно реализовать мою систему. А этот датчик - это уже отдельный геммор, лучше уж оставить магнитометр.
надо иметь какой то датчик, который реагирует, когда поворачивается в сторону оператора
радиокомпас с узко направленной антенной)))
как по вашему самолёты во вторую мировую пролетали пол континента и не сбивались с курса без GPS?
Во во, что то подобное
имея 3 антенны разнесенные по краям борта можно вычислить не только координаты но и положение в пространстве по сигналам со спутников … но … борт должен быть достаточно большим (чем дальше разнесены антенны тем точнее ) … при достаточно больших габаритах можно воспользоваться геодезическими высокоточными модулями , но их цена оставляет желать лучшего …
в идеале обрабатывать сигнал со всех антенн одним модулем , тогда точность будет выше (можно будет мерять задержки/фазовый сдвиг сигнала между антеннами)… только существует ли такой модуль …
Вы меня не поняли, магнитометр очень капризная штука, неровное его расположение будет давать перекосы в ориентации при наклонах аппарата
Да, это факт … , любые т.н. “MARG алгоритмы” этим страдают, поэтому подмешивать компас в общую кучу с горизонтом - плохая затея, не смотря на всю красивость алгоритмов типа Магвика и Махони…
Вот посмотри: (щас кстати пытаюсь реализовать у себя) www.ncbi.nlm.nih.gov/pmc/articles/PMC4570372/
- товарищи хитрым способом сделали (?) курс по магнитометру+аксель отдельно от всего остального и кажется довольно не плохо…
казалось бы как один человек мог столько успеть?!
хитрым способом сделали (?) курс по магнитометру+аксель отдельно от всего остального и кажется довольно не плохо
Ну это было на заре развития, сейчас я уверен, что все полетники магнитометр только для курса используют. Но даже если только курс подвержен наводкам магнитного поля - все равно это остается проблемой. Интересно, как удалось решить эту проблему ребятам из X-sense. В их сенсорах есть режим AHS - Active Heading Stabilization, где они заявляют крайне низкие ошибки по курсу даже в условиях сильных магнитных помех. Если кто тестировал его, интересно услышать так ли это на самом деле.
казалось бы как один человек мог столько успеть?!
РИП. Человек занимался Делом. А большинство из нас постоянно находятся в беличьем колесе житейских проблем…
любые т.н. “MARG алгоритмы” этим страдают, поэтому подмешивать компас в общую кучу с горизонтом - плохая затея, не смотря на всю красивость алгоритмов типа Магвика и Махони…
Я беру только проекцию мага на на мнимый горизонт, поэтому компас влияет только на курсовую ориентацию. Однако в моих широтах даже небольшой завал горизонта существенно отклоняет курс (
Интересно, как удалось решить эту проблему ребятам из X-sense. В их сенсорах есть режим AHS - Active Heading Stabilization, где они заявляют крайне низкие ошибки по курсу даже в условиях сильных магнитных помех.
Действительно интересно, особенное если без внешней коррекции (только как?!).
И на каких датчиках?
Если кто тестировал его, интересно услышать так ли это на самом деле.
Да, крайне интересно.
Да, это факт … , любые т.н. “MARG алгоритмы” этим страдают, поэтому подмешивать компас в общую кучу с горизонтом - плохая затея, не смотря на всю красивость алгоритмов типа Магвика и Махони…
У меня сделано просто - вектор магнитного поля (в системе отсчета ЛА) вращается в систему отсчета “Земля”, от него отбрасывается Z, вращается обратно в систему ЛА и скармливается MARG. На самом деле более оптимально, но смысл тот же. Так компас а) скомпенсирован по крену (наклоны ЛА не влияют на курс), б) не влияет на горизонт.
вектор магнитного поля (в системе отсчета ЛА) вращается в систему отсчета “Земля”, от него отбрасывается Z, вращается обратно в систему ЛА
Точно так.
В их сенсорах есть режим AHS - Active Heading Stabilization, где они заявляют крайне низкие ошибки по курсу даже в условиях сильных магнитных помех. Если кто тестировал его, интересно услышать так ли это на самом деле.
По мануалу они используют фильтр Калмана. Я предполагаю, что в матрицу шумов измерений магнитометра идет разница между константным вектором магнитного поля Земли для данной местности + офсеты (калиброванным) и полученными измерениями. То есть по-простому, если вектор отличается от нормального, то есть какие то наводки, значит измерения сильно зашумлены, им не доверяем.
UPD.
4.4 Active Heading Stabilization (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. This way,
drift in heading can be as low as 1 deg after 60 minutes for the MTi 100-series and 3 deg after 60
minutes for the MTi 10-series. 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.
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.
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.
Собсно, кратко они пишут, что при резких изменениях магнитного поля, частота оценки биаса гиры по Z становится меньше. При этом если скорость изменения магнитного поля низкая, то AHS будет работать некорректно, и соответсвенно курс будет неверен.
Собсно, кратко они пишут, что при резких изменениях магнитного поля, частота оценки биаса гиры по Z становится меньше. При этом если скорость изменения магнитного поля низкая, то AHS будет работать некорректно, и соответсвенно курс будет неверен.
Да, предполагал что-то подобное. Спасибо за информацию. А датчики у них от Fairchild - FIS 1100. www.fairchildsemi.com/products/sensors/
У меня есть отладочный комплект - один-в-один как на картинке xsense. Видимо, лицензировали софт и продают под разными брендами.
Действительно интересно, особенное если без внешней коррекции (только как?!).
Я вот ссылочку выше давал, может кто попробует сделать реализацию на Си ? Там вроде идея относительно просто и доступно разжевана… если есть понятия в области кватернионов то вообще - “конструкция выходного дня”…
(я себе накропал уже функцию, но пока че то не клеится, с кватернионами приходится разбираться по ходу,… может оси не так выравниваю, может банальная ошибка в алгоритме вращения… короче работать надо)
Вот тогда и обсудим результаты, глядишь чего и получится…(уж больно хорошие результаты пророчат авторы статьи)
Там, кстати, во второй части (пока не смотрел подробно) ещё и про влияние негравитационых ускорений акселя есть решение, через динамический коэфициент усиления…
если кому интересно, вот моя пробная функция (попытка реализации):
Я вот ссылочку выше давал, может кто попробует сделать реализацию на Си ? Там вроде идея относительно просто и доступно разжевана… если есть понятия в области кватернионов то вообще - “конструкция выходного дня”…
Там разжевывают математику как вращать кватернионами локальные вектора и как комп.фильтр сделать в кватернионах. Где там “изюминка” с хорошим магом?
Где там “изюминка” с хорошим магом?
Идея алгоритма такая (как я понял в меру знания языка):
- рожаем кватернион акселерометра (с нужными хитрыми ограничениями для избежания сингулярностей)
- берём вектор магнитометра и вращаем его полученным ранее хитрым кватернионом акселя.
- теперь рожаем кватернион магнитометра (тоже с хитрыми ограничениями от авторов…)
- рожаем конечный кватернион (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;
}