jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Inport Freescale's magnetic calibration math

Browse source
This commit is contained in:
PaulStoffregen 2016-03-10 08:37:02 -08:00
commit ae4c221308
5 changed files with 1030 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -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)

52
imuread.h
View file

@ -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

530
magcal.c Normal file
View file

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

446
matrix.c Normal file
View file

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

2
visualize.c
View file

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