Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bugfix quaternion from euler #43

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Bugfix quaternion from euler #43

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7a8e909
BUGFIX: Wrong sign in caldulation
realtimepeople Mar 3, 2025
471674b
BUGFIX: Wrong sign in calculation
realtimepeople Mar 3, 2025
fdfbfb9
Merge branch 'BUGFIX_quaternion_from_euler' of [email protected]:larus-b…
realtimepeople Mar 3, 2025
9d14bc7
Source simplified.
realtimepeople Mar 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
123 changes: 62 additions & 61 deletions Generic_Algorithms/quaternion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,23 +51,23 @@ template <class datatype > class quaternion: public vector <datatype, 4>
//! normalize quaternion absolute value to ONE
inline void normalize(void)
{
datatype tmp = vector<datatype, 4>::e[0] * vector<datatype, 4>::e[0]
+ vector<datatype, 4>::e[1] * vector<datatype, 4>::e[1]
+ vector<datatype, 4>::e[2] * vector<datatype, 4>::e[2]
+ vector<datatype, 4>::e[3] * vector<datatype, 4>::e[3] ;
datatype tmp = this->e[0] * this->e[0]
+ this->e[1] * this->e[1]
+ this->e[2] * this->e[2]
+ this->e[3] * this->e[3] ;
tmp = ONE / tmp;
tmp = SQRT( tmp);
for( int i = 0; i < 4; ++i)
vector<datatype, 4>::e[i] *= tmp;
this->e[i] *= tmp;
};

//! quaternion -> euler angle transformation
operator eulerangle <datatype> () const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];

eulerangle <datatype> _euler;

Expand All @@ -80,49 +80,49 @@ template <class datatype > class quaternion: public vector <datatype, 4>
//! linearized euler component e0 (approximate nick angle / rad)
inline datatype lin_e0( void) const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];
return TWO * (e0*e1 + e2*e3) / ( e0*e0 - e1*e1 - e2*e2 + e3*e3);

}
//! linearized euler component e1 (approximate roll angle / rad)
inline datatype lin_e1( void) const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];
return TWO * (e0*e2 - e3*e1);
}
//! euler component e2
inline datatype get_e2( void) const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];
return ATAN2( TWO * (e0*e3 + e1*e2) , e0*e0 + e1*e1 - e2*e2 - e3*e3 );
}

//! get north component of attitude
inline datatype get_north( void) const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];
return e0*e0 + e1*e1 - e2*e2 - e3*e3;
}

//! get east component of attitude
inline datatype get_east( void) const
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];
return TWO * (e0*e3 + e1*e2);
}

Expand All @@ -139,23 +139,24 @@ template <class datatype > class quaternion: public vector <datatype, 4>
//! quaternion update using rotation vector
void rotate( datatype p, datatype q, datatype r)
{
datatype e0 = vector<datatype, 4>::e[0];
datatype e1 = vector<datatype, 4>::e[1];
datatype e2 = vector<datatype, 4>::e[2];
datatype e3 = vector<datatype, 4>::e[3];
datatype e0 = this->e[0];
datatype e1 = this->e[1];
datatype e2 = this->e[2];
datatype e3 = this->e[3];

//! R.Rogers formula 2.92
vector<datatype, 4>::e[0] += (-e1*p -e2*q -e3*r);
vector<datatype, 4>::e[1] += ( e0*p +e2*r -e3*q);
vector<datatype, 4>::e[2] += ( e0*q -e1*r +e3*p);
vector<datatype, 4>::e[3] += ( e0*r +e1*q -e2*p);
this->e[0] += (-e1*p -e2*q -e3*r);
this->e[1] += ( e0*p +e2*r -e3*q);
this->e[2] += ( e0*q -e1*r +e3*p);
this->e[3] += ( e0*r +e1*q -e2*p);

normalize();
}

//! euler angle -> quaternion transformation
void from_euler( datatype p, datatype q, datatype r)
{
// 3-2-1: yaw, then pitch, then roll
p *= HALF; q *= HALF; r *= HALF;
datatype sinphi = SIN( p);
datatype cosphi = COS( p);
Expand All @@ -164,20 +165,20 @@ template <class datatype > class quaternion: public vector <datatype, 4>
datatype sinpsi = SIN( r);
datatype cospsi = COS( r);

vector<datatype, 4>::e[0] = cosphi*costheta*cospsi + sinphi*sintheta*sinpsi;
vector<datatype, 4>::e[1] = sinphi*costheta*cospsi + cosphi*sintheta*sinpsi;
vector<datatype, 4>::e[2] = cosphi*sintheta*cospsi + sinphi*costheta*sinpsi;
vector<datatype, 4>::e[3] = cosphi*costheta*sinpsi - sinphi*sintheta*cospsi;
this->e[0] = cosphi*costheta*cospsi + sinphi*sintheta*sinpsi;
this->e[1] = sinphi*costheta*cospsi - cosphi*sintheta*sinpsi;
this->e[2] = cosphi*sintheta*cospsi + sinphi*costheta*sinpsi;
this->e[3] = cosphi*costheta*sinpsi - sinphi*sintheta*cospsi;
}

//! quaternion -> rotation matrix transformation
void get_rotation_matrix (matrix<datatype, 3> &m) const
{
//! R.Rogers formula 2.90
datatype e0=vector<datatype, 4>::e[0];
datatype e1=vector<datatype, 4>::e[1];
datatype e2=vector<datatype, 4>::e[2];
datatype e3=vector<datatype, 4>::e[3];
datatype e0=this->e[0];
datatype e1=this->e[1];
datatype e2=this->e[2];
datatype e3=this->e[3];

m.e[0][0] = TWO * (e0*e0+e1*e1) - ONE;
m.e[0][1] = TWO * (e1*e2-e0*e3);
Expand All @@ -194,18 +195,18 @@ template <class datatype > class quaternion: public vector <datatype, 4>
quaternion <datatype> operator * ( quaternion <datatype> & right) //!< quaternion multiplication
{
quaternion <datatype> result;
datatype e0=vector<datatype, 4>::e[0];
datatype e1=vector<datatype, 4>::e[1];
datatype e2=vector<datatype, 4>::e[2];
datatype e3=vector<datatype, 4>::e[3];
datatype re0=right.vector<datatype, 4>::e[0];
datatype re1=right.vector<datatype, 4>::e[1];
datatype re2=right.vector<datatype, 4>::e[2];
datatype re3=right.vector<datatype, 4>::e[3];
result.vector<datatype, 4>::e[0] = e0 * re0 - e1 * re1 - e2 * re2 + e3 * re3;
result.vector<datatype, 4>::e[1] = e1 * re0 + e2 * re3 - e3 * re2 + e0 * re1;
result.vector<datatype, 4>::e[2] = e3 * re1 + e0 * re2 - e1 * re3 + e2 * re0;
result.vector<datatype, 4>::e[3] = e1 * re2 - e2 * re1 + e0 * re3 + e3 * re0;
datatype e0=this->e[0];
datatype e1=this->e[1];
datatype e2=this->e[2];
datatype e3=this->e[3];
datatype re0=right.e[0];
datatype re1=right.e[1];
datatype re2=right.e[2];
datatype re3=right.e[3];
result.e[0] = e0 * re0 - e1 * re1 - e2 * re2 + e3 * re3;
result.e[1] = e1 * re0 + e2 * re3 - e3 * re2 + e0 * re1;
result.e[2] = e3 * re1 + e0 * re2 - e1 * re3 + e2 * re0;
result.e[3] = e1 * re2 - e2 * re1 + e0 * re3 + e3 * re0;
return result;
}

Expand All @@ -216,11 +217,11 @@ template <class datatype > class quaternion: public vector <datatype, 4>
//! formula from roenbaeck p35
tmp = SQRT( tmp);
tmp *= HALF;
vector<datatype, 4>::e[0] = tmp;
this->e[0] = tmp;
tmp = QUARTER / tmp;
vector<datatype, 4>::e[1] = tmp * (rotm.e[2][1] - rotm.e[1][2]);
vector<datatype, 4>::e[2] = tmp * (rotm.e[0][2] - rotm.e[2][0]);
vector<datatype, 4>::e[3] = tmp * (rotm.e[1][0] - rotm.e[0][1]);
this->e[1] = tmp * (rotm.e[2][1] - rotm.e[1][2]);
this->e[2] = tmp * (rotm.e[0][2] - rotm.e[2][0]);
this->e[3] = tmp * (rotm.e[1][0] - rotm.e[0][1]);
normalize(); // compensate computational inaccuracies
};
};
Expand Down
90 changes: 90 additions & 0 deletions NAV_Algorithms/setup_tester.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
#include "float3vector.h"
#include "float3matrix.h"
#include "quaternion.h"

const float ldi[]={0.1f, +0.25f, -10.0f}; // left wing down
const float rdi[]={0.1f, -0.15f, -10.0f}; // right wing down
const float hi[] ={0.0f, 0.0f, -10.0f}; // horizontal

const float test1[] ={ 1.0f, 0.0f, 0.0f};
const float test2[] ={ 0.57735f, 0.57735f, -0.57735f};
const float test3[] ={ 0.57735f, -0.57735f, 0.57735f};

void setup_tester(void)
{
// create measurements in aircraft body frame
float3vector left_down_body( ldi);
float3vector right_down_body( rdi);
float3vector horiz_body( hi);

// setup the sensor orientation to test
quaternion<float>sensor_q;
// sensor_q.from_euler( 45.0 * M_PI/180.0, 135.0 * M_PI/180.0, -30.0 * M_PI/180.0);
// sensor_q.from_euler( -135.0 * M_PI/180.0, 45.0 * M_PI/180.0, 150.0 * M_PI/180.0);
// sensor_q.from_euler( -10.0 * M_PI/180.0, 75.0 * M_PI/180.0, -45.0 * M_PI/180.0);
sensor_q.from_euler( 10.0 * M_PI/180.0, -75.0 * M_PI/180.0, +125.0 * M_PI/180.0);

float3matrix sensor_2_body_assumption;
sensor_q.get_rotation_matrix(sensor_2_body_assumption);

//check if we get the rotation angles using the matrix given
quaternion<float> qin;
qin.from_rotation_matrix(sensor_2_body_assumption);
eulerangle<float> euler_in = qin;
euler_in.r *= 180/M_PI;
euler_in.p *= 180/M_PI;
euler_in.y *= 180/M_PI;

// map measurements into the sensor frame
float3vector left_down_sensor = sensor_2_body_assumption.reverse_map( left_down_body);
float3vector right_down_sensor= sensor_2_body_assumption.reverse_map( right_down_body);
float3vector horiz_sensor = sensor_2_body_assumption.reverse_map( horiz_body);

// resolve sensor orientation using measurements
float3vector front_down_sensor_helper = right_down_sensor.vector_multiply(left_down_sensor);
float3vector u_right_sensor = front_down_sensor_helper.vector_multiply(horiz_sensor);
u_right_sensor.normalize();

float3vector u_down_sensor = horiz_sensor * -1.0f;
u_down_sensor.normalize();

float3vector u_front_sensor=u_right_sensor.vector_multiply(u_down_sensor);
u_front_sensor.normalize();

// calculate the rotation matrix using our calibration data
float3matrix sensor_2_body;
sensor_2_body.e[0][0]=u_front_sensor[0];
sensor_2_body.e[0][1]=u_front_sensor[1];
sensor_2_body.e[0][2]=u_front_sensor[2];

sensor_2_body.e[1][0]=u_right_sensor[0];
sensor_2_body.e[1][1]=u_right_sensor[1];
sensor_2_body.e[1][2]=u_right_sensor[2];

sensor_2_body.e[2][0]=u_down_sensor[0];
sensor_2_body.e[2][1]=u_down_sensor[1];
sensor_2_body.e[2][2]=u_down_sensor[2];

// test if the rotation matrix recovers the body frame data
float3vector test_left_down_body = sensor_2_body * left_down_sensor;
float3vector test_right_down_body = sensor_2_body * right_down_sensor;
float3vector test_horiz_body = sensor_2_body * horiz_sensor;

// test if the matrix provides pure rotation
float3vector vtest1 = sensor_2_body * float3vector( test1);
float test1_abs=vtest1.abs();
float3vector vtest2 = sensor_2_body * float3vector( test2);
float test2_abs=vtest2.abs();
float3vector vtest3 = sensor_2_body * float3vector( test3);
float test3_abs=vtest3.abs();

//check if we get the rotation angles using the matrix we have calculated
quaternion<float> q;
q.from_rotation_matrix(sensor_2_body);
eulerangle<float> euler = q;
euler.r *= 180/M_PI;
euler.p *= 180/M_PI;
euler.y *= 180/M_PI;
}