Добрый день. Сделал модельку, которая считает углы эйлера для объекта. Поворот в модели задаю угловыми скоростями (в град/сек, вдоль каждой оси, затем кнопочкой имитирую шаг (условно 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;
Непонятки с кватернионами, повороты не сходятся
Sep 15 2013, 19:43
