From ae4c221308b167db4fd474dc4d9b4bec5559523f Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Thu, 10 Mar 2016 08:37:02 -0800 Subject: [PATCH] Inport Freescale's magnetic calibration math --- Makefile | 2 +- imuread.h | 52 ++++++ magcal.c | 530 ++++++++++++++++++++++++++++++++++++++++++++++++++++ matrix.c | 446 +++++++++++++++++++++++++++++++++++++++++++ visualize.c | 2 +- 5 files changed, 1030 insertions(+), 2 deletions(-) create mode 100644 magcal.c create mode 100644 matrix.c diff --git a/Makefile b/Makefile index 06c5182..ef8bf95 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ VERSION = 0.01 endif -OBJS = visualize.o serialdata.o rawdata.o +OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o all: $(ALL) diff --git a/imuread.h b/imuread.h index f2f77a2..c789acb 100644 --- a/imuread.h +++ b/imuread.h @@ -73,6 +73,58 @@ void visualize_init(void); void display_callback(void); void resize_callback(int width, int height); + + +// magnetometer measurement buffer +struct MagneticBuffer +{ + int16_t iBpFast[3][MAGBUFFSIZEX][MAGBUFFSIZEY]; // uncalibrated magnetometer readings + int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY]; // array of time indices + int16_t tanarray[MAGBUFFSIZEX - 1]; // array of tangents of (100 * angle) + int16_t iMagBufferCount; // number of magnetometer readings +}; + +// magnetic calibration structure +struct MagCalibration +{ + float fV[3]; // current hard iron offset x, y, z, (uT) + float finvW[3][3]; // current inverse soft iron matrix + float fB; // current geomagnetic field magnitude (uT) + float fFourBsq; // current 4*B*B (uT^2) + float fFitErrorpc; // current fit error % + float ftrV[3]; // trial value of hard iron offset z, y, z (uT) + float ftrinvW[3][3]; // trial inverse soft iron matrix size + float ftrB; // trial value of geomagnetic field magnitude in uT + float ftrFitErrorpc; // trial value of fit error % + float fA[3][3]; // ellipsoid matrix A + float finvA[3][3]; // inverse of ellipsoid matrix A + float fmatA[10][10]; // scratch 10x10 matrix used by calibration algorithms + float fmatB[10][10]; // scratch 10x10 matrix used by calibration algorithms + float fvecA[10]; // scratch 10x1 vector used by calibration algorithms + float fvecB[4]; // scratch 4x1 vector used by calibration algorithms + int8_t iCalInProgress; // flag denoting that a calibration is in progress + int8_t iMagCalHasRun; // flag denoting that at least one calibration has been launched + int8_t iValidMagCal; // integer value 0, 4, 7, 10 denoting both valid calibration and solver used +}; + +void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer); +void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer); +void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer); + + +void f3x3matrixAeqI(float A[][3]); +void fmatrixAeqI(float *A[], int16_t rc); +void f3x3matrixAeqScalar(float A[][3], float Scalar); +void f3x3matrixAeqInvSymB(float A[][3], float B[][3]); +void f3x3matrixAeqAxScalar(float A[][3], float Scalar); +void f3x3matrixAeqMinusA(float A[][3]); +float f3x3matrixDetA(float A[][3]); +void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n); +void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivot[], int8_t isize); +void fmatrixAeqRenormRotA(float A[][3]); + + + #ifdef __cplusplus } // extern "C" #endif diff --git a/magcal.c b/magcal.c new file mode 100644 index 0000000..0045872 --- /dev/null +++ b/magcal.c @@ -0,0 +1,530 @@ +// Copyright (c) 2014, Freescale Semiconductor, Inc. +// All rights reserved. +// vim: set ts=4: +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Freescale Semiconductor, Inc. nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// This file contains magnetic calibration functions. It is STRONGLY RECOMMENDED +// that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file +// is extremely complex, and it will be very easy (almost inevitable) that you screw it +// up. +// + +#include "imuread.h" + +#define FXOS8700_UTPERCOUNT 0.1f +#define DEFAULTB 50.0F // default geomagnetic field (uT) +// vector components +#define X 0 +#define Y 1 +#define Z 2 +#define ONETHIRD 0.33333333F // one third +#define ONESIXTH 0.166666667F // one sixth + + +// 4 element calibration using 4x4 matrix inverse +void fUpdateCalibration4INV(struct MagCalibration *MagCal, + struct MagneticBuffer *MagBuffer) +{ + // local variables + float fBp2; // fBp[X]^2+fBp[Y]^2+fBp[Z]^2 + float fSumBp4; // sum of fBp2 + float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING + float fE; // error function = r^T.r + int16_t iOffset[3]; // offset to remove large DC hard iron bias in matrix + int16_t iCount; // number of measurements counted + int8_t i, j, k, l; // loop counters + + // working arrays for 4x4 matrix inversion + float *pfRows[4]; + int8_t iColInd[4]; + int8_t iRowInd[4]; + int8_t iPivot[4]; + + // compute fscaling to reduce multiplications later + fscaling = FXOS8700_UTPERCOUNT / DEFAULTB; + + // the trial inverse soft iron matrix invW always equals + // the identity matrix for 4 element calibration + f3x3matrixAeqI(MagCal->ftrinvW); + + // zero fSumBp4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above + // diagonal elements of fmatA=X^T*X (4x4) + fSumBp4 = 0.0F; + for (i = 0; i < 4; i++) { + MagCal->fvecB[i] = 0.0F; + for (j = i; j < 4; j++) { + MagCal->fmatA[i][j] = 0.0F; + } + } + + // the offsets are guaranteed to be set from the first element but to avoid compiler error + iOffset[X] = iOffset[Y] = iOffset[Z] = 0; + + // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices + iCount = 0; + for (j = 0; j < MAGBUFFSIZEX; j++) { + for (k = 0; k < MAGBUFFSIZEY; k++) { + if (MagBuffer->index[j][k] != -1) { + // use first valid magnetic buffer entry as estimate (in counts) for offset + if (iCount == 0) { + for (l = X; l <= Z; l++) { + iOffset[l] = MagBuffer->iBpFast[l][j][k]; + } + } + + // store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5] + for (l = X; l <= Z; l++) { + MagCal->fvecA[l] = (float)((int32_t)MagBuffer->iBpFast[l][j][k] + - (int32_t)iOffset[l]) * fscaling; + MagCal->fvecA[l + 3] = MagCal->fvecA[l] * MagCal->fvecA[l]; + } + + // calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2) + fBp2 = MagCal->fvecA[3] + MagCal->fvecA[4] + MagCal->fvecA[5]; + + // accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y + fSumBp4 += fBp2 * fBp2; + + // now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ]) + for (l = X; l <= Z; l++) { + MagCal->fvecB[l] += MagCal->fvecA[l] * fBp2; + } + + //accumulate fvecB[3] = X^T.Y =sum(fBp2) + MagCal->fvecB[3] += fBp2; + + // accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3] + MagCal->fmatA[0][0] += MagCal->fvecA[X + 3]; + MagCal->fmatA[0][1] += MagCal->fvecA[X] * MagCal->fvecA[Y]; + MagCal->fmatA[0][2] += MagCal->fvecA[X] * MagCal->fvecA[Z]; + MagCal->fmatA[0][3] += MagCal->fvecA[X]; + MagCal->fmatA[1][1] += MagCal->fvecA[Y + 3]; + MagCal->fmatA[1][2] += MagCal->fvecA[Y] * MagCal->fvecA[Z]; + MagCal->fmatA[1][3] += MagCal->fvecA[Y]; + MagCal->fmatA[2][2] += MagCal->fvecA[Z + 3]; + MagCal->fmatA[2][3] += MagCal->fvecA[Z]; + + // increment the counter for next iteration + iCount++; + } + } + } + + // set the last element of the measurement matrix to the number of buffer elements used + MagCal->fmatA[3][3] = (float) iCount; + + // store the number of measurements accumulated (defensive programming, should never be needed) + MagBuffer->iMagBufferCount = iCount; + + // use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X + for (i = 0; i < 4; i++) { + for (j = i; j < 4; j++) { + MagCal->fmatB[i][j] = MagCal->fmatB[j][i] + = MagCal->fmatA[j][i] = MagCal->fmatA[i][j]; + } + } + + // calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X + for (i = 0; i < 4; i++) { + pfRows[i] = MagCal->fmatB[i]; + } + fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4); + + // calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB + for (i = 0; i < 4; i++) { + MagCal->fvecA[i] = 0.0F; + for (k = 0; k < 4; k++) { + MagCal->fvecA[i] += MagCal->fmatB[i][k] * MagCal->fvecB[k]; + } + } + + // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta + // = fSumBp4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA + // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBp4 - 2 * fvecA^T.fvecB + fE = 0.0F; + for (i = 0; i < 4; i++) { + fE += MagCal->fvecA[i] * MagCal->fvecB[i]; + } + fE = fSumBp4 - 2.0F * fE; + + // set fvecB = (X^T.X).beta = fmatA.fvecA + for (i = 0; i < 4; i++) { + MagCal->fvecB[i] = 0.0F; + for (k = 0; k < 4; k++) { + MagCal->fvecB[i] += MagCal->fmatA[i][k] * MagCal->fvecA[k]; + } + } + + // complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB + for (i = 0; i < 4; i++) { + fE += MagCal->fvecB[i] * MagCal->fvecA[i]; + } + + // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING) + for (l = X; l <= Z; l++) { + MagCal->ftrV[l] = 0.5F * MagCal->fvecA[l]; + } + + // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING) + MagCal->ftrB = sqrtf(MagCal->fvecA[3] + MagCal->ftrV[X] * MagCal->ftrV[X] + + MagCal->ftrV[Y] * MagCal->ftrV[Y] + MagCal->ftrV[Z] * MagCal->ftrV[Z]); + + // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength + MagCal->ftrFitErrorpc = sqrtf(fE / (float) MagBuffer->iMagBufferCount) * 100.0F / + (2.0F * MagCal->ftrB * MagCal->ftrB); + + // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT) + for (l = X; l <= Z; l++) { + MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB + + (float)iOffset[l] * FXOS8700_UTPERCOUNT; + } + + // correct the geomagnetic field strength B to correct scaling (result in uT) + MagCal->ftrB *= DEFAULTB; +} + + + + + + + + + + +// 7 element calibration using direct eigen-decomposition +void fUpdateCalibration7EIG(struct MagCalibration *MagCal, + struct MagneticBuffer *MagBuffer) +{ + // local variables + float det; // matrix determinant + float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING + float ftmp; // scratch variable + int16_t iOffset[3]; // offset to remove large DC hard iron bias + int16_t iCount; // number of measurements counted + int8_t i, j, k, l, m, n; // loop counters + + // compute fscaling to reduce multiplications later + fscaling = FXOS8700_UTPERCOUNT / 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++) { + MagCal->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 (MagBuffer->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] = MagBuffer->iBpFast[l][j][k]; + } + } + + // apply the offset and scaling and store in fvecA + for (l = X; l <= Z; l++) { + MagCal->fvecA[l + 3] = (float)((int32_t)MagBuffer->iBpFast[l][j][k] - (int32_t)iOffset[l]) * fscaling; + MagCal->fvecA[l] = MagCal->fvecA[l + 3] * MagCal->fvecA[l + 3]; + } + + // accumulate the on-and above-diagonal terms of MagCal->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++) { + MagCal->fmatA[m][6] += MagCal->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++) { + MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n]; + } + } + + // increment the measurement counter for the next iteration + iCount++; + } + } + } + + // finally set the last element fmatA[6][6] to the number of measurements + MagCal->fmatA[6][6] = (float) iCount; + + // store the number of measurements accumulated (defensive programming, should never be needed) + MagBuffer->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++) { + MagCal->fmatA[m][n] = MagCal->fmatA[n][m]; + } + } + + // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA + eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 7); + + // find the smallest eigenvalue + j = 0; + for (i = 1; i < 7; i++) { + if (MagCal->fvecA[i] < MagCal->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(MagCal->fA, 0.0F); + det = 1.0F; + for (l = X; l <= Z; l++) { + MagCal->fA[l][l] = MagCal->fmatB[l][j]; + det *= MagCal->fA[l][l]; + MagCal->ftrV[l] = -0.5F * MagCal->fmatB[l + 3][j] / MagCal->fA[l][l]; + } + + // negate A if it has negative determinant + if (det < 0.0F) { + f3x3matrixAeqMinusA(MagCal->fA); + MagCal->fmatB[6][j] = -MagCal->fmatB[6][j]; + det = -det; + } + + // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING) + ftmp = -MagCal->fmatB[6][j]; + for (l = X; l <= Z; l++) { + ftmp += MagCal->fA[l][l] * MagCal->ftrV[l] * MagCal->ftrV[l]; + } + + // calculate the trial normalized fit error as a percentage + MagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(MagCal->fvecA[j]) / (float) MagBuffer->iMagBufferCount) / fabs(ftmp); + + // normalize the ellipsoid matrix A to unit determinant + f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD))); + + // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize + MagCal->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(MagCal->ftrinvW); + for (l = X; l <= Z; l++) { + MagCal->ftrinvW[l][l] = sqrtf(fabs(MagCal->fA[l][l])); + MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB + + (float)iOffset[l] * FXOS8700_UTPERCOUNT; + } + + return; +} + +// 10 element calibration using direct eigen-decomposition +void fUpdateCalibration10EIG(struct MagCalibration *MagCal, + struct MagneticBuffer *MagBuffer) +{ + // local variables + float det; // matrix determinant + float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING + float ftmp; // scratch variable + int16_t iOffset[3]; // offset to remove large DC hard iron bias in matrix + int16_t iCount; // number of measurements counted + int8_t i, j, k, l, m, n; // loop counters + + // compute fscaling to reduce multiplications later + fscaling = FXOS8700_UTPERCOUNT / 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++) { + MagCal->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 (MagBuffer->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] = MagBuffer->iBpFast[l][j][k]; + } + } + + // apply the fixed offset and scaling and enter into fvecA[6-8] + for (l = X; l <= Z; l++) { + MagCal->fvecA[l + 6] = (float)((int32_t)MagBuffer->iBpFast[l][j][k] - (int32_t)iOffset[l]) * fscaling; + } + + // compute measurement vector elements fvecA[0-5] from fvecA[6-8] + MagCal->fvecA[0] = MagCal->fvecA[6] * MagCal->fvecA[6]; + MagCal->fvecA[1] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[7]; + MagCal->fvecA[2] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[8]; + MagCal->fvecA[3] = MagCal->fvecA[7] * MagCal->fvecA[7]; + MagCal->fvecA[4] = 2.0F * MagCal->fvecA[7] * MagCal->fvecA[8]; + MagCal->fvecA[5] = MagCal->fvecA[8] * MagCal->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++) { + MagCal->fmatA[m][9] += MagCal->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++) { + MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n]; + } + } + + // increment the measurement counter for the next iteration + iCount++; + } + } + } + + // set the last element fmatA[9][9] to the number of measurements + MagCal->fmatA[9][9] = (float) iCount; + + // store the number of measurements accumulated (defensive programming, should never be needed) + MagBuffer->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++) { + MagCal->fmatA[m][n] = MagCal->fmatA[n][m]; + } + } + + // set MagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA + eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->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 (MagCal->fvecA[i] < MagCal->fvecA[j]) { + j = i; + } + } + MagCal->fA[0][0] = MagCal->fmatB[0][j]; + MagCal->fA[0][1] = MagCal->fA[1][0] = MagCal->fmatB[1][j]; + MagCal->fA[0][2] = MagCal->fA[2][0] = MagCal->fmatB[2][j]; + MagCal->fA[1][1] = MagCal->fmatB[3][j]; + MagCal->fA[1][2] = MagCal->fA[2][1] = MagCal->fmatB[4][j]; + MagCal->fA[2][2] = MagCal->fmatB[5][j]; + + // negate entire solution if A has negative determinant + det = f3x3matrixDetA(MagCal->fA); + if (det < 0.0F) { + f3x3matrixAeqMinusA(MagCal->fA); + MagCal->fmatB[6][j] = -MagCal->fmatB[6][j]; + MagCal->fmatB[7][j] = -MagCal->fmatB[7][j]; + MagCal->fmatB[8][j] = -MagCal->fmatB[8][j]; + MagCal->fmatB[9][j] = -MagCal->fmatB[9][j]; + det = -det; + } + + // compute the inverse of the ellipsoid matrix + f3x3matrixAeqInvSymB(MagCal->finvA, MagCal->fA); + + // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING + for (l = X; l <= Z; l++) { + MagCal->ftrV[l] = 0.0F; + for (m = X; m <= Z; m++) { + MagCal->ftrV[l] += MagCal->finvA[l][m] * MagCal->fmatB[m + 6][j]; + } + MagCal->ftrV[l] *= -0.5F; + } + + // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING + MagCal->ftrB = sqrtf(fabs(MagCal->fA[0][0] * MagCal->ftrV[X] * MagCal->ftrV[X] + + 2.0F * MagCal->fA[0][1] * MagCal->ftrV[X] * MagCal->ftrV[Y] + + 2.0F * MagCal->fA[0][2] * MagCal->ftrV[X] * MagCal->ftrV[Z] + + MagCal->fA[1][1] * MagCal->ftrV[Y] * MagCal->ftrV[Y] + + 2.0F * MagCal->fA[1][2] * MagCal->ftrV[Y] * MagCal->ftrV[Z] + + MagCal->fA[2][2] * MagCal->ftrV[Z] * MagCal->ftrV[Z] - MagCal->fmatB[9][j])); + + // calculate the trial normalized fit error as a percentage + MagCal->ftrFitErrorpc = 50.0F * sqrtf( + fabs(MagCal->fvecA[j]) / (float) MagBuffer->iMagBufferCount) / + (MagCal->ftrB * MagCal->ftrB); + + // correct for the measurement matrix offset and scaling and + // get the computed hard iron offset in uT + for (l = X; l <= Z; l++) { + MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB + + (float)iOffset[l] * FXOS8700_UTPERCOUNT; + } + + // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A + MagCal->ftrB *= DEFAULTB; + + // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor + f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD))); + MagCal->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++) { + MagCal->fmatA[i][j] = MagCal->fA[i][j]; + } + } + eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 3); + + // set MagCal->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(MagCal->fvecA[j]))); + for (i = 0; i < 3; i++) { // loop over rows i + MagCal->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++) { + MagCal->ftrinvW[i][j] = 0.0F; + // accumulate the matrix product + for (k = 0; k < 3; k++) { + MagCal->ftrinvW[i][j] += MagCal->fmatB[i][k] * MagCal->fmatB[j][k]; + } + // copy to below diagonal element + MagCal->ftrinvW[j][i] = MagCal->ftrinvW[i][j]; + } + } +} + + diff --git a/matrix.c b/matrix.c new file mode 100644 index 0000000..733dfd2 --- /dev/null +++ b/matrix.c @@ -0,0 +1,446 @@ +// Copyright (c) 2014, Freescale Semiconductor, Inc. +// All rights reserved. +// vim: set ts=4: +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Freescale Semiconductor, Inc. nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// This file contains matrix manipulation functions. +// +#include "imuread.h" + +// compile time constants that are private to this file +#define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix + +// vector components +#define X 0 +#define Y 1 +#define Z 2 + +// function sets the 3x3 matrix A to the identity matrix +void f3x3matrixAeqI(float A[][3]) +{ + float *pAij; // pointer to A[i][j] + int8_t i, j; // loop counters + + for (i = 0; i < 3; i++) { + // set pAij to &A[i][j=0] + pAij = A[i]; + for (j = 0; j < 3; j++) { + *(pAij++) = 0.0F; + } + A[i][i] = 1.0F; + } +} + +// function sets the matrix A to the identity matrix +void fmatrixAeqI(float *A[], int16_t rc) +{ + // rc = rows and columns in A + + float *pAij; // pointer to A[i][j] + int8_t i, j; // loop counters + + for (i = 0; i < rc; i++) { + // set pAij to &A[i][j=0] + pAij = A[i]; + for (j = 0; j < rc; j++) { + *(pAij++) = 0.0F; + } + A[i][i] = 1.0F; + } +} + +// function sets every entry in the 3x3 matrix A to a constant scalar +void f3x3matrixAeqScalar(float A[][3], float Scalar) +{ + float *pAij; // pointer to A[i][j] + int8_t i, j; // counters + + for (i = 0; i < 3; i++) { + // set pAij to &A[i][j=0] + pAij = A[i]; + for (j = 0; j < 3; j++) { + *(pAij++) = Scalar; + } + } +} + +// function multiplies all elements of 3x3 matrix A by the specified scalar +void f3x3matrixAeqAxScalar(float A[][3], float Scalar) +{ + float *pAij; // pointer to A[i][j] + int8_t i, j; // loop counters + + for (i = 0; i < 3; i++) { + // set pAij to &A[i][j=0] + pAij = A[i]; + for (j = 0; j < 3; j++) { + *(pAij++) *= Scalar; + } + } +} + +// function negates all elements of 3x3 matrix A +void f3x3matrixAeqMinusA(float A[][3]) +{ + float *pAij; // pointer to A[i][j] + int8_t i, j; // loop counters + + for (i = 0; i < 3; i++) { + // set pAij to &A[i][j=0] + pAij = A[i]; + for (j = 0; j < 3; j++) { + *pAij = -*pAij; + pAij++; + } + } +} + +// function directly calculates the symmetric inverse of a symmetric 3x3 matrix +// only the on and above diagonal terms in B are used and need to be specified +void f3x3matrixAeqInvSymB(float A[][3], float B[][3]) +{ + float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2] + float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2] + float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2] + float ftmp; // determinant and then reciprocal + + // calculate useful products + fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2]; + fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2]; + fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2]; + + // set ftmp to the determinant of the input matrix B + ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02; + + // set A to the inverse of B for any determinant except zero + if (ftmp != 0.0F) { + ftmp = 1.0F / ftmp; + A[0][0] = fB11B22mB12B12 * ftmp; + A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp; + A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp; + A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp; + A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp; + A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp; + } else { + // provide the identity matrix if the determinant is zero + f3x3matrixAeqI(A); + } +} + +// function calculates the determinant of a 3x3 matrix +float f3x3matrixDetA(float A[][3]) +{ + return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) + + A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) + + A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X])); +} + +// function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1] +// stored in the top left of a 10x10 array A[10][10] +// A[][] is changed on output. +// eigval[0..n-1] returns the eigenvalues of A[][]. +// eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][] +// the eigenvectors are not sorted by value +void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) +{ + // maximum number of iterations to achieve convergence: in practice 6 is typical +#define NITERATIONS 15 + + // various trig functions of the jacobi rotation angle phi + float cot2phi, tanhalfphi, tanphi, sinphi, cosphi; + // scratch variable to prevent over-writing during rotations + float ftmp; + // residue from remaining non-zero above diagonal terms + float residue; + // matrix row and column indices + int8_t ir, ic; + // general loop counter + int8_t j; + // timeout ctr for number of passes of the algorithm + int8_t ctr; + + // initialize eigenvectors matrix and eigenvalues array + for (ir = 0; ir < n; ir++) { + // loop over all columns + for (ic = 0; ic < n; ic++) { + // set on diagonal and off-diagonal elements to zero + eigvec[ir][ic] = 0.0F; + } + + // correct the diagonal elements to 1.0 + eigvec[ir][ir] = 1.0F; + + // initialize the array of eigenvalues to the diagonal elements of m + eigval[ir] = A[ir][ir]; + } + + // initialize the counter and loop until converged or NITERATIONS reached + ctr = 0; + do { + // compute the absolute value of the above diagonal elements as exit criterion + residue = 0.0F; + // loop over rows excluding last row + for (ir = 0; ir < n - 1; ir++) { + // loop over above diagonal columns + for (ic = ir + 1; ic < n; ic++) { + // accumulate the residual off diagonal terms which are being driven to zero + residue += fabs(A[ir][ic]); + } + } + + // check if we still have work to do + if (residue > 0.0F) { + // loop over all rows with the exception of the last row (since only rotating above diagonal elements) + for (ir = 0; ir < n - 1; ir++) { + // loop over columns ic (where ic is always greater than ir since above diagonal) + for (ic = ir + 1; ic < n; ic++) { + // only continue with this element if the element is non-zero + if (fabs(A[ir][ic]) > 0.0F) { + // calculate cot(2*phi) where phi is the Jacobi rotation angle + cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]); + + // calculate tan(phi) correcting sign to ensure the smaller solution is used + tanphi = 1.0F / (fabs(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi)); + if (cot2phi < 0.0F) { + tanphi = -tanphi; + } + + // calculate the sine and cosine of the Jacobi rotation angle phi + cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi); + sinphi = tanphi * cosphi; + + // calculate tan(phi/2) + tanhalfphi = sinphi / (1.0F + cosphi); + + // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements + ftmp = tanphi * A[ir][ic]; + + // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array + // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic] + eigval[ir] -= ftmp; + // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic] + eigval[ic] += ftmp; + + // by definition, applying the jacobi rotation on element ir, ic results in 0.0 + A[ir][ic] = 0.0F; + + // apply the jacobi rotation to all elements of the eigenvector matrix + for (j = 0; j < n; j++) { + // store eigvec[j][ir] + ftmp = eigvec[j][ir]; + // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir]) + eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp); + // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic]) + eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]); + } + + // apply the jacobi rotation only to those elements of matrix m that can change + for (j = 0; j <= ir - 1; j++) { + // store A[j][ir] + ftmp = A[j][ir]; + // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir]) + A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp); + // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic]) + A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]); + } + for (j = ir + 1; j <= ic - 1; j++) { + // store A[ir][j] + ftmp = A[ir][j]; + // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j]) + A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp); + // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic]) + A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]); + } + for (j = ic + 1; j < n; j++) { + // store A[ir][j] + ftmp = A[ir][j]; + // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j]) + A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp); + // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j]) + A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]); + } + } // end of test for matrix element already zero + } // end of loop over columns + } // end of loop over rows + } // end of test for non-zero residue + } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop +} + +// 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_t iColInd[], int8_t iRowInd[], int8_t iPivot[], int8_t isize) +{ + 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_t i, j, k, l, m; // index counters + int8_t iPivotRow, iPivotCol; // row and column of pivot element + + // to avoid compiler warnings + iPivotRow = iPivotCol = 0; + + // 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 (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] > 1) { + // zero determinant situation: exit with identity matrix + fmatrixAeqI(A, isize); + 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 + fmatrixAeqI(A, isize); + 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]; + 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; + } + } + } +} + +// function re-orthonormalizes a 3x3 rotation matrix +void fmatrixAeqRenormRotA(float A[][3]) +{ + float ftmp; // scratch variable + + // normalize the X column of the low pass filtered orientation matrix + ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]); + if (ftmp > CORRUPTMATRIX) { + // normalize the x column vector + ftmp = 1.0F / ftmp; + A[X][X] *= ftmp; + A[Y][X] *= ftmp; + A[Z][X] *= ftmp; + } else { + // set x column vector to {1, 0, 0} + A[X][X] = 1.0F; + A[Y][X] = A[Z][X] = 0.0F; + } + + // force the y column vector to be orthogonal to x using y = y-(x.y)x + ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y]; + A[X][Y] -= ftmp * A[X][X]; + A[Y][Y] -= ftmp * A[Y][X]; + A[Z][Y] -= ftmp * A[Z][X]; + + // normalize the y column vector + ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]); + if (ftmp > CORRUPTMATRIX) { + // normalize the y column vector + ftmp = 1.0F / ftmp; + A[X][Y] *= ftmp; + A[Y][Y] *= ftmp; + A[Z][Y] *= ftmp; + } else { + // set y column vector to {0, 1, 0} + A[Y][Y] = 1.0F; + A[X][Y] = A[Z][Y] = 0.0F; + } + + // finally set the z column vector to x vector cross y vector (automatically normalized) + A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y]; + A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y]; + A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y]; +} diff --git a/visualize.c b/visualize.c index 0c5c760..18d7314 100644 --- a/visualize.c +++ b/visualize.c @@ -109,7 +109,7 @@ static GLuint spherelowreslist; void display_callback(void) { - static int updatenum=0; + //static int updatenum=0; int i, count=0; float xscale, yscale, zscale; float xoff, yoff, zoff;