Как преобразовать кватернионные значения в определенные углы - PullRequest
Новая
       47

Как преобразовать кватернионные значения в определенные углы

0 голосов
/ 15 января 2019

Я получаю кватернионные значения и хочу преобразовать эти значения в 2 угла.
Один из углов находится между «земной плоскостью» (пожалуйста, поправьте меня, если это неправильное название для нее) и вымышленным вектором между началом координат и точкой кватерниона (дополнительный угол гаммы).
Второй угол - это угол поворота, то есть поворот, который вектор совершил относительно плоскости, перпендикулярной вектору (на рисунке R).

Я много смотрел в интернете, но все, что я пробую, имеет диапазон от -90 ° до 90 °, а я хочу, чтобы он шел от -180 ° до 180 °.

Это последняя версия кода, с которым я закончил: 

float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan(gx / sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan(gy/gz); // about X axis

Serial.print("  yaw ");
Serial.print(yaw * 180/M_PI,0);
Serial.print("  pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print("  sideways ");
// Don't mind this section as I'm applying a mathematical function I created
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);

Picture

1 Ответ

0 голосов
/ 16 января 2019

Вы должны всегда использовать atan2 (y, x) вместо atan (y / x). Это распространенная ошибка. - сомос

Он написал это в математической форме, где я тоже об этом спрашивал, и это была моя глупая ошибка -_-

Моя новая версия: 

float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan2(gx, sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan2(gy, gz); // about X axis

/*Serial.print("  yaw ");
Serial.print(yaw * 180/M_PI,0);*/
Serial.print("  pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print("  sideways ");
// Please don't pay  attention to the extra function I made for the project but it doesn't have to do with the problem
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);
...