KL46 Accelerometer + Magnetometer, with 3-axis calibration. Readout through OpenSDA CDC.
Dependencies: MAG3110 MMA8451Q USBDevice mbed
magnetic.cpp
- Committer:
- wue
- Date:
- 2014-04-10
- Revision:
- 0:c569d820861b
File content as of revision 0:c569d820861b:
// Source: eCompass v3 #include "magnetic.h" #include "mbed.h" float xftmpA4x4[4][4], *ftmpA4x4[4]; // scratch 4x4 matrix float xftmpB4x4[4][4], *ftmpB4x4[4]; // scratch 4x4 matrix float xftmpA4x1[4][1], *ftmpA4x1[4]; // scratch 4x1 matrix float xftmpB4x1[4][1], *ftmpB4x1[4]; // scratch 4x1 matrix int32 xicolind[MAXMATINV][1], *icolind[MAXMATINV]; int32 xirowind[MAXMATINV][1], *irowind[MAXMATINV]; int32 xipivot[MAXMATINV][1], *ipivot[MAXMATINV]; void magUpdateCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer) { int i; //while(1); // 4 row arrays for (i = 0; i < 4; i++) { ftmpA4x4[i] = xftmpA4x4[i]; ftmpB4x4[i] = xftmpB4x4[i]; ftmpA4x1[i] = xftmpA4x1[i]; ftmpB4x1[i] = xftmpB4x1[i]; }; // MAXMATINV row arrays for (i = 0; i < MAXMATINV; i++) { icolind[i] = xicolind[i]; irowind[i] = xirowind[i]; ipivot[i] = xipivot[i]; }; fUpdateCalibration4INV(pthisMagCal, pthisMagneticBuffer, ftmpA4x4, ftmpB4x4, ftmpA4x1, ftmpB4x1, icolind, irowind, ipivot); }; // function calculates the matrix product A = B x C void fmatrixAeqBxC(float **A, float **B, float **C, int32 rB, int32 cBrC, int32 cC) { // rB = rows in B // cBrC = columns in B = rows in C // cC = columns in C // A has dimension rB rows x cC columns int32 i, j, k; // counters for (i = 0; i < rB; i++) { for (j = 0; j < cC; j++) { A[i][j] = 0.0F; for (k = 0; k < cBrC; k++) A[i][j] += B[i][k] * C[k][j]; } } return; } // function sets the matrix A to the identity matrix void fmatrixAeqI(float **A, int32 rc) { // rc = rows and columns in A int32 i, j; // loop counters for (i = 0; i < rc; i++) { for (j = 0; j < rc; j++) { A[i][j] = 0.0F; } A[i][i] = 1.0F; } return; } /* function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ */ /* on exit, A is replaced with its inverse */ void fmatrixAeqInvA(float **A, int32 isize, int32 **icolind, int32 **irowind, int32 **ipivot) { int32 i, j, k, l, m; // index counters int32 ipivotrow, ipivotcol; // row and column of pivot element float largest; // largest element used for pivoting float scaling; // scaling factor in pivoting float recippiv; // reciprocal of pivot element float ftmp; // temporary variable used in swaps // initialize the pivot array to 0 for (j = 0; j < isize; j++) { ipivot[j][0] = 0; } // main loop i over the dimensions of the square matrix A for (i = 0; i < isize; i++) { // zero the largest element found for pivoting largest = 0.0F; // loop over candidate rows j for (j = 0; j < isize; j++) { // check if row j has been previously pivoted if (ipivot[j][0] != 1) { // loop over candidate columns k for (k = 0; k < isize; k++) { // check if column k has previously been pivoted if (ipivot[k][0] == 0) { // check if the pivot element is the largest found so far if (fabs(A[j][k]) >= largest) { // and store this location as the current best candidate for pivoting ipivotrow = j; ipivotcol = k; largest = (float) fabs(A[ipivotrow][ipivotcol]); } } else if (ipivot[k][0] > 1) { // zero determinant situation: exit with identity matrix fmatrixAeqI(A, 3); return; } } } } // increment the entry in ipivot to denote it has been selected for pivoting ipivot[ipivotcol][0]++; // check the pivot rows ipivotrow and ipivotcol are not the same before swapping if (ipivotrow != ipivotcol) { // loop over columns l for (l = 0; l < isize; l++) { // and swap all elements of rows ipivotrow and ipivotcol ftmp = A[ipivotrow][l]; A[ipivotrow][l] = A[ipivotcol][l]; A[ipivotcol][l] = ftmp; } } // record that on the i-th iteration rows ipivotrow and ipivotcol were swapped irowind[i][0] = ipivotrow; icolind[i][0] = ipivotcol; // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected if (A[ipivotcol][ipivotcol] == 0.0F) { // zero determinant situation: exit with identity matrix fmatrixAeqI(A, 3); return; } // calculate the reciprocal of the pivot element knowing it's non-zero recippiv = 1.0F / A[ipivotcol][ipivotcol]; // by definition, the diagonal element normalizes to 1 A[ipivotcol][ipivotcol] = 1.0F; // multiply all of row ipivotcol by the reciprocal of the pivot element including the diagonal element // the diagonal element A[ipivotcol][ipivotcol] now has value equal to the reciprocal of its previous value for (l = 0; l < isize; l++) { A[ipivotcol][l] *= recippiv; } // loop over all rows m of A for (m = 0; m < isize; m++) { if (m != ipivotcol) { // scaling factor for this row m is in column ipivotcol scaling = A[m][ipivotcol]; // zero this element A[m][ipivotcol] = 0.0F; // loop over all columns l of A and perform elimination for (l = 0; l < isize; l++) { A[m][l] -= A[ipivotcol][l] * scaling; } } } } // end of loop i over the matrix dimensions // finally, loop in inverse order to apply the missing column swaps for (l = isize - 1; l >= 0; l--) { // set i and j to the two columns to be swapped i = irowind[l][0]; j = icolind[l][0]; // check that the two columns i and j to be swapped are not the same if (i != j) { // loop over all rows k to swap columns i and j of A for (k = 0; k < isize; k++) { ftmp = A[k][i]; A[k][i] = A[k][j]; A[k][j] = ftmp; } } } return; } void ResetMagCalibration(struct MagCalibration *pthisMagCal/*, struct MagneticBuffer *pthisMagneticBuffer*/) { int32 i, j, k, l; // loop counters for (i = 0; i < 3; i++) { pthisMagCal->finvW[i] = pthisMagCal->xfinvW[i]; pthisMagCal->ftrinvW[i] = pthisMagCal->xftrinvW[i]; pthisMagCal->fA[i] = pthisMagCal->xfA[i]; pthisMagCal->finvA[i] = pthisMagCal->xinvA[i]; }; // initialize the calibration hard and soft iron estimate to null fmatrixAeqI(pthisMagCal->finvW, 3); pthisMagCal->fVx = pthisMagCal->fVy = pthisMagCal->fVz = 0.0F; pthisMagCal->fB = 0.0F; pthisMagCal->fFitErrorpc = 1000.0F; pthisMagCal->iValidMagCal = 0; // set magnetic buffer index to invalid value -1 to denote invalid /*pthisMagneticBuffer->iMagBufferCount = 0; for (j = 0; j < MAGBUFFSIZE; j++) { for (k = 0; k < MAGBUFFSIZE; k++) { for (l = 0; l < MAGBUFFSIZE; l++) { pthisMagneticBuffer->index[j][k][l] = -1; } } }*/ return; } // 4 element calibration using 4x4 matrix inverse void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer, float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1, float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot) { int32 i, j, /*k, l,*/ m, n; // loop counters int32 ilocalMagBufferCount; // local count of measurements for this process float fOffsetx, fOffsety, fOffsetz; // offset to remove large DC hard iron bias in matrix float ftmpBpx, ftmpBpy, ftmpBpz; // x, y, z magnetometer readings (uT) float ftmpBpxsq, ftmpBpysq, ftmpBpzsq; // squares of x, y, z magnetometer readings (uT) float fy; // dependent variable float fYTY; // scalar equal to Y^T.Y float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING float fP; // performance function = r^T.r // compute fscaling to reduce multiplications later fscaling = FUTPERCOUNT * FMATRIXSCALING; // set trial inverse soft iron matrix invW to the identity matrix for 4 element calibration pthisMagCal->ftrinvW[0][0] = pthisMagCal->ftrinvW[1][1] = pthisMagCal->ftrinvW[2][2] = 1.0F; pthisMagCal->ftrinvW[0][1] = pthisMagCal->ftrinvW[0][2] = pthisMagCal->ftrinvW[1][0] = 0.0F; pthisMagCal->ftrinvW[1][2] = pthisMagCal->ftrinvW[2][0] = pthisMagCal->ftrinvW[2][1] = 0.0F; // zero fYTY=Y^T.Y, ftmpA4x1=X^T.Y and on and above diagonal elements of ftmpA4x4=X^T*X fYTY = 0.0F; for (m = 0; m < 4; m++) { ftmpA4x1[m][0] = 0.0F; for (n = m; n < 4; n++) { ftmpA4x4[m][n] = 0.0F; } } // the offsets are guaranteed to be set from the first element but to avoid compiler error fOffsetx = fOffsety = fOffsetz = 0.0F; // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices i = 0; for (j = 0; j < MAGBUFFSIZE; j++) { //for (k = 0; k < MAGBUFFSIZE; k++) //{ //for (l = 0; l < MAGBUFFSIZE; l++) //{ //if (pthisMagneticBuffer->index[j][k][l] != -1) //{ // use first valid magnetic buffer entry as estimate (in counts) for offset printf("."); if (i == 0) { fOffsetx = (float)pthisMagneticBuffer->iBx[j]/*[k][l]*/; fOffsety = (float)pthisMagneticBuffer->iBy[j]/*[k][l]*/; fOffsetz = (float)pthisMagneticBuffer->iBz[j]/*[k][l]*/; } // calculate offset and scaled magnetic buffer vector data Bx, By, Bz (scaled uT) ftmpBpx = ((float)pthisMagneticBuffer->iBx[j]/*[k][l]*/ - fOffsetx) * fscaling; ftmpBpy = ((float)pthisMagneticBuffer->iBy[j]/*[k][l]*/ - fOffsety) * fscaling; ftmpBpz = ((float)pthisMagneticBuffer->iBz[j]/*[k][l]*/ - fOffsetz) * fscaling; // calculate y = Bx^2 + By^2 + Bz^2 (scaled uT^2) and accumulate Y^T.Y ftmpBpxsq = ftmpBpx * ftmpBpx; ftmpBpysq = ftmpBpy * ftmpBpy; ftmpBpzsq = ftmpBpz * ftmpBpz; fy = ftmpBpxsq + ftmpBpysq + ftmpBpzsq; fYTY += fy * fy; // accumulate ftmpA4x1 = X^T.Y ftmpA4x1[0][0] += ftmpBpx * fy; ftmpA4x1[1][0] += ftmpBpy * fy; ftmpA4x1[2][0] += ftmpBpz * fy; ftmpA4x1[3][0] += fy; // accumulate on and above-diagonal terms of ftmpA4x4 = X^T.X ftmpA4x4[0][0] += ftmpBpxsq; ftmpA4x4[0][1] += ftmpBpx * ftmpBpy; ftmpA4x4[0][2] += ftmpBpx * ftmpBpz; ftmpA4x4[0][3] += ftmpBpx; ftmpA4x4[1][1] += ftmpBpysq; ftmpA4x4[1][2] += ftmpBpy * ftmpBpz; ftmpA4x4[1][3] += ftmpBpy; ftmpA4x4[2][2] += ftmpBpzsq; ftmpA4x4[2][3] += ftmpBpz; // increment the counter for next iteration i++; //} //} //} } //printf("[dbg1]"); // store the number of measurements accumulated ilocalMagBufferCount = i; // set the last element of the measurement matrix to the number of buffer elements used ftmpA4x4[3][3] = (float) i; // use above diagonal elements of symmetric matrix ftmpA4x4 to create ftmpB4x4 = ftmpA4x4 = X^T.X for (m = 0; m < 4; m++) { for (n = m; n < 4; n++) { ftmpB4x4[m][n] = ftmpB4x4[n][m] = ftmpA4x4[n][m] = ftmpA4x4[m][n]; } } //printf("[dbg2]"); // calculate in situ inverse of ftmpB4x4 = inv(X^T.X) (4x4) fmatrixAeqInvA(ftmpB4x4, 4, icolind, irowind, ipivot); // calculate ftmpB4x1 = solution vector beta (4x1) = inv(X^T.X).X^T.Y = ftmpB4x4 * ftmpA4x1 fmatrixAeqBxC(ftmpB4x1, ftmpB4x4, ftmpA4x1, 4, 4, 1); // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta // = fYTY - 2 * ftmpB4x1^T.ftmpA4x1 + ftmpB4x1^T.ftmpA4x4.ftmpB4x1 // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fYTY - 2 * ftmpB4x1^T.ftmpA4x1 fP = fYTY - 2.0F * (ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] + ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0]); // set ftmpA4x1 = (X^T.X).beta = ftmpA4x4.ftmpB4x1 fmatrixAeqBxC(ftmpA4x1, ftmpA4x4, ftmpB4x1, 4, 4, 1); // add beta^T.(X^T.X).beta = ftmpB4x1^T * ftmpA4x1 to P fP += ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] + ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0]; // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING) pthisMagCal->ftrVx = 0.5F * ftmpB4x1[0][0]; pthisMagCal->ftrVy = 0.5F * ftmpB4x1[1][0]; pthisMagCal->ftrVz = 0.5F * ftmpB4x1[2][0]; // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING) pthisMagCal->ftrB = (float)sqrt(ftmpB4x1[3][0] + pthisMagCal->ftrVx * pthisMagCal->ftrVx + pthisMagCal->ftrVy * pthisMagCal->ftrVy + pthisMagCal->ftrVz * pthisMagCal->ftrVz); // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength pthisMagCal->ftrFitErrorpc = (float)sqrt(fP / (float) ilocalMagBufferCount) * 100.0F / (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB); //printf("\n\nTrial new calibration fit error=%9.4f%% versus previous %9.4f%%", pthisMagCal->ftrFitErrorpc, pthisMagCal->fFitErrorpc); // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT) pthisMagCal->ftrVx = pthisMagCal->ftrVx * FINVMATRIXSCALING + fOffsetx * FUTPERCOUNT; pthisMagCal->ftrVy = pthisMagCal->ftrVy * FINVMATRIXSCALING + fOffsety * FUTPERCOUNT; pthisMagCal->ftrVz = pthisMagCal->ftrVz * FINVMATRIXSCALING + fOffsetz * FUTPERCOUNT; //printf("\n\nTrial new calibration hard iron (uT) Vx=%9.3f Vy=%9.3f Vz=%9.3f", pthisMagCal->ftrVx, pthisMagCal->ftrVy, pthisMagCal->ftrVz); // correct the geomagnetic field strength B to correct scaling (result in uT) pthisMagCal->ftrB *= FINVMATRIXSCALING; //printf("\n\nTrial new calibration geomagnetic field (uT) B=%9.3f", pthisMagCal->ftrB); // set the valid calibration flag to true pthisMagCal->iValidMagCal = 1; return; }