Извлечение рыскания из кватерниона
lsof -i предоставит список сокетов открытой сети. -n опция предотвратит поиски DNS, который полезен, когда Ваше сетевое соединение является медленным или ненадежным.
2 ответа
Кватернионное представление вращения - это изменение оси и угла. Так что если вы поворачиваете на r радиан вокруг оси x , y , z , то ваш кватернион q равен :
q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;
Если вы хотите создать кватернион, который вращается только вокруг оси y , вы обнуляете x и z осей, а затем повторно нормализовать кватернион:
q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;
Если вы хотите получить результирующий угол:
double ang = 2*acos(q[0]);
Предполагается, что кватернионное представление сохраняется: w, x, y, z , Если оба q [0] и q [2] равны нулю или близки к нему, результирующий кватернион должен быть просто {1,0,0,0}.
Примечание : я проверил приведенный ниже код по уравнениям Википедии плюс документации Pixhawk , и это правильно.
Если вы работаете с дронами / авиацией, ниже приведен код (взят непосредственно из DJI SDK ). Здесь q0, q1, q2, q3 соответствуют w, x, y, z компонентам кватерниона соответственно. Также обратите внимание, что рыскание, наклон, крен могут упоминаться как заголовок, отношение и крен соответственно в некоторой литературе.
float roll = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));
Если вам нужно вычислить все 3, то вы можете избежать пересчета общих терминов, используя следующие функции:
//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
EulerianAngle ans;
double q2sqr = data.q2 * data.q2;
double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
ans.pitch = asin(t2);
ans.roll = atan2(t3, t4);
ans.yaw = atan2(t1, t0);
return ans;
}
QuaternionData Flight::toQuaternion(EulerianAngle data)
{
QuaternionData ans;
double t0 = cos(data.yaw * 0.5);
double t1 = sin(data.yaw * 0.5);
double t2 = cos(data.roll * 0.5);
double t3 = sin(data.roll * 0.5);
double t4 = cos(data.pitch * 0.5);
double t5 = sin(data.pitch * 0.5);
ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
return ans;
}
Заметка о собственной библиотеке
Если вы используете библиотеку Eigen, у нее есть другой способ сделать это преобразование, однако, это может быть не так оптимизировано, как приведенный выше прямой код:
Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler[0]; pitch = euler[1]; roll = euler[2];