Непонятки с кватернионами, повороты не сходятся Опции

[alexPec]

Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 1с). Каждый шаг - интегрирование через кватернионы (см. код ниже). Смутило вот что: задаю скорости X и Z по 10 град/сек (обе сразу) и делаю шаг, получаю углы эйлера, затем последовательно делаю x = -10 град/сек, z=0, делаю шаг, затем x=0,z=-10 град/сек, делаю шаг, углы при этом в ноль не выходят. Это нормально?

Реализовал гироскоп через кватернионы в железе - такая ситуация:

Если наклонять плоскость гироскопа отдельно вдоль осей - все нормально, положишь в исходное положение - углы встают почти в 0. Дрейф ну совсем маленький. Если по кругу наклонять плоскость XY гироскопа (т.е. вперед, вперед-вправо, вправо, назад-вправо,назад, назад - влево, и т.д. по кругу) "по часовой стрелке" то через несколько кругов заметно прибывает угол по оси Z, когда ставишь в исходное положение. Если наклонять плоскость по кругу "против часовой стрелки", то при возврате в исходное положение угол по оси Z уменьшается, не дрейф точно, дрейф намного меньше. Почему так? Замечу: по оси Z вращения нет - только наклоны проскости XY. Отсчеты с гироскопа беру через регулярные промежутки времени, 2мс... В чем еще может быть затык?
Напрягает регулярность - по часовой - всегда прибывает угол, против - всегда убывает. Что-то с математикой неправильно?
В предыдущем топике выкладывал, но чтоб все под рукой было если кто захочет подсказать - вот еще раз код гироскопа:

Код

// Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = imu_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

тут q0-q3 - кватернион, gx,gy,gz - угловые скорости, переведенные в рад/сек.

Углы считаю так:

Код

angs[0]=atan2(2 * q1q2 - 2 * q0q3, 2 * q0q0 + 2 * q1q1 - 1) * 180/M_PI;
     angs[1]=-asin(2 * q1q3 + 2 * q0q2) * 180/M_PI;
     angs[2]=atan2(2 * q2q3 - 2 * q0q1, 2 * q0q0 + 2 * q3q3 - 1) * 180/M_PI;

[deeper_79]

Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 1с). Каждый шаг - интегрирование через кватернионы (см. код ниже). Смутило вот что: задаю скорости X и Z по 10 град/сек (обе сразу) и делаю шаг, получаю углы эйлера, затем последовательно делаю x = -10 град/сек, z=0, делаю шаг, затем x=0,z=-10 град/сек, делаю шаг, углы при этом в ноль не выходят. Это нормально?

Реализовал гироскоп через кватернионы в железе - такая ситуация:

Если наклонять плоскость гироскопа отдельно вдоль осей - все нормально, положишь в исходное положение - углы встают почти в 0. Дрейф ну совсем маленький. Если по кругу наклонять плоскость XY гироскопа (т.е. вперед, вперед-вправо, вправо, назад-вправо,назад, назад - влево, и т.д. по кругу) "по часовой стрелке" то через несколько кругов заметно прибывает угол по оси Z, когда ставишь в исходное положение. Если наклонять плоскость по кругу "против часовой стрелки", то при возврате в исходное положение угол по оси Z уменьшается, не дрейф точно, дрейф намного меньше. Почему так? Замечу: по оси Z вращения нет - только наклоны проскости XY. Отсчеты с гироскопа беру через регулярные промежутки времени, 2мс... В чем еще может быть затык?
Напрягает регулярность - по часовой - всегда прибывает угол, против - всегда убывает. Что-то с математикой неправильно?
В предыдущем топике выкладывал, но чтоб все под рукой было если кто захочет подсказать - вот еще раз код гироскопа:

Код

// Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = imu_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

тут q0-q3 - кватернион, gx,gy,gz - угловые скорости, переведенные в рад/сек.

Углы считаю так:

Код

angs[0]=atan2(2 * q1q2 - 2 * q0q3, 2 * q0q0 + 2 * q1q1 - 1) * 180/M_PI;
     angs[1]=-asin(2 * q1q3 + 2 * q0q2) * 180/M_PI;
     angs[2]=atan2(2 * q2q3 - 2 * q0q1, 2 * q0q0 + 2 * q3q3 - 1) * 180/M_PI;

Все так и должно быть, смотрите уравнения угловых скоростей в связанной системе координат
плюс еще погрешности неортогональности осей чувстиетльности гироскопов, ну и еще поправки Севеджа но здесь они должны быть очень малы, гараздо меньше дрейфа

Все так и должно быть, смотрите уравнения угловых скоростей в связанной системе координат
плюс еще погрешности неортогональности осей чувстиетльности гироскопов, ну и еще поправки Севеджа но здесь они должны быть очень малы, гараздо меньше дрейфа

поторопилься сорри

у вас тут ошибки одни как мне кажеться

Fx, Fy, Fz - интегралы от гирскопов, тогда

FF2=sqrt(Fx^2+Fy^2+Fz^2)
if(FF2=0) FF3=0.5
else FF3=sin(FF2*0.5)/FF2
поулчаем кватернион
H[0]=cos(FF2*0.5);
H[1]=FF3*Fx;
H[2]=FF3*Fy;
H[3]=FF3*Fz;

//Матрица направляющих косинусов связывающая ГСК и объект (это кусок из кода в место L0a[0] подстваить Н[0] и так далее )
C0a[2][2] = L0a[0]*L0a[0]+ L0a[2]*L0a[2]- L0a[1]*L0a[1]- L0a[3]*L0a[3];
C0a[1][2] = 2.*(L0a[1]*L0a[2] + L0a[0]*L0a[3]);
C0a[3][1] = 2.*(L0a[1]*L0a[3] + L0a[0]*L0a[2]);
C0a[3][2] = 2.*(L0a[2]*L0a[3] - L0a[0]*L0a[1]);
C0a[3][3] = L0a[0]*L0a[0]+ L0a[3]*L0a[3]- L0a[1]*L0a[1]- L0a[2]*L0a[2] ;

// Находим углы ориентации и передаем их на выход
psi_A = asin(C0a[3][2]) ;
if (fabs(C0a[3][3]) < 0.001) psi_A = M_PI*0.5 ;

tet_A = -atan2(C0a[3][1],C0a_A[3][3]);

KURa_A = atan2(C0a[1][2], C0a[2][2]);
if (KURa_A < 0.0) KURa_A += 2.*M_PI;


это будет как бы трехосный свободный гироскоп

[alexPec]

поторопилься сорри

у вас тут ошибки одни как мне кажеться

Fx, Fy, Fz - интегралы от гирскопов, то

1. Интегралы от гироскопов это подразумевается угловая скорость помноженная на dt?

2.Тут тоже непонятно:

FF2=sqrt(Fx^2+Fy^2+Fz^2)
if(FF2=0) FF3=0.5
else FF3=sin(FF2*0.5)/FF2

Это вроде как нормирующий множитель, но непонятно откуда sin взялся.

3. Нормируем интегралы скоростей (если верно п.1), а у Вас написано что получаем кватернион:
H[0]=cos(FF2*0.5);
H[1]=FF3*Fx;
H[2]=FF3*Fy;
H[3]=FF3*Fz;

Умножаем на существующий кватернион на получившийся трехмерный нормированный угол:

//Матрица направляющих косинусов связывающая ГСК и объект (это кусок из кода в место L0a[0] подстваить Н[0] и так далее )
C0a[2][2] = L0a[0]*L0a[0]+ L0a[2]*L0a[2]- L0a[1]*L0a[1]- L0a[3]*L0a[3];
C0a[1][2] = 2.*(L0a[1]*L0a[2] + L0a[0]*L0a[3]);
C0a[3][1] = 2.*(L0a[1]*L0a[3] + L0a[0]*L0a[2]);
C0a[3][2] = 2.*(L0a[2]*L0a[3] - L0a[0]*L0a[1]);
C0a[3][3] = L0a[0]*L0a[0]+ L0a[3]*L0a[3]- L0a[1]*L0a[1]- L0a[2]*L0a[2] ;

Если математика кватернионная почему 5 параметров? У меня все сделано вроде как по науке:

gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);


На входе кватернион и на выходе, 4 параметра на входе помножаем на угловые скорости, на dt на 0.5 и получаем выходные 4 параметра.

Правило умножения взято из книжки(см рис), умножение кватернионов это я та понимаю и есть поворот одного на другой, если можно так выразится. В Вашем файле собственно тоже самое (см. второй рис) - умножаем кватернион H на омега (только почему на омега? Надо вроде как на омега*dt?). Ну и углы ориентации это вторично, главное чтобы кватернион считался правильно. Меня это и интересует. Правильно ли считаю? Углы ориентации то можно и не считать в основном цикле, изредка делать преобразование кватерниона в углы и все.

Эскизы прикрепленных изображений

 

[deeper_79]

1. да это так


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

3. это не параметры кватерниона а значения матрицы направляющих косинусов между ГСК и ССК, элементы матрицы составлены из параметров кватерниов, т.е. переход от парметров кватерниона к углам

4. на омега это дифференциальное уравнение см. выше