Вычисление кватерниона из данных компаса и акселерометра

lorry

linvinus писал(а):

Цитата:

Делить на Pi и умножить на 180, это есть перевод значений угла из радиан в градусы.

http://ru.yasno.tv/article/math/45-ugly ... -i-radiany

Уважаемый linvinus, а что здесь смешного? Или вы не согласны со следующим утверждением?
Для того чтобы перевести радианы в градусы воспользуйтесь формулой:

α°= αрад*(180°/π)

linvinus

По сути вы правы но ваш ответ был не про то.

Цитата:

Зачем данные с гироскопа умножаются на Pi и делятся на 180?
Делить на Pi и умножить на 180, это есть перевод значений угла из радиан в градусы.

А автору вопроса я посоветовал почитать в каком формате данные на выходе датчиков.

maxbelyx

Наткнулся на аналогичную проблему. Имею датчик MPU9250, код на Си для Atmega. Считываю данные по шине SPI, хотел бы отфильтровать данные фильтром маджвика, тот, который без компаса.

По началу получал углы с акселерометра вот таким образом: (ax, ay, az - "сырые" данные с датчика)
AccX = atan2(ax, az) * 180/Pi;
AccY = atan2(ay, az) * 180/Pi;

А с гироскопа вот таким: (gx,gy,gz - "сырые" данные с датчика)
GyroX += gx / GYRO_SENS * 0.001; // 0.001 - время опроса датчика, GYRO_SENS = 131 для диапазона 250DPS
GyroY += gy / GYRO_SENS * 0.001;
GyroZ += gz / GYRO_SENS * 0.001;

Угол рысканья по акселерометру, как я понял, получить нельзя. Обрабатывал комплиментарным фильтром по такой формуле:

Угол = 0.98 * GyroN + (1.0 - 0.98) * AccN;

А теперь, собственно, вопрос - покопал в сторону фильтра маджвика, там есть вариант с данными только гироскопа и акселерометра. В каком виде надо подавать туда эти данные? Я так опнял это не сырые показания с датчиков, а что-то другое. С английским, к сожалению, очень плохо, потому сам разобраться не смог. Буду благодарен за любую помощь.

Максим.

metric

У маджвика, если правильно помню, кватернионы используются, а не сырые данные. 9250 может сразу кватернионы отдавать, а не только сырые данные.

maxbelyx

Маджвик работает с квантернионами, а не принимает их, если я верно понял. То, что 9250 может отдавать их, и про DMP знаю, интересует именно то, о чем спросил

Максим.

linvinus

Цитата:

В каком виде надо подавать туда эти данные?

данные гироскопа нужно конвертировать в радианы

вот рабочий пример https://github.com/kriswiner/MPU-9250/b ... icAHRS.ino

>MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

стабильного курса без компаса не получите, а компас ещё грамотно откалибровать нужно и более менее работает только если среда не меняется (внешние магнитные поля)

Пяткин

В квартире магнитометр нормально работать не будет
Выводы сделал на основе своего опыта

maxbelyx

linvinus, спасибо большое! Буду пробовать, посмотрим, что получу на выходе.
Сразу еще вопрос, можно ли эти же данные подать в фильтр маджвика?
Я имею ввиду данные акселя, помноженные на aRes (n/32768), и данные гироскопа помноженные на gRes (n/32768) и переведенные в радианы?
И что Вы имели ввиду под курсом? Если значение по yaw, то оно, на самом деле, не так принципиально, важны лишь данные по pitch и roll. Или они тоже будут плавать, если использовать без магнетометра?
Максим.

linvinus

мадвик или махони большой разницы нет.
данные акселя безразмерные, они нормализуются в алгоритме до юнит вектора

данные гиро обязательно в радианах

да yaw , если yaw не нужен и объект статичен (не находится на быстро движущейся тележке) то аксель вам всегда будет давать точный pitch и roll, если объект движется то гироскоп помогает компенсировать показания акселя, т.к. в этом случае аксель показывает что то среднее между G и вектором движения.

магнетометр нужен только для стабилизации yaw, pitch И roll хорошо получаются от акселя+гиро.
yaw можно получить только с гироскопа и компаса, но гироскоп постоянно уплывает, поэтому компас нужен чтобы компенсировать уплывание гироскопа.

maxbelyx

Подставил, вроде все работает, спасибо!
Но, заметил что очень долго значения возвращаются в ноль - т.е. делаю резкое воздействие, и ставлю на стол платку, и только спустя секунды 3 оно возвращается в ноль. Поигрался с значениями sampleFreq и betaDef в самом фильтре (использую MadgwickAHRSupdateIMU ), - стало лучше, и даже вроде как удалось подстроить как надо. Но все же, есть какие то правила, по которым надо ориентировать в настройке этих параметров?
Максим.

linvinus

sampleFreq это частота вызова функции MadgwickAHRSupdateIMU, вы с какой частотой её вызываете?

maxbelyx

На данный момент 50

linvinus

чем выше частота тем луче.

вообще более правильно туда подставлять реально время сколько прошло с момента прошлого запуска MadgwickAHRSupdateIMU

вот код где это используется

Код:

   // Integrate rate of change of quaternion to yield quaternion
   q0 += qDot1 * (1.0f / sampleFreq);
   q1 += qDot2 * (1.0f / sampleFreq);
   q2 += qDot3 * (1.0f / sampleFreq);
   q3 += qDot4 * (1.0f / sampleFreq);

я добавил в функцию ещё один параметр deltat, и получилось так

Код:

   // Integrate rate of change of quaternion to yield quaternion
   q0 += qDot1 * deltat;
   q1 += qDot2 * deltat;
   q2 += qDot3 * deltat;
   q3 += qDot4 * deltat;

deltat -время с прошлого запуска, единица измерения секунда.
это влияет на точность интегрирования

grumman351

Подскажите пожалуйста, подставляю в фильтр Маджвика (http://x-io.co.uk/open-source-ahrs-with-x-imu/) данные акселерометра, гироскопа и магнитометра.(mpu6050, lsm303dlhc)

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)

Как бы с бубном не танцевал - получаются пляшущие данные меньше нуля :

Вложение:

Quaternion.jpg [ 135.29 КиБ | Просмотров: 4720 ]

Подставив эти данные в формулу, получаются нули.

roll = atan2( 2 *(q0*q1 + q2*q3), pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2));
pitch = asin(-2 *(q1*q3 - q0*q2) );
yaw = atan2((2*(q1*q2 + q0*q3)),( pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2)));

Как должны выглядеть входные данные акселерометра, гироскопа и магнитометра? (у меня с посчитанным смещением)
Как должны выглядеть данные кватернионов из фильтра Маджвика ?

Благодарю.

linvinus

гиро рад/с
аксель не важно, можно сырые данные скармливать
компас нужно калибровать долго и упорно, для начала возьмите функцию без компаса.
на выходе кватернионы, из них можно получить углы эйлера (вся математика в языках программирования по умолчанию в радианах)

roboforum.ru

Вычисление кватерниона из данных компаса и акселерометра

Кто сейчас на конференции