[Sasha 0]

Всем привет! Пытаюсь запустить фильтр Маджвика в своем проекте на stm32c8t6, но безуспешно. Реализацию взял здесь https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/. На вход фильтра подаю данные гироскопа в рад/с, данные акселерометра и магнетометра. В конце фильтра функции перевод в радианы по этим формулам:

yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); и перевожу из радиан в градусы * (180/pi). Еще нашел такой проект https://github.com/kriswiner/MPU9250/blob/master/MPU9250BasicAHRS_t3.ino. Проверял все по строчкам, все то же самое, получаю данные с датчика отправляю на фиьтр и тд.   В итоге получаю значения не углов, а пляски значений. Во-первых они бывают не постоянны, да еще и не соотвествуют реальности. Вот на скрине все показания, первые паоказания это через фильтр Маджвика, а последние 3 значения углы, которые соответствуют реальности, полученные через комплементарный фильтр, единсвенно последнее значение рысканья без учета магнетометра, просто с гироскопа, которое соответсвенно плывет. По крену конструкция реально почти 90 градусов, это видно по последним показаниям R. По тангажу то же самое. Но первые показания не понятно что показывают и пляшут. Но если трясти конструкци, удерживая ее ровно по осям, углы уже сильно скачут, когда ориентация конструкции не изменилась, то комплементарный фильтр совсем не годится.  Еще заметил один момент, если выровнять консрукцию до 0, то показания с фильтра маджвика совпадают с комплементарным, там 0 и там 0 по той оси, что выровнял. Прошу подсказать, кто шарит в этой теме, где я могу косячить и чего не учел.

 

Изменено 12 августа, 2022 пользователем Sasha

[AleksBak 0]

В 13.08.2022 в 01:19, Sasha сказал:

в своем проекте на stm32c8t6

А это какой? В смысле STM32F103C8T6? У автора (с гитхаба) 401-ый применен (может не зря).

[Sasha 0]

В 15.08.2022 в 11:55, AleksBak сказал:

А это какой? В смысле STM32F103C8T6? У автора (с гитхаба) 401-ый применен (может не зря).

Ага он,  STM32F103C8T6. Его вполне хватает. Вчера нашел ошибку, почему все не работало, исправил, фильтр стал работать, но появились другие проблемы. Во-первых копится ошибся и углы все плывут, а вторая, долгий отклик, систему повернул с 0 градусов до 90, она через 1-2 сек только дойдет до 90 градусов. 

[jcxz 184]

2 часа назад, Sasha сказал:

Ага он,  STM32F103C8T6. Его вполне хватает.

Ой ли?  

А ничего что он не имеет FPU, в отличие от слизанного проекта?

Да и в целом, даже не считая FPU  - МК намного более тормозной чем в источнике.

2 часа назад, Sasha сказал:

долгий отклик, систему повернул с 0 градусов до 90, она через 1-2 сек только дойдет до 90 градусов. 

Ну значит - надо увеличивать частоту обработки (частоту потока сырых данных и их ввода в фильтр). А какая у вас частота?

У меня например 2кГц. И то задержка иногда бывает заметной. Но боюсь на 2кГц ваш "вполне хватает", с такой математикой "тупо в лоб" как у вас, сядет на попу. Просто не потянет. Так как не имеет FPU. А об оптимизации вычислений вы даже не задумывались как видно из кода.

[Sasha 0]

В 15.08.2022 в 14:57, jcxz сказал:

Ой ли?  

А ничего что он не имеет FPU, в отличие от слизанного проекта?

Да и в целом, даже не считая FPU  - МК намного более тормозной чем в источнике.

Ну значит - надо увеличивать частоту обработки (частоту потока сырых данных и их ввода в фильтр). А какая у вас частота?

У меня например 2кГц. И то задержка иногда бывает заметной. Но боюсь на 2кГц ваш "вполне хватает", с такой математикой "тупо в лоб" как у вас, сядет на попу. Просто не потянет. Так как не имеет FPU. А об оптимизации вычислений вы даже не задумывались как видно из кода.

В проекте на гитхаб, я только сверял реализацию. Если заметите, то файлы проекта предназначенны для Arduino))) Так что данный фильтр легко будет работать на Pro Mini. Данный фильтр позиционирует себя как "менее требовательный к вычислительным ресурсам". Частота 1кГц для получения сырых данных. Пихал данные по времени с заданной частотой и сразу без таймера, результат один и тот же.  С комплементарным фильтром задержки нет.

[KnightIgor 2]

У меня тут похожая задачка, только без гироскопа.

На плате есть магнетометр, измеряющий магнитное поле Земли, и акселерометр. Эта плата помещается под асфальт, и хотелось бы знать, как она там расположилась относительно поверхности. Как я понимаю, если взять код, нужно просто скормить ему данные от accel и mag и нулевые значения гироскопа, т.к. плата не движется?

Изменено 16 сентября, 2022 пользователем KnightIgor

[KnightIgor 2]

On 9/16/2022 at 2:15 PM, KnightIgor said:

У меня тут похожая задачка, только без гироскопа.

На плате есть магнетометр, измеряющий магнитное поле Земли, и акселерометр. Эта плата помещается под асфальт, и хотелось бы знать, как она там расположилась относительно поверхности. Как я понимаю, если взять код, нужно просто скормить ему данные от accel и mag и нулевые значения гироскопа, т.к. плата не движется?

 

Сам себе отвечу по результатам.

Пропустил я результаты через фильтр Маджвика с учетом того, что у меня нет гироскопа (его часть данных всегда нули). Программу запилил на Lazarus (сконвертировав C код). Выходные углы очень похожи на правду (*см. далее), а реакция на изменение положения - медленное (точно, как в видео https://www.youtube.com/watch?v=C7JQ7Rpwn2k&t=1908s&ab_channel=GoogleTechTalks, когда при демонстрации отключали гироскоп).

Но есть одно "но": угол yaw (вокруг оси Z) выдается как бы почти удвоенный: я поворачиваю платку на 90 градусов (с севера на восток), а угол достигает почти 180. Поворачиваю далее до юга, а показания доходят до северо-запада. Так как формулы сложные, тупо поделил значение на 2, но начались вообще странные вещи (прыгает словно ArcSin или кто там в разрыв идет). Две другие оси показывают правильно.

Что-бы это могло быть?

[113 3]

On 8/15/2022 at 12:17 PM, Sasha said:

долгий отклик, систему повернул с 0 градусов до 90, она через 1-2 сек только дойдет до 90 градусов. 

Похоже на неправильный масштаб гироскопа. Проверить просто: если масштаб больше, то при резком повороте происходит "перерегулирование" с постепенным возвратом назад. Если масштаб меньше - недорегулирование с постепенной коррекцией. А если все правильно подобрано - при всех поворотах реакция будет быстрая и без шумов.

У меня в реализации есть перевод из °/сек в рад/сек.

upd: И скорость опроса здесь не определяющее значение имеет. 100Гц обычно достаточно. Главное учитывать ее в фильтре.

Изменено 21 октября, 2022 пользователем 113
upd

[Sasha 0]

В 21.10.2022 в 10:51, 113 сказал:

Похоже на неправильный масштаб гироскопа. Проверить просто: если масштаб больше, то при резком повороте происходит "перерегулирование" с постепенным возвратом назад. Если масштаб меньше - недорегулирование с постепенной коррекцией. А если все правильно подобрано - при всех поворотах реакция будет быстрая и без шумов.

У меня в реализации есть перевод из °/сек в рад/сек.

upd: И скорость опроса здесь не определяющее значение имеет. 100Гц обычно достаточно. Главное учитывать ее в фильтре.

 

Вы имеете ввиду про диапазон измерения, что настраивается в чипе?

В 29.09.2022 в 17:48, KnightIgor сказал:

Сам себе отвечу по результатам.

Пропустил я результаты через фильтр Маджвика с учетом того, что у меня нет гироскопа (его часть данных всегда нули). Программу запилил на Lazarus (сконвертировав C код). Выходные углы очень похожи на правду (*см. далее), а реакция на изменение положения - медленное (точно, как в видео https://www.youtube.com/watch?v=C7JQ7Rpwn2k&t=1908s&ab_channel=GoogleTechTalks, когда при демонстрации отключали гироскоп).

Но есть одно "но": угол yaw (вокруг оси Z) выдается как бы почти удвоенный: я поворачиваю платку на 90 градусов (с севера на восток), а угол достигает почти 180. Поворачиваю далее до юга, а показания доходят до северо-запада. Так как формулы сложные, тупо поделил значение на 2, но начались вообще странные вещи (прыгает словно ArcSin или кто там в разрыв идет). Две другие оси показывают правильно.

Что-бы это могло быть?

Калибровку магнитометра делали? Так как это датчик магнитного поля, то окружающие предметы могут влиять на это поле, например liion АКБ иди колонки на столе, поэтому обратите на это внимание тоже.

 

[113 3]

Масштаб и в чипе надо настроить и в маджвике, т.к. чип выдает (обычно) в °/сек, а маджвик (в зависимости от реализации) принимает рад/сек.

Про калибровку магнитометра - это вообще отдельная задача. Если батарейка не сменная и окружающее устройство поле не сильно изменяется, то можно обойтись  однократной калибровкой. В противном случае необходимо достаточно ресурсоемкие алгоритмы встраивать в программу для калибровки в реальном времени, а это - задача по нахождению 3х-осного эллипсоида (9 неизвестных) + корреляция пары векторов магнитного поля и гравитации (которую надо выделить из общего ускорения).

И зачем использовать маджвика без гироскопа? Это проще тогда комплементарный фильтр. Тоже самое примерно и получится.

Изменено 23 октября, 2022 пользователем 113

[Sasha 0]

В 23.10.2022 в 12:29, 113 сказал:

Масштаб и в чипе надо настроить и в маджвике, т.к. чип выдает (обычно) в °/сек, а маджвик (в зависимости от реализации) принимает рад/сек.

Про калибровку магнитометра - это вообще отдельная задача. Если батарейка не сменная и окружающее устройство поле не сильно изменяется, то можно обойтись  однократной калибровкой. В противном случае необходимо достаточно ресурсоемкие алгоритмы встраивать в программу для калибровки в реальном времени, а это - задача по нахождению 3х-осного эллипсоида (9 неизвестных) + корреляция пары векторов магнитного поля и гравитации (которую надо выделить из общего ускорения).

И зачем использовать маджвика без гироскопа? Это проще тогда комплементарный фильтр. Тоже самое примерно и получится.

 

Чип выдает результат своего АЦП, то есть измеренное напряжение каждой оси. Далее вы сами его эти измерения переводите либо в градусы в сек либо в рад в сек умножая на соответствующий коэффициент в зависимости от выбранной чувствительности. Все таки не сильно понял про масштаб, в фильтре я такого не видал, только чувствительность настраиваемая в датчике.

[113 3]

Чип выдает все же +/- приведенные данные. Вот например IAM20380:

  switch((FULL_SCALE >> 3) & 0x03)
  {
  case 0:       //±250 dps
    Gyro_Sensitivity = 1.0/131.0;
    break;
  case 1:       //±500 dps
    Gyro_Sensitivity = 1.0/65.5;
    break;
  case 2:       //±1000 dps
    Gyro_Sensitivity = 1.0/32.8;
    break;
  case 3:       //±2000 dps
    Gyro_Sensitivity = 1.0/16.4;
    break;
  }

Как видно, гироскоп выдает значения в °/сек. Конечно эти данные тоже неплохо бы откалибровать во всем температурном диапазоне, но не суть.

Маджвик же оперирует с радианами, поэтому надо:

    //Scaling gyro from d/s to rad/s
    gx *= DEG2RAD;
    gy *= DEG2RAD;
    gz *= DEG2RAD;

//#define DEG2RAD                 0.017453292f

 

[Sasha 0]

Ну да у вас все здесь верно. Да, забыл что коэффициенты из документации приводятся для получения °/сек. Можно кстати и так получить коэффициент чувствительности через выбранный диапазон, наример для ±250 °/сек

gSens = 250.0/32768.0; // ±250 º/s

Все таки вы писали про масштаб, и тут же про единицы измерения, может все таки масштаб вы имели под единицами измерения или нужно подобрать нужную чувствительность, чтобы не было долгого отклика? У меня на гироскопа использовались рад/c, поэтому тут ошибки не было и все таки отклик не быстрый. 

[113 3]

Надо пробовать. У меня на одном гироскопе, вопреки документации значения в 2 раза меньше выдавались. 
самое простое - проинтегрировать данные с гироскопа во время быстрого поворота например на 180°. Если в результате получится значение +/- такое же, значит гироскоп ок и дело в чем-то другом. 
Под масштабом я имел в виду поправочный коэффициент. Он может быть как для перевода из разных единиц измерения, так для точной калибровки. Методика калибровки - как описано выше. 
Только при проведении калибровки нужно учитывать, что мемс гироскопы чувствительны к линейным ускорениям и соответственно, к ориентации оси в пространстве. 

[KnightIgor 2]

On 10/23/2022 at 11:29 AM, 113 said:

Масштаб и в чипе надо настроить и в маджвике, т.к. чип выдает (обычно) в °/сек, а маджвик (в зависимости от реализации) принимает рад/сек.

Про калибровку магнитометра - это вообще отдельная задача. Если батарейка не сменная и окружающее устройство поле не сильно изменяется, то можно обойтись  однократной калибровкой. В противном случае необходимо достаточно ресурсоемкие алгоритмы встраивать в программу для калибровки в реальном времени, а это - задача по нахождению 3х-осного эллипсоида (9 неизвестных) + корреляция пары векторов магнитного поля и гравитации (которую надо выделить из общего ускорения).

И зачем использовать маджвика без гироскопа? Это проще тогда комплементарный фильтр. Тоже самое примерно и получится.

 

Я не спец в этих 6D алгоритмах, а Маджвик есть в исходнике. У меня нет гироскопа, т.к. это не задача стабилизации объекта в пространстве в реальном времени. У меня есть некая плата с магнетометром и акселерометром, и я хочу просто знать, в каком положении она закопана под землю - ее ориентацию там. Если наведете на тот самый "комплементарный фильтр" с исходником, буду благодарен.