by 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]);
}
}