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