// (FLOAT) function computes inverse of a symmetric 4x4 matrix A void f4x4matrixAeqInvSymA(float **A) { // global arrays used: ftmpA4x1, ftmpA4x4 // local variables int32 i, j, k; // loop counters float fdet; // matrix determinant // set tmpA4x1 to the eigenvalues and tmpA4x4 to the eigenvectors of A // function eigencompute does not use any additional global arrays eigencompute(A, 4, ftmpA4x1, ftmpA4x4); // check if the matrix A is singular by computing its determinant from the product of its eigenvalues fdet = 1.0F; for (i = 0; i < 4; i++) fdet *= ftmpA4x1[i][0]; // compute the inverse if the determinant is non-zero if (fdet != 0.0F) { // take the reciprocal of the eigenvalues for (i = 0; i < 4; i++) ftmpA4x1[i][0] = 1.0F / ftmpA4x1[i][0]; // set A to be eigenvectors . diag(1.0F / eigenvalues) . eigenvectors^T for (i = 0; i < 4; i++) // loop over rows i { for (j = 0; j < 4; j++) // loop over columns j { A[i][j] = 0.0F; for (k = 0; k < 4; k++) { A[i][j] += ftmpA4x4[i][k] * ftmpA4x1[k][0] * ftmpA4x4[j][k]; } } } } else { // the matrix A is singular so return the identity matrix fmatrixAeqI(A, 4); } return; }
// 10 element calibration using direct eigen-decomposition void fUpdateCalibration10EIG(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 in matrix 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[X] = iOffset[Y] = iOffset[Z] = 0; // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA for (m = 0; m < 10; m++) { for (n = m; n < 10; n++) { pthisMagCal->fmatA[m][n] = 0.0F; } } // sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 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 estimate for offset to help solution (bit counts) if (iCount == 0) { for (l = X; l <= Z; l++) { iOffset[l] = pthisMagBuffer->iBpFast[l][j][k]; } } // apply the fixed offset and scaling and enter into fvecA[6-8] for (l = X; l <= Z; l++) { pthisMagCal->fvecA[l + 6] = (float)((int32)pthisMagBuffer->iBpFast[l][j][k] - (int32)iOffset[l]) * fscaling; } // compute measurement vector elements fvecA[0-5] from fvecA[6-8] pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6]; pthisMagCal->fvecA[1] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[7]; pthisMagCal->fvecA[2] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[8]; pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7]; pthisMagCal->fvecA[4] = 2.0F * pthisMagCal->fvecA[7] * pthisMagCal->fvecA[8]; pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8]; // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA} // with the exception of fmatA[9][9] which equals the number of measurements // update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9] for (m = 0; m < 9; m++) { pthisMagCal->fmatA[m][9] += pthisMagCal->fvecA[m]; } // update the on and above diagonal terms of fmatA ignoring right hand column 9 for (m = 0; m < 9; m++) { for (n = m; n < 9; n++) { pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n]; } } // increment the measurement counter for the next iteration iCount++; } } } // set the last element fmatA[9][9] to the number of measurements pthisMagCal->fmatA[9][9] = (float) iCount; // store the number of measurements accumulated (defensive programming, should never be needed) pthisMagBuffer->iMagBufferCount = iCount; // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal for (m = 1; m < 10; m++) { for (n = 0; n < m; n++) { pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m]; } } // set pthisMagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 10); // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue j = 0; for (i = 1; i < 10; i++) { if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j]) { j = i; } } pthisMagCal->fA[0][0] = pthisMagCal->fmatB[0][j]; pthisMagCal->fA[0][1] = pthisMagCal->fA[1][0] = pthisMagCal->fmatB[1][j]; pthisMagCal->fA[0][2] = pthisMagCal->fA[2][0] = pthisMagCal->fmatB[2][j]; pthisMagCal->fA[1][1] = pthisMagCal->fmatB[3][j]; pthisMagCal->fA[1][2] = pthisMagCal->fA[2][1] = pthisMagCal->fmatB[4][j]; pthisMagCal->fA[2][2] = pthisMagCal->fmatB[5][j]; // negate entire solution if A has negative determinant det = f3x3matrixDetA(pthisMagCal->fA); if (det < 0.0F) { f3x3matrixAeqMinusA(pthisMagCal->fA); pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j]; pthisMagCal->fmatB[7][j] = -pthisMagCal->fmatB[7][j]; pthisMagCal->fmatB[8][j] = -pthisMagCal->fmatB[8][j]; pthisMagCal->fmatB[9][j] = -pthisMagCal->fmatB[9][j]; det = -det; } // compute the inverse of the ellipsoid matrix f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA); // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING for (l = X; l <= Z; l++) { pthisMagCal->ftrV[l] = 0.0F; for (m = X; m <= Z; m++) { pthisMagCal->ftrV[l] += pthisMagCal->finvA[l][m] * pthisMagCal->fmatB[m + 6][j]; } pthisMagCal->ftrV[l] *= -0.5F; } // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING pthisMagCal->ftrB = sqrtf(fabs(pthisMagCal->fA[0][0] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[X] + 2.0F * pthisMagCal->fA[0][1] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Y] + 2.0F * pthisMagCal->fA[0][2] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Z] + pthisMagCal->fA[1][1] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Y] + 2.0F * pthisMagCal->fA[1][2] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Z] + pthisMagCal->fA[2][2] * pthisMagCal->ftrV[Z] * pthisMagCal->ftrV[Z] - pthisMagCal->fmatB[9][j])); // calculate the trial normalized fit error as a percentage pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / (pthisMagCal->ftrB * pthisMagCal->ftrB); // correct for the measurement matrix offset and scaling and get the computed hard iron offset in uT for (l = X; l <= Z; l++) { pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l] * pthisMag->fuTPerCount; } // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A pthisMagCal->ftrB *= DEFAULTB; // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD))); pthisMagCal->ftrB *= powf(det, -(ONESIXTH)); // compute trial invW from the square root of fA (both with normalized determinant) // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA // where fmatA holds the 3x3 matrix fA in its top left elements for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j]; } } eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 3); // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA)) for (j = 0; j < 3; j++) // loop over columns j { ftmp = sqrtf(sqrtf(fabs(pthisMagCal->fvecA[j]))); for (i = 0; i < 3; i++) // loop over rows i { pthisMagCal->fmatB[i][j] *= ftmp; } } // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric) // loop over rows for (i = 0; i < 3; i++) { // loop over on and above diagonal columns for (j = i; j < 3; j++) { pthisMagCal->ftrinvW[i][j] = 0.0F; // accumulate the matrix product for (k = 0; k < 3; k++) { pthisMagCal->ftrinvW[i][j] += pthisMagCal->fmatB[i][k] * pthisMagCal->fmatB[j][k]; } // copy to below diagonal element pthisMagCal->ftrinvW[j][i] = pthisMagCal->ftrinvW[i][j]; } } 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[X] = iOffset[Y] = iOffset[Z] = 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; } } // place from MINEQUATIONS to MAXEQUATIONS 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 = X; l <= Z; l++) { iOffset[l] = pthisMagBuffer->iBpFast[l][j][k]; } } // apply the offset and scaling and store in fvecA for (l = X; l <= Z; l++) { pthisMagCal->fvecA[l + 3] = (float)((int32)pthisMagBuffer->iBpFast[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 eigencompute(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 = X; l <= Z; 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 = X; l <= Z; 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(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / fabs(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(fabs(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 = X; l <= Z; l++) { pthisMagCal->ftrinvW[l][l] = sqrtf(fabs(pthisMagCal->fA[l][l])); pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l] * pthisMag->fuTPerCount; } return; }
// 7 element calibration using direct eigen-decomposition void fUpdateCalibration7EIG(void) { // global working arrays used // fX7, ftmpA7x1, ftmpA7x7, ftmpB7x7 // local variables int32 i, j, k, l; // loop counters float fOffsetx, fOffsety, fOffsetz; // offset to remove large DC hard iron bias in matrix float ftmpBpx, ftmpBpy, ftmpBpz; // scratch variables float smallest; // smallest eigenvalue printf("\n\nThe calibration has ended"); printf("\n\n7 element EIG calibration at iteration %d with %d in Magnetic Buffer", loopcounter, MagBufferCount); // the offsets are guaranteed to be set from the first element but to avoid compiler error fOffsetx = fOffsety = fOffsetz = 0.0F; // place from MINEQUATIONS to MAXEQUATIONS entries into the measurement matrix i = 0; for (j = 0; j < MAGBUFFSIZE; j++) { for (k = 0; k < MAGBUFFSIZE; k++) { for (l = 0; l < MAGBUFFSIZE; l++) { if (iMagBuff[j][k][l].index != -1) { // set tmp to valid data from the magnetic buffer ftmpBpx = (float)iMagBuff[j][k][l].iBx * fuTpercount; ftmpBpy = (float)iMagBuff[j][k][l].iBy * fuTpercount; ftmpBpz = (float)iMagBuff[j][k][l].iBz * fuTpercount; // use first valid magnetic buffer entry as estimate (in uT) for offset to help solution if (i == 0) { fOffsetx = ftmpBpx; fOffsety = ftmpBpy; fOffsetz = ftmpBpz; } // apply the fixed offset and scaling to all measurement vectors for this iteration ftmpBpx = (ftmpBpx - fOffsetx) * fmatrixscaling; ftmpBpy = (ftmpBpy - fOffsety) * fmatrixscaling; ftmpBpz = (ftmpBpz - fOffsetz) * fmatrixscaling; // enter into the measurement matrix X scaling to make entries near unity fX7[i][0] = ftmpBpx * ftmpBpx; fX7[i][1] = ftmpBpy * ftmpBpy; fX7[i][2] = ftmpBpz * ftmpBpz; fX7[i][3] = ftmpBpx; fX7[i][4] = ftmpBpy; fX7[i][5] = ftmpBpz; fX7[i][6] = 1.0F; i++; } } } } // compute the 7x7 matrix ftmpB7x7=fX7^T.fX7 fmatrixAeqTrBxC(ftmpB7x7, fX7, fX7, MagBufferCount, 7, 7); // set tmpA7x1 to the unsorted eigenvalues and tmpA7x7 to the unsorted eigenvectors of ftmpB7x7=fX7^T.fX7 eigencompute(ftmpB7x7, 7, ftmpA7x1, ftmpA7x7); // set j to the index of the smallest eigenvalue j = 0; smallest = ftmpA7x1[0][0]; for (i = 1; i < 7; i++) { if (ftmpA7x1[i][0] < smallest) { j = i; smallest = ftmpA7x1[j][0]; } } // set ellipsoid matrix A to the solution vector column j with smallest eigenvalue A[0][0] = ftmpA7x7[0][j]; A[1][1] = ftmpA7x7[1][j]; A[2][2] = ftmpA7x7[2][j]; A[0][1] = A[0][2] = A[1][0] = A[1][2] = A[2][0] = A[2][1] = 0.0F; // compute the trial hard iron vector in offset bit counts times fmatrixscaling ftrVx = -0.5F * ftmpA7x7[3][j] / A[0][0]; ftrVy = -0.5F * ftmpA7x7[4][j] / A[1][1]; ftrVz = -0.5F * ftmpA7x7[5][j] / A[2][2]; // negate A and gain if A has negative determinant. Sign change cancels for hard iron vector det = A[0][0] * A[1][1] * A[2][2]; if (det < 0.0F) { //printf("\n\nEllipsoid matrix A has negative determinant %9.3f so inverting solution", det); fmatrixAeqAxScalar(A, -1.0F, 3, 3); ftmpA7x7[6][j] = -ftmpA7x7[6][j]; det = -det; } // compute the trial geomagnetic field strength B in bit counts times fmatrixscaling ftrB = (float)sqrt(fabs(A[0][0] * ftrVx * ftrVx + A[1][1] * ftrVy * ftrVy + A[2][2] * ftrVz * ftrVz - ftmpA7x7[6][j])); // calculate the trial normalised fit error as a percentage ftrFitErrorpc = 100.0F * (float) sqrt(fabs(ftmpA7x1[j][0]) / (double) MagBufferCount) / (2.0F * ftrB * ftrB); //printf("\n\nTrial new calibration fit error=%9.4f%% versus previous %9.4f%%", ftrFitErrorpc, fFitErrorpc); // correct for the measurement matrix offset and scaling and get the computed trial hard iron offset in uT ftrVx = ftrVx * finvmatrixscaling + fOffsetx; ftrVy = ftrVy * finvmatrixscaling + fOffsety; ftrVz = ftrVz * finvmatrixscaling + fOffsetz; //printf("\n\nTrial new calibration hard iron (uT) Vx=%9.3f Vy=%9.3f Vz=%9.3f", ftrVx, ftrVy, ftrVz); // convert the geomagnetic field strength B into uT for current soft iron matrix A ftrB *= finvmatrixscaling; // normalise the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor fmatrixAeqAxScalar(A, (float)pow((double)det, (double) (-1.0F / 3.0F)), 3, 3); ftrB *= (float)pow((double)det, (double) (-1.0F / 6.0F)); //printf("\n\nTrial new calibration geomagnetic field (uT) B=%9.3f", ftrB); //printf("\n\nTrial new calibration ellipsoid matrix A (normalized)"); //fmatrixPrintA(A, 0, 2, 0, 2); // compute trial invW from the square root of A also with normalised determinant ftrinvW[0][0] = (float)sqrt(fabs(A[0][0])); ftrinvW[1][1] = (float)sqrt(fabs(A[1][1])); ftrinvW[2][2] = (float)sqrt(fabs(A[2][2])); ftrinvW[0][1] = ftrinvW[0][2] = ftrinvW[1][0] = ftrinvW[1][2] = ftrinvW[2][0] = ftrinvW[2][1] = 0.0F; //printf("\n\nTrial new calibration inverse soft iron matrix invW (normalized)"); //fmatrixPrintA(ftrinvW, 0, 2, 0, 2); // for convenience show the original optimal invW //printf("\n\nFor comparison: Simulation inverse soft iron matrix invW (normalized)"); //***********comentados //fmatrixPrintA(invSimW, 0, 2, 0, 2); // finally set the valid calibration flag to true validmagcal = 1; return; }