Преобразование вектора из относительной системы координат в абсолютную

Question

Я пытаюсь сделать дрон горючего.Я использую mpu6050, чтобы получить ускорение и угловую скорость, а затем преобразовать их в Roll / pitch / yaw

. С ускорением я также пытаюсь получить скорость и положение путем интеграции.Однако мне нужно, чтобы они были в абсолютной системе координат, потому что mpu6050 дает вам значения в своей относительной системе координат.

источником новой системы координат является начальная позиция дрона, а направление«где дрон смотрит», мы предполагаем, что Yaw = 0 в начале, и мы получаем рыскание, используя данные гирометра.

Я пытался вращать векторы, используя значения Roll / Pitch, но это некажется, работает очень хорошо.

Я пытался с этим, как вектор гравитации, например: (-2, -2, -1) Если я преобразовать его в абсолютную систему координат, я должен получить: (0,0, 3)

#include <iostream>

using namespace std;

#include <math.h>

// Vector class, to handle all the vector operations for us
// Thanks to : https://stackoverflow.com/questions/14607640/rotating-a-vector-in-3d-space
class cVector
{
public:
    float x;
    float y;
    float z;
    // Constructor
    cVector();
    cVector(float x1, float y1, float z1);

    // returns the vector's magnitude
    float Magnitude();

    // Normalize ( change length to 1, while keeping the same direction)
    void Normalize();


    // Rotate around the Axis
    void RotateX(float angle);
    void RotateY(float angle);
    void RotateZ(float angle);

    // TODO : Add operators for Addition & Substraction

    // Addition
    cVector operator+(cVector const& v1) const
    {
        return cVector(x + v1.x,
                        y + v1.y,
                        z + v1.z);
    }
    void operator+=(cVector const& v1)
    {
        x += v1.x;
        y += v1.y;
        z += v1.z;
    }
    // Substraction
    cVector operator-(cVector const& v1) const
    {
        return cVector(x - v1.x,
                        y - v1.y,
                        z - v1.z);
    }
    void operator-=(cVector const& v1)
    {
        x -= v1.x;
        y -= v1.y;
        z -= v1.z;
    }

    // Multiplication
    void operator*=(const float scalar)
    {
        x *= scalar;
        y *= scalar;
        z *= scalar;
    }
    cVector operator*(const float scalar) const
    {
        return cVector(x * scalar,
                        y * scalar,
                        z * scalar);
    }

    // Division
    void operator/=(const float scalar)
    {
        x /= scalar;
        y /= scalar;
        z /= scalar;
    }
    cVector operator/(const float scalar) const
    {
        return cVector(x / scalar,
                        y / scalar,
                        z / scalar);
    }
};

// Constructor
cVector::cVector()
{

}

cVector::cVector(float x1, float y1, float z1)
{
    x = x1;
    y = y1;
    z = z1;
}

// returns the vector's magnitude
float cVector::Magnitude()
{
    return sqrt((x * x) + (y * y) + (z * z));
}

// Normalize ( change length to 1, while keeping the same direction)
void cVector::Normalize()
{
    float flMagnitude = Magnitude();

    // We devide the coordinates by the magnitude
    x /= flMagnitude;
    y /= flMagnitude;
    z /= flMagnitude;
}

// Rotate around the Axis
void cVector::RotateX(float angle)
{
    // Calculate the sinus and cosinus
    float flCos = static_cast<float>(cos(angle));
    float flSin = static_cast<float>(sin(angle));

    // We save the current values temporarily
    float _y = y;
    float _z = z;

    y = _y * flCos - _z * flSin;
    z = _y * flSin + _z * flCos;
}

void cVector::RotateY(float angle)
{
    // Calculate the sinus and cosinus
    float flCos = static_cast<float>(cos(angle));
    float flSin = static_cast<float>(sin(angle));

    // We save the current values temporarily
    float _x = x;
    float _z = z;

    x = _x * flCos + _z * flSin;
    z = - _x * flSin + _z * flCos;
}

void cVector::RotateZ(float angle)
{
    // Calculate the sinus and cosinus
    float flCos = static_cast<float>(cos(angle));
    float flSin = static_cast<float>(sin(angle));

    // We save the current values temporarily
    float _x = x;
    float _y = y;

    x = _x * flCos - _y * flSin;
    y = _x * flSin + _y * flCos;
}


void PrintVector(cVector vec)
{
    cout << "X : " << vec.x << " Y : " << vec.y << " Z : " << vec.z << endl;
}

// TODO : Add operators for Addition & Substraction
int main()
{

    cVector vec(-2, -2, -1);
    // Calculate pitch / roll
     float pitch = static_cast<float>(atan2( vec.y , sqrt( pow(vec.x,2) + pow(vec.z,2) ) ));
     float roll = static_cast<float>(atan2(-1 * vec.x , sqrt( pow(vec.y,2) + pow(vec.z,2) ) ));

    // vec.RotateY(1.570796f);

    vec.RotateX(roll);
    vec.RotateY(pitch);

    PrintVector(vec);

    cin.get();
    return 0;
}

ожидаемый результат (0, 0, 3) Фактические результаты: (-0,104919, -0,824045, -2,8827)