// Xtrinsic Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR void f3DOFTiltNED(float fR[][3], float fGp[]) { // the NED self-consistency twist occurs at 90 deg pitch // local variables int16 i; // counter float fmodGxyz; // modulus of the x, y, z accelerometer readings float fmodGyz; // modulus of the y, z accelerometer readings float frecipmodGxyz; // reciprocal of modulus float ftmp; // scratch variable // compute the accelerometer squared magnitudes fmodGyz = fGp[Y] * fGp[Y] + fGp[Z] * fGp[Z]; fmodGxyz = fmodGyz + fGp[X] * fGp[X]; // check for freefall special case where no solution is possible if (fmodGxyz == 0.0F) { f3x3matrixAeqI(fR); return; } // check for vertical up or down gimbal lock case if (fmodGyz == 0.0F) { f3x3matrixAeqScalar(fR, 0.0F); fR[Y][Y] = 1.0F; if (fGp[X] >= 0.0F) { fR[X][Z] = 1.0F; fR[Z][X] = -1.0F; } else { fR[X][Z] = -1.0F; fR[Z][X] = 1.0F; } return; } // compute moduli for the general case fmodGyz = sqrtf(fmodGyz); fmodGxyz = sqrtf(fmodGxyz); frecipmodGxyz = 1.0F / fmodGxyz; ftmp = fmodGxyz / fmodGyz; // normalize the accelerometer reading into the z column for (i = X; i <= Z; i++) { fR[i][Z] = fGp[i] * frecipmodGxyz; } // construct x column of orientation matrix fR[X][X] = fmodGyz * frecipmodGxyz; fR[Y][X] = -fR[X][Z] * fR[Y][Z] * ftmp; fR[Z][X] = -fR[X][Z] * fR[Z][Z] * ftmp; // // construct y column of orientation matrix fR[X][Y] = 0.0F; fR[Y][Y] = fR[Z][Z] * ftmp; fR[Z][Y] = -fR[Y][Z] * ftmp; return; }
// 7 element calibration using direct eigen-decomposition void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag) { // local variables float det; // matrix determinant float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING float ftmp; // scratch variable int16 iOffset[3]; // offset to remove large DC hard iron bias int16 iCount; // number of measurements counted int8 i, j, k, l, m, n; // loop counters // compute fscaling to reduce multiplications later fscaling = pthisMag->fuTPerCount / DEFAULTB; // the offsets are guaranteed to be set from the first element but to avoid compiler error iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0; // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA for (m = 0; m < 7; m++) { for (n = m; n < 7; n++) { pthisMagCal->fmatA[m][n] = 0.0F; } } // add megnetic buffer entries into product matrix fmatA iCount = 0; for (j = 0; j < MAGBUFFSIZEX; j++) { for (k = 0; k < MAGBUFFSIZEY; k++) { if (pthisMagBuffer->index[j][k] != -1) { // use first valid magnetic buffer entry as offset estimate (bit counts) if (iCount == 0) { for (l = CHX; l <= CHZ; l++) { iOffset[l] = pthisMagBuffer->iBs[l][j][k]; } } // apply the offset and scaling and store in fvecA for (l = CHX; l <= CHZ; l++) { pthisMagCal->fvecA[l + 3] = (float)((int32)pthisMagBuffer->iBs[l][j][k] - (int32)iOffset[l]) * fscaling; pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3]; } // accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA} // with the exception of fmatA[6][6] which will sum to the number of measurements // and remembering that fvecA[6] equals 1.0F // update the right hand column [6] of fmatA except for fmatA[6][6] for (m = 0; m < 6; m++) { pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m]; } // update the on and above diagonal terms except for right hand column 6 for (m = 0; m < 6; m++) { for (n = m; n < 6; n++) { pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n]; } } // increment the measurement counter for the next iteration iCount++; } } } // finally set the last element fmatA[6][6] to the number of measurements pthisMagCal->fmatA[6][6] = (float) iCount; // store the number of measurements accumulated (defensive programming, should never be needed) pthisMagBuffer->iMagBufferCount = iCount; // copy the above diagonal elements of fmatA to below the diagonal for (m = 1; m < 7; m++) { for (n = 0; n < m; n++) { pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m]; } } // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA eigencompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 7); // find the smallest eigenvalue j = 0; for (i = 1; i < 7; i++) { if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j]) { j = i; } } // set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant // and the hard iron offset (scaled and offset) f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F); det = 1.0F; for (l = CHX; l <= CHZ; l++) { pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j]; det *= pthisMagCal->fA[l][l]; pthisMagCal->ftrV[l] = -0.5F * pthisMagCal->fmatB[l + 3][j] / pthisMagCal->fA[l][l]; } // negate A if it has negative determinant if (det < 0.0F) { f3x3matrixAeqMinusA(pthisMagCal->fA); pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j]; det = -det; } // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING) ftmp = -pthisMagCal->fmatB[6][j]; for (l = CHX; l <= CHZ; l++) { ftmp += pthisMagCal->fA[l][l] * pthisMagCal->ftrV[l] * pthisMagCal->ftrV[l]; } // calculate the trial normalized fit error as a percentage pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / fabsf(ftmp); // normalize the ellipsoid matrix A to unit determinant f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD))); // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH)); // compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT f3x3matrixAeqI(pthisMagCal->ftrinvW); for (l = CHX; l <= CHZ; l++) { pthisMagCal->ftrinvW[l][l] = sqrtf(fabsf(pthisMagCal->fA[l][l])); pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l] * pthisMag->fuTPerCount; } return; }
// Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR void f3DOFTiltWin8(float fR[][3], float fGs[]) { // the Win8 self-consistency twist occurs at 90 deg roll // local variables float fmodGxyz; // modulus of the x, y, z accelerometer readings float fmodGxz; // modulus of the x, z accelerometer readings float frecipmodGxyz; // reciprocal of modulus float ftmp; // scratch variable int8 i; // counter // compute the accelerometer squared magnitudes fmodGxz = fGs[CHX] * fGs[CHX] + fGs[CHZ] * fGs[CHZ]; fmodGxyz = fmodGxz + fGs[CHY] * fGs[CHY]; // check for freefall special case where no solution is possible if (fmodGxyz == 0.0F) { f3x3matrixAeqI(fR); return; } // check for vertical up or down gimbal lock case if (fmodGxz == 0.0F) { f3x3matrixAeqScalar(fR, 0.0F); fR[CHX][CHX] = 1.0F; if (fGs[CHY] >= 0.0F) { fR[CHY][CHZ] = -1.0F; fR[CHZ][CHY] = 1.0F; } else { fR[CHY][CHZ] = 1.0F; fR[CHZ][CHY] = -1.0F; } return; } // compute moduli for the general case fmodGxz = sqrtf(fmodGxz); fmodGxyz = sqrtf(fmodGxyz); frecipmodGxyz = 1.0F / fmodGxyz; ftmp = fmodGxyz / fmodGxz; if (fGs[CHZ] < 0.0F) { ftmp = -ftmp; } // normalize the negated accelerometer reading into the z column for (i = CHX; i <= CHZ; i++) { fR[i][CHZ] = -fGs[i] * frecipmodGxyz; } // construct x column of orientation matrix fR[CHX][CHX] = -fR[CHZ][CHZ] * ftmp; fR[CHY][CHX] = 0.0F; fR[CHZ][CHX] = fR[CHX][CHZ] * ftmp;; // // construct y column of orientation matrix fR[CHX][CHY] = fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp; fR[CHY][CHY] = -fmodGxz * frecipmodGxyz; if (fGs[CHZ] < 0.0F) { fR[CHY][CHY] = -fR[CHY][CHY]; } fR[CHZ][CHY] = fR[CHY][CHZ] * fR[CHZ][CHZ] * ftmp; return; }