roboforum.ru

linvinus писал(а):

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

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

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

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

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

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

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

Наткнулся на аналогичную проблему. Имею датчик 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;

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

Максим.

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

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

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

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

вот рабочий пример 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);

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

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

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

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

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

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

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

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

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

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

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

вообще более правильно туда подставлять реально время сколько прошло с момента прошлого запуска 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 -время с прошлого запуска, единица измерения секунда.
это влияет на точность интегрирования

Подскажите пожалуйста, подставляю в фильтр Маджвика (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)

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



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

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)));

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

Благодарю.

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