The issuses of conversion Quaternion to Euler

News, Setup, Compiling, Flashing, Development

Re: The issuses of conversion Quaternion to Euler

Postby bn999 » Tue Jun 17, 2014 4:40 pm

Last try :) Do you have an example 4 value normalized quaternion which fails to convert properly with the navUkfQuatExtractEuler() function?

Example of a 4 value quat:

[1.0, 0.0, 0.0, 0.0]

q0 = 1.0
q1 = 0.0
q2 = 0.0
q3 = 0.0
bn999
 
Posts: 1559
Joined: Thu Jun 21, 2012 11:40 pm

Re: The issuses of conversion Quaternion to Euler

Postby srinath » Tue Jun 17, 2014 4:50 pm

What is the rotation sequence assumed in AQ ?
YPR--> I think not
RPY?
or something else
srinath
 
Posts: 1028
Joined: Mon Jun 25, 2012 5:47 pm

Re: The issuses of conversion Quaternion to Euler

Postby chestnut » Tue Jun 17, 2014 5:54 pm

Thanks srinath,

I had fixed this issues. We should transform to Matrix 3x3. Now, it work. pitch and roll can -180 to 180.
void navUkfFinish(void) {
float mat3x3[3*3];
navUkfQuatToMatrix(mat3x3, &UKF_Q1, 1);
navUkfMatrixExtractEuler(mat3x3, &navUkfData.yaw, &navUkfData.pitch, &navUkfData.roll);
navUkfData.yaw = compassNormalize(navUkfData.yaw * RAD_TO_DEG);
navUkfData.pitch *= RAD_TO_DEG;
navUkfData.roll *= RAD_TO_DEG;
}
static void navUkfQuatToMatrix(float *m, float *q, int normalize) {
float sqw = q[0]*q[0];
float sqx = q[1]*q[1];
float sqy = q[2]*q[2];
float sqz = q[3]*q[3];
float tmp1, tmp2;
float invs;

// get the invert square length
if (normalize)
invs = 1.0f / (sqx + sqy + sqz + sqw);
else
invs = 1.0f;

// rotation matrix is scaled by inverse square length
m[0*3 + 0] = ( sqx - sqy - sqz + sqw) * invs;
m[1*3 + 1] = (-sqx + sqy - sqz + sqw) * invs;
m[2*3 + 2] = (-sqx - sqy + sqz + sqw) * invs;

tmp1 = q[1]*q[2];
tmp2 = q[3]*q[0];
m[1*3 + 0] = 2.0f * (tmp1 + tmp2) * invs;
m[0*3 + 1] = 2.0f * (tmp1 - tmp2) * invs;

tmp1 = q[1]*q[3];
tmp2 = q[2]*q[0];
m[2*3 + 0] = 2.0f * (tmp1 - tmp2) * invs;
m[0*3 + 2] = 2.0f * (tmp1 + tmp2) * invs;

tmp1 = q[2]*q[3];
tmp2 = q[1]*q[0];
m[2*3 + 1] = 2.0f * (tmp1 + tmp2) * invs;
m[1*3 + 2] = 2.0f * (tmp1 - tmp2) * invs;
}
void navUkfMatrixExtractEuler(float *m, float *yaw, float *pitch, float *roll) {
if (m[1*3+0] > 0.998f) { // singularity at north pole
*pitch = atan2f(m[0*3+2], m[2*3+2]);
*yaw = M_PI/2.0f;
*roll = 0.0f;
} else if (m[1*3+0] < -0.998f) { // singularity at south pole
*pitch = atan2f(m[0*3+2] ,m[2*3+2]);
*yaw = -M_PI/2.0f;
*roll = 0.0f;
}
else {
*pitch = atan2f(-m[2*3+0] ,m[0*3+0]);
*yaw = asinf(m[1*3+0]);
*roll = atan2f(-m[1*3+2], m[1*3+1]);
}
}
chestnut
 
Posts: 71
Joined: Wed Oct 23, 2013 3:45 am
Location: VietNam

Previous

Return to AQ Firmware

Who is online

Users browsing this forum: No registered users and 17 guests

cron