Diff: magnetic.cpp

Revision:

0:c569d820861b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/magnetic.cpp	Thu Apr 10 07:48:46 2014 +0000
@@ -0,0 +1,410 @@
+// 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;
+}
\ No newline at end of file