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

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