Euler angles and Quaternion convention

2022-06-19 552 words 3 minutes

Contents

Transfer between Euler angles to Quaternion

Euler angles to quaternion convention

Formula

\[ \begin{aligned} \mathbf{q}_{\mathrm{IB}} &=\left[\begin{array}{c}\cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2)\end{array}\right]\left[\begin{array}{c}\cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0\end{array}\right]\left[\begin{array}{c}\cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0\end{array}\right] \\ &=\left[\begin{array}{l}\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)\end{array}\right] \end{aligned} \]

Code

#include <iostream>
#include <cmath>
struct Quaternion
{
    double w, x, y, z;
};

Quaternion ToQuaternion(double yaw, double pitch, double roll)
{
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;
    return q;
}
int main() {
    double pi = 3.1415;
    double yaw = pi/2;
    double pitch = pi/3;
    double roll = pi/4;
    Quaternion res = ToQuaternion(yaw, pitch, roll);
    printf("w: %f, x: %f, y: %f, z: %f", res.w, res.x, res.y, res.z);

}

w: 0.701068, x: -0.092280, y: 0.560976, z: 0.430458

Quaternions to Euler angles

Formula

\[ \left[\begin{array}{c}\phi \\ \theta \\ \psi\end{array}\right]=\left[\begin{array}{c}\arctan \frac{2\left(q_{0} q_{1}+q_{2} q_{3}\right)}{1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \\ \arcsin \left(2\left(q_{0} q_{2}-q_{3} q_{1}\right)\right) \\ \arctan \frac{2\left(q_{0} q_{3}+q_{1} q_{2}\right)}{1-2\left(q_{2}^{2}+q_{3}^{2}\right)}\end{array}\right] \]

Code

#include <cmath>
#include <iostream>
struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}
int main() {
    // test the code with previous results
    double w = 0.701068;
    double x = -0.092280;
    double y = 0.560976;
    double z = 0.430458;
    double pi = 3.1415;
    Quaternion q;
    q.w = w;
    q.x = x;
    q.y = y;
    q.z = z;
    EulerAngles e = ToEulerAngles(q);
    printf("roll: %f, pitch: %f, yaw: %f\n", e.roll, e.pitch, e.yaw);
    printf("pi/2: %f, pi/3: %f, pi/4: %f", pi/2, pi/3, pi/4);
}

roll: 0.785375, pitch: 1.047167, yaw: 1.570749
pi/2: 1.570750, pi/3: 1.047167, pi/4: 0.785375