Converting vector from relative coordinate system to an absolute one

Question

I'm trying to make a quadcopter drone. I'm using an mpu6050 to get acceleration & angular speed and then convert them to Roll / pitch / yaw

With the acceleration, i'm also trying to get the speed & position by integration. However , i need them to be in absolute coordinate system, because the mpu6050 gives you the values in its relative coordinates system.

the origin of the new coordinate system is the starting position of the drone, and the direction is "where the drone is looking" , we assume Yaw = 0 at the beginning, and we get yaw using the gyrometer's data.

I tried to rotate the vectors using the Roll / pitch values, but that doesn't seem to work very well.

I tried with this being the gravity vector for example : (-2, -2, -1) If i convert it to the absolute coordinate system, i should get : (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;
}

expected result ( 0, 0, 3 ) Actual results : (-0.104919 , -0.824045, -2.8827 )

Are you aware of atan2() ? And the reasons to use it, i.e. the ambiguity of using atan? — Yunnosch, Feb 08 '19 at 11:11
the `static_cast` is a bit fishy here. Did you put it to silence a warning or error? — 463035818_is_not_an_ai, Feb 08 '19 at 11:14
Thanks for the information, i just googled the difference : https://stackoverflow.com/questions/283406/what-is-the-difference-between-atan-and-atan2-in-c — Mohamed Amine Brouk, Feb 08 '19 at 11:16
i replaced the atan with atan2 , but i still get the same results, and even if i delete the statitc_cast , it's the same result — Mohamed Amine Brouk, Feb 08 '19 at 11:17
I posted the full code , including the RotateX , RotateY. I already tested the functions and they seem to be working. — Mohamed Amine Brouk, Feb 08 '19 at 11:36
Should the transformation not be independent on vec? You compute some transformation dependent on your vector, but in my opioning this does not make any sense. If this indeed what you want, just compute length of vector and do length * direction and you are done — , Feb 08 '19 at 11:37
How do you compute the direction vector then ? in the case of the gravity, we already know it's ( 0, 0, 1 ), but if the drone is moving , it won't be the same. — Mohamed Amine Brouk, Feb 08 '19 at 11:44
This is what I tried to say, you need a transformation independent of vec — , Feb 08 '19 at 11:46

Converting vector from relative coordinate system to an absolute one

0 Answers0