将向量从相对坐标系转换为绝对坐标系

Question

我正在尝试制造四旋翼无人机。我正在使用mpu6050来获取加速度和角速度，然后将它们转换为滚动/俯仰/偏航

随着加速度的增加，我也试图通过积分获得速度和位置。但是，我需要它们位于绝对坐标系中，因为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）