// (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;
}
void ResetMagCalibrationFunc(void)
{
int32 j, k, l; // loop counters
// initialise the calibration hard and soft iron estimate to null
fmatrixAeqI(finvW, 3);
fVx = fVy = fVz = 0.;
fB = 0.0F;
fFitErrorpc = 1000.0F;
validmagcal = 0;
// set the loop counter to 0 to denote first pass
loopcounter = 0;
// set magnetic buffer index to invalid value -1 to denote invalid
MagBufferCount = 0;
for (j = 0; j < MAGBUFFSIZE; j++)
for (k = 0; k < MAGBUFFSIZE; k++)
for (l = 0; l < MAGBUFFSIZE; l++)
iMagBuff[j][k][l].index = -1;
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[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
{
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
int8 i, j, k, l, m; // index counters
int8 iPivotRow, iPivotCol; // row and column of pivot element
// to avoid compiler warnings
iPivotRow = iPivotCol = 0;
// default to successful inversion
*pierror = false;
// initialize the pivot array to 0
for (j = 0; j < isize; j++)
{
iPivot[j] = 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] != 1)
{
// loop over candidate columns k
for (k = 0; k < isize; k++)
{
// check if column k has previously been pivoted
if (iPivot[k] == 0)
{
// check if the pivot element is the largest found so far
if (fabsf(A[j][k]) >= largest)
{
// and store this location as the current best candidate for pivoting
iPivotRow = j;
iPivotCol = k;
largest = (float) fabsf(A[iPivotRow][iPivotCol]);
}
}
else if (iPivot[k] > 1)
{
// zero determinant situation: exit with identity matrix and set error flag
fmatrixAeqI(A, isize);
*pierror = true;
return;
}
}
}
}
// increment the entry in iPivot to denote it has been selected for pivoting
iPivot[iPivotCol]++;
// 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] = iPivotRow;
iColInd[i] = 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 and set error flag
fmatrixAeqI(A, isize);
*pierror = true;
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++)
{
if (A[iPivotCol][l] != 0.0F)
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++)
{
if ((A[iPivotCol][l] != 0.0F) && (scaling != 0.0F))
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];
j = iColInd[l];
// 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;
}
