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;
}