// extract the Android angles in degrees from the Android rotation matrix
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
float *pfRhoDeg, float *pfChiDeg)
{
// calculate the roll angle -90.0 <= Phi <= 90.0 deg
*pfPhiDeg = fasin_deg(R[X][Z]);
// calculate the pitch angle -180.0 <= The < 180.0 deg
*pfTheDeg = fatan2_deg(-R[Y][Z], R[Z][Z]);
// map +180 pitch onto the functionally equivalent -180 deg pitch
if (*pfTheDeg == 180.0F)
{
*pfTheDeg = -180.0F;
}
// calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
if (*pfPhiDeg == 90.0F)
{
// vertical downwards gimbal lock case
*pfPsiDeg = fatan2_deg(R[Y][X], R[Y][Y]) - *pfTheDeg;
}
else if (*pfPhiDeg == -90.0F)
{
// vertical upwards gimbal lock case
*pfPsiDeg = fatan2_deg(R[Y][X], R[Y][Y]) + *pfTheDeg;
}
else
{
// // general case
*pfPsiDeg = fatan2_deg(-R[X][Y], R[X][X]);
}
// map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
if (*pfPsiDeg < 0.0F)
{
*pfPsiDeg += 360.0F;
}
// check for rounding errors mapping small negative angle to 360 deg
if (*pfPsiDeg >= 360.0F)
{
*pfPsiDeg = 0.0F;
}
// the compass heading angle Rho equals the yaw angle Psi
// this definition is compliant with Motorola Xoom tablet behavior
*pfRhoDeg = *pfPsiDeg;
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
*pfChiDeg = facos_deg(R[Z][Z]);
return;
}
// extract the NED angles in degrees from the NED rotation matrix
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
float *pfRhoDeg, float *pfChiDeg)
{
// calculate the pitch angle -90.0 <= Theta <= 90.0 deg
*pfTheDeg = fasin_deg(-R[X][Z]);
// calculate the roll angle range -180.0 <= Phi < 180.0 deg
*pfPhiDeg = fatan2_deg(R[Y][Z], R[Z][Z]);
// map +180 roll onto the functionally equivalent -180 deg roll
if (*pfPhiDeg == 180.0F)
{
*pfPhiDeg = -180.0F;
}
// calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
if (*pfTheDeg == 90.0F)
{
// vertical upwards gimbal lock case
*pfPsiDeg = fatan2_deg(R[Z][Y], R[Y][Y]) + *pfPhiDeg;
}
else if (*pfTheDeg == -90.0F)
{
// vertical downwards gimbal lock case
*pfPsiDeg = fatan2_deg(-R[Z][Y], R[Y][Y]) - *pfPhiDeg;
}
else
{
// general case
*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]);
}
// map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
if (*pfPsiDeg < 0.0F)
{
*pfPsiDeg += 360.0F;
}
// check for rounding errors mapping small negative angle to 360 deg
if (*pfPsiDeg >= 360.0F)
{
*pfPsiDeg = 0.0F;
}
// for NED, the compass heading Rho equals the yaw angle Psi
*pfRhoDeg = *pfPsiDeg;
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
*pfChiDeg = facos_deg(R[Z][Z]);
return;
}
// extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
float *pfRhoDeg, float *pfChiDeg)
{
// calculate the roll angle -90.0 <= Phi <= 90.0 deg
if (R[Z][Z] == 0.0F)
{
if (R[X][Z] >= 0.0F)
{
// tan(phi) is -infinity
*pfPhiDeg = -90.0F;
}
else
{
// tan(phi) is +infinity
*pfPhiDeg = 90.0F;
}
}
else
{
// general case
*pfPhiDeg = fatan_deg(-R[X][Z] / R[Z][Z]);
}
// first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
*pfTheDeg = fasin_deg(R[Y][Z]);
// use R[Z][Z]=cos(Phi)*cos(The) to correct the quadrant of The remembering
// cos(Phi) is non-negative so that cos(The) has the same sign as R[Z][Z].
if (R[Z][Z] < 0.0F)
{
// wrap The around +90 deg and -90 deg giving result 90 to 270 deg
*pfTheDeg = 180.0F - *pfTheDeg;
}
// map the pitch angle The to the range -180.0 <= The < 180.0 deg
if (*pfTheDeg >= 180.0F)
{
*pfTheDeg -= 360.0F;
}
// calculate the yaw angle Psi
if (*pfTheDeg == 90.0F)
{
// vertical upwards gimbal lock case: -270 <= Psi < 90 deg
*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) - *pfPhiDeg;
}
else if (*pfTheDeg == -90.0F)
{
// vertical downwards gimbal lock case: -270 <= Psi < 90 deg
*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) + *pfPhiDeg;
}
else
{
// general case: -180 <= Psi < 180 deg
*pfPsiDeg = fatan2_deg(-R[Y][X], R[Y][Y]);
// correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
if (fabs(*pfTheDeg) >= 90.0F)
{
*pfPsiDeg += 180.0F;
}
}
// map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
if (*pfPsiDeg < 0.0F)
{
*pfPsiDeg += 360.0F;
}
// check for any rounding error mapping small negative angle to 360 deg
if (*pfPsiDeg >= 360.0F)
{
*pfPsiDeg = 0.0F;
}
// compute the compass angle Rho = 360 - Psi
*pfRhoDeg = 360.0F - *pfPsiDeg;
// check for rounding errors mapping small negative angle to 360 deg and zero degree case
if (*pfRhoDeg >= 360.0F)
{
*pfRhoDeg = 0.0F;
}
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
*pfChiDeg = facos_deg(R[Z][Z]);
return;
}
// Win8: 6DOF e-Compass function computing rotation matrix fR
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
{
// local variables
float fmod[3]; // column moduli
float fmodBc; // modulus of Bc
float fGdotBc; // dot product of vectors G.Bc
float ftmp; // scratch variable
int8 i, j; // loop counters
// set the inclination angle to zero in case it is not computed later
*pfDelta = 0.0F;
// place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
for (i = X; i <= Z; i++)
{
fR[i][Z] = -fGp[i];
fR[i][Y] = fBc[i];
}
// set x vector to vector product of y and z vectors
fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
// set y vector to vector product of z and x vectors
fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
// calculate the rotation matrix column moduli
fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
// normalize the rotation matrix columns
if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
{
// loop over columns j
for (j = X; j <= Z; j++)
{
ftmp = 1.0F / fmod[j];
// loop over rows i
for (i = X; i <= Z; i++)
{
// normalize by the column modulus
fR[i][j] *= ftmp;
}
}
}
else
{
// no solution is possible to set rotation to identity matrix
f3x3matrixAeqI(fR);
return;
}
// compute the geomagnetic inclination angle
fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
{
*pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
}
return;
}
// Win8: 6DOF e-Compass function computing least squares fit to orientation quaternion fq
// on the assumption that the geomagnetic field fB and magnetic inclination angle fDelta are known
void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta,
float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
{
// local variables
float fK[4][4]; // K measurement matrix
float eigvec[4][4]; // matrix of eigenvectors of K
float eigval[4]; // vector of eigenvalues of K
float fmodGsSq; // modulus of fGs[] squared
float fmodBcSq; // modulus of fBc[] squared
float fmodGs; // modulus of fGs[]
float fmodBc; // modulus of fBc[]
float fGsdotBc; // scalar product of Gs and Bc
float f*g, fam; // relative weightings
float fagOvermodGs; // a0 / |Gs|
float famOvermodBc; // a1 / |Bc|
float famOvermodBccosDelta; // a1 / |Bc| * cos(Delta)
float famOvermodBcsinDelta; // a1 / |Bc| * sin(Delta)
float ftmp; // scratch
int8 i; // loop counter
// calculate the measurement vector moduli and return with identity quaternion if either is null.
fmodGsSq = fGs[CHX] * fGs[CHX] + fGs[CHY] * fGs[CHY] + fGs[CHZ] * fGs[CHZ];
fmodBcSq = fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ];
fmodGs = sqrtf(fmodGsSq);
fmodBc = sqrtf(fmodBcSq);
if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
{
pfq->q0 = 1.0F;
pfq->q1 = pfq->q2 = pfq->q3 = 0.0F;
return;
}
// calculate the accelerometer and magnetometer noise covariances (units rad^2) and least squares weightings
*pfQvGQa = fabsf(fmodGsSq - 1.0F);
*pfQvBQd = fabsf(fmodBcSq - fB * fB);
f*g = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
fam = 1.0F - f*g;
// compute useful ratios to reduce computation
fagOvermodGs = f*g / fmodGs;
famOvermodBc = fam / fmodBc;
famOvermodBccosDelta = famOvermodBc * fcosDelta;
famOvermodBcsinDelta = famOvermodBc * fsinDelta;
// compute the scalar product Gs.Bc and 6DOF accelerometer plus magnetometer geomagnetic inclination angle (deg)
fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
*pfDelta6DOF = fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
// set the K matrix to the non-zero accelerometer components
fK[0][0] = fK[3][3] = -fagOvermodGs * fGs[CHZ];
fK[1][1] = fK[2][2] = -fK[0][0];
fK[0][1] = fK[2][3] = -fagOvermodGs * fGs[CHY];
fK[1][3] = -fagOvermodGs * fGs[CHX];
fK[0][2] = -fK[1][3];
// update the K matrix with the magnetometer component
ftmp = famOvermodBcsinDelta * fBc[CHX];
fK[0][2] += ftmp;
fK[1][3] -= ftmp;
fK[0][3] = fK[1][2] = famOvermodBccosDelta * fBc[CHX];
ftmp = famOvermodBccosDelta * fBc[CHY];
fK[0][0] += ftmp;
fK[1][1] -= ftmp;
fK[2][2] += ftmp;
fK[3][3] -= ftmp;
ftmp = famOvermodBcsinDelta * fBc[CHZ];
fK[0][0] -= ftmp;
fK[1][1] += ftmp;
fK[2][2] += ftmp;
fK[3][3] -= ftmp;
ftmp = famOvermodBccosDelta * fBc[CHZ];
fK[0][1] -= ftmp;
fK[2][3] += ftmp;
ftmp = famOvermodBcsinDelta * fBc[CHY];
fK[0][1] -= ftmp;
fK[2][3] -= ftmp;
// copy above diagonal elements to below diagonal
fK[1][0] = fK[0][1];
fK[2][0] = fK[0][2];
fK[2][1] = fK[1][2];
fK[3][0] = fK[0][3];
fK[3][1] = fK[1][3];
fK[3][2] = fK[2][3];
// set eigval to the unsorted eigenvalues and eigvec to the unsorted normalized eigenvectors of fK
eigencompute4(fK, eigval, eigvec, 4);
// copy the largest eigenvector into the orientation quaternion fq
i = 0;
if (eigval[1] > eigval[i]) i = 1;
if (eigval[2] > eigval[i]) i = 2;
if (eigval[3] > eigval[i]) i = 3;
pfq->q0 = eigvec[0][i];
pfq->q1 = eigvec[1][i];
pfq->q2 = eigvec[2][i];
pfq->q3 = eigvec[3][i];
// force q0 to be non-negative
if (pfq->q0 < 0.0F)
{
pfq->q0 = -pfq->q0;
pfq->q1 = -pfq->q1;
pfq->q2 = -pfq->q2;
pfq->q3 = -pfq->q3;
}
return;
}
