From 7a8e9095c400f63ff2a6633a1a6129622a4abfca Mon Sep 17 00:00:00 2001 From: realtimepeople Date: Mon, 3 Mar 2025 21:48:25 +0100 Subject: [PATCH 1/3] BUGFIX: Wrong sign in caldulation Euler angles -> quaternion --- Generic_Algorithms/quaternion.h | 3 +- NAV_Algorithms/setup_tester.cpp | 90 +++++++++++++++++++++++++++++++++ 2 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 NAV_Algorithms/setup_tester.cpp diff --git a/Generic_Algorithms/quaternion.h b/Generic_Algorithms/quaternion.h index 882a594..1570fb1 100644 --- a/Generic_Algorithms/quaternion.h +++ b/Generic_Algorithms/quaternion.h @@ -156,6 +156,7 @@ template class quaternion: public vector //! 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); @@ -165,7 +166,7 @@ template class quaternion: public vector datatype cospsi = COS( r); vector::e[0] = cosphi*costheta*cospsi + sinphi*sintheta*sinpsi; - vector::e[1] = sinphi*costheta*cospsi + cosphi*sintheta*sinpsi; + vector::e[1] = sinphi*costheta*cospsi - cosphi*sintheta*sinpsi; vector::e[2] = cosphi*sintheta*cospsi + sinphi*costheta*sinpsi; vector::e[3] = cosphi*costheta*sinpsi - sinphi*sintheta*cospsi; } diff --git a/NAV_Algorithms/setup_tester.cpp b/NAV_Algorithms/setup_tester.cpp new file mode 100644 index 0000000..5a71bc7 --- /dev/null +++ b/NAV_Algorithms/setup_tester.cpp @@ -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 + quaternionsensor_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 qin; + qin.from_rotation_matrix(sensor_2_body_assumption); + eulerangle 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 q; + q.from_rotation_matrix(sensor_2_body); + eulerangle euler = q; + euler.r *= 180/M_PI; + euler.p *= 180/M_PI; + euler.y *= 180/M_PI; +} + + From 471674b62a8e283b9a7ba5da67e78e12a74b72ba Mon Sep 17 00:00:00 2001 From: realtimepeople Date: Mon, 3 Mar 2025 21:48:25 +0100 Subject: [PATCH 2/3] BUGFIX: Wrong sign in calculation Euler angles -> quaternion --- Generic_Algorithms/quaternion.h | 3 +- NAV_Algorithms/setup_tester.cpp | 90 +++++++++++++++++++++++++++++++++ 2 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 NAV_Algorithms/setup_tester.cpp diff --git a/Generic_Algorithms/quaternion.h b/Generic_Algorithms/quaternion.h index 882a594..1570fb1 100644 --- a/Generic_Algorithms/quaternion.h +++ b/Generic_Algorithms/quaternion.h @@ -156,6 +156,7 @@ template class quaternion: public vector //! 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); @@ -165,7 +166,7 @@ template class quaternion: public vector datatype cospsi = COS( r); vector::e[0] = cosphi*costheta*cospsi + sinphi*sintheta*sinpsi; - vector::e[1] = sinphi*costheta*cospsi + cosphi*sintheta*sinpsi; + vector::e[1] = sinphi*costheta*cospsi - cosphi*sintheta*sinpsi; vector::e[2] = cosphi*sintheta*cospsi + sinphi*costheta*sinpsi; vector::e[3] = cosphi*costheta*sinpsi - sinphi*sintheta*cospsi; } diff --git a/NAV_Algorithms/setup_tester.cpp b/NAV_Algorithms/setup_tester.cpp new file mode 100644 index 0000000..5a71bc7 --- /dev/null +++ b/NAV_Algorithms/setup_tester.cpp @@ -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 + quaternionsensor_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 qin; + qin.from_rotation_matrix(sensor_2_body_assumption); + eulerangle 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 q; + q.from_rotation_matrix(sensor_2_body); + eulerangle euler = q; + euler.r *= 180/M_PI; + euler.p *= 180/M_PI; + euler.y *= 180/M_PI; +} + + From 9d14bc745e232edd598e40520a27c6f386f7a9d1 Mon Sep 17 00:00:00 2001 From: realtimepeople Date: Mon, 3 Mar 2025 23:15:53 +0100 Subject: [PATCH 3/3] Source simplified. --- Generic_Algorithms/quaternion.h | 122 ++++++++++++++++---------------- 1 file changed, 61 insertions(+), 61 deletions(-) diff --git a/Generic_Algorithms/quaternion.h b/Generic_Algorithms/quaternion.h index 1570fb1..ac2139d 100644 --- a/Generic_Algorithms/quaternion.h +++ b/Generic_Algorithms/quaternion.h @@ -51,23 +51,23 @@ template class quaternion: public vector //! normalize quaternion absolute value to ONE inline void normalize(void) { - datatype tmp = vector::e[0] * vector::e[0] - + vector::e[1] * vector::e[1] - + vector::e[2] * vector::e[2] - + vector::e[3] * vector::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::e[i] *= tmp; + this->e[i] *= tmp; }; //! quaternion -> euler angle transformation operator eulerangle () const { - datatype e0 = vector::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::e[3]; + datatype e0 = this->e[0]; + datatype e1 = this->e[1]; + datatype e2 = this->e[2]; + datatype e3 = this->e[3]; eulerangle _euler; @@ -80,49 +80,49 @@ template class quaternion: public vector //! linearized euler component e0 (approximate nick angle / rad) inline datatype lin_e0( void) const { - datatype e0 = vector::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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); } @@ -139,16 +139,16 @@ template class quaternion: public vector //! quaternion update using rotation vector void rotate( datatype p, datatype q, datatype r) { - datatype e0 = vector::e[0]; - datatype e1 = vector::e[1]; - datatype e2 = vector::e[2]; - datatype e3 = vector::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::e[0] += (-e1*p -e2*q -e3*r); - vector::e[1] += ( e0*p +e2*r -e3*q); - vector::e[2] += ( e0*q -e1*r +e3*p); - vector::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(); } @@ -165,20 +165,20 @@ template class quaternion: public vector datatype sinpsi = SIN( r); datatype cospsi = COS( r); - vector::e[0] = cosphi*costheta*cospsi + sinphi*sintheta*sinpsi; - vector::e[1] = sinphi*costheta*cospsi - cosphi*sintheta*sinpsi; - vector::e[2] = cosphi*sintheta*cospsi + sinphi*costheta*sinpsi; - vector::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 &m) const { //! R.Rogers formula 2.90 - datatype e0=vector::e[0]; - datatype e1=vector::e[1]; - datatype e2=vector::e[2]; - datatype e3=vector::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); @@ -195,18 +195,18 @@ template class quaternion: public vector quaternion operator * ( quaternion & right) //!< quaternion multiplication { quaternion result; - datatype e0=vector::e[0]; - datatype e1=vector::e[1]; - datatype e2=vector::e[2]; - datatype e3=vector::e[3]; - datatype re0=right.vector::e[0]; - datatype re1=right.vector::e[1]; - datatype re2=right.vector::e[2]; - datatype re3=right.vector::e[3]; - result.vector::e[0] = e0 * re0 - e1 * re1 - e2 * re2 + e3 * re3; - result.vector::e[1] = e1 * re0 + e2 * re3 - e3 * re2 + e0 * re1; - result.vector::e[2] = e3 * re1 + e0 * re2 - e1 * re3 + e2 * re0; - result.vector::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; } @@ -217,11 +217,11 @@ template class quaternion: public vector //! formula from roenbaeck p35 tmp = SQRT( tmp); tmp *= HALF; - vector::e[0] = tmp; + this->e[0] = tmp; tmp = QUARTER / tmp; - vector::e[1] = tmp * (rotm.e[2][1] - rotm.e[1][2]); - vector::e[2] = tmp * (rotm.e[0][2] - rotm.e[2][0]); - vector::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 }; };