Witek Ewert / Mbed 2 deprecated kl46_accel_mag

KL46 Accelerometer + Magnetometer, with 3-axis calibration. Readout through OpenSDA CDC.

Dependencies:   MAG3110 MMA8451Q USBDevice mbed

magnetic.cpp@0:c569d820861b, 2014-04-10 (annotated)

Committer:
wue
Date:
Thu Apr 10 07:48:46 2014 +0000
Revision:
0:c569d820861b
KL46Z Accelerometer & Magnetometer readout over OpenSDA CDC.; 3-axis 4-value calibration taken from eCompass.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wue 0:c569d820861b 1 // Source: eCompass v3
wue 0:c569d820861b 2
wue 0:c569d820861b 3 #include "magnetic.h"
wue 0:c569d820861b 4 #include "mbed.h"
wue 0:c569d820861b 5
wue 0:c569d820861b 6 float xftmpA4x4[4][4], *ftmpA4x4[4]; // scratch 4x4 matrix
wue 0:c569d820861b 7 float xftmpB4x4[4][4], *ftmpB4x4[4]; // scratch 4x4 matrix
wue 0:c569d820861b 8 float xftmpA4x1[4][1], *ftmpA4x1[4]; // scratch 4x1 matrix
wue 0:c569d820861b 9 float xftmpB4x1[4][1], *ftmpB4x1[4]; // scratch 4x1 matrix
wue 0:c569d820861b 10
wue 0:c569d820861b 11 int32 xicolind[MAXMATINV][1], *icolind[MAXMATINV];
wue 0:c569d820861b 12 int32 xirowind[MAXMATINV][1], *irowind[MAXMATINV];
wue 0:c569d820861b 13 int32 xipivot[MAXMATINV][1], *ipivot[MAXMATINV];
wue 0:c569d820861b 14
wue 0:c569d820861b 15
wue 0:c569d820861b 16 void magUpdateCalibration(struct MagCalibration *pthisMagCal,
wue 0:c569d820861b 17 struct MagneticBuffer *pthisMagneticBuffer) {
wue 0:c569d820861b 18
wue 0:c569d820861b 19 int i;
wue 0:c569d820861b 20
wue 0:c569d820861b 21 //while(1);
wue 0:c569d820861b 22 // 4 row arrays
wue 0:c569d820861b 23 for (i = 0; i < 4; i++)
wue 0:c569d820861b 24 {
wue 0:c569d820861b 25 ftmpA4x4[i] = xftmpA4x4[i];
wue 0:c569d820861b 26 ftmpB4x4[i] = xftmpB4x4[i];
wue 0:c569d820861b 27 ftmpA4x1[i] = xftmpA4x1[i];
wue 0:c569d820861b 28 ftmpB4x1[i] = xftmpB4x1[i];
wue 0:c569d820861b 29 };
wue 0:c569d820861b 30
wue 0:c569d820861b 31 // MAXMATINV row arrays
wue 0:c569d820861b 32 for (i = 0; i < MAXMATINV; i++) {
wue 0:c569d820861b 33 icolind[i] = xicolind[i];
wue 0:c569d820861b 34 irowind[i] = xirowind[i];
wue 0:c569d820861b 35 ipivot[i] = xipivot[i];
wue 0:c569d820861b 36 };
wue 0:c569d820861b 37
wue 0:c569d820861b 38
wue 0:c569d820861b 39 fUpdateCalibration4INV(pthisMagCal, pthisMagneticBuffer, ftmpA4x4, ftmpB4x4, ftmpA4x1, ftmpB4x1, icolind, irowind, ipivot);
wue 0:c569d820861b 40
wue 0:c569d820861b 41 };
wue 0:c569d820861b 42
wue 0:c569d820861b 43
wue 0:c569d820861b 44
wue 0:c569d820861b 45
wue 0:c569d820861b 46 // function calculates the matrix product A = B x C
wue 0:c569d820861b 47 void fmatrixAeqBxC(float **A, float **B, float **C, int32 rB, int32 cBrC, int32 cC)
wue 0:c569d820861b 48 {
wue 0:c569d820861b 49 // rB = rows in B
wue 0:c569d820861b 50 // cBrC = columns in B = rows in C
wue 0:c569d820861b 51 // cC = columns in C
wue 0:c569d820861b 52 // A has dimension rB rows x cC columns
wue 0:c569d820861b 53
wue 0:c569d820861b 54 int32 i, j, k; // counters
wue 0:c569d820861b 55
wue 0:c569d820861b 56 for (i = 0; i < rB; i++)
wue 0:c569d820861b 57 {
wue 0:c569d820861b 58 for (j = 0; j < cC; j++)
wue 0:c569d820861b 59 {
wue 0:c569d820861b 60 A[i][j] = 0.0F;
wue 0:c569d820861b 61 for (k = 0; k < cBrC; k++)
wue 0:c569d820861b 62 A[i][j] += B[i][k] * C[k][j];
wue 0:c569d820861b 63 }
wue 0:c569d820861b 64 }
wue 0:c569d820861b 65 return;
wue 0:c569d820861b 66 }
wue 0:c569d820861b 67
wue 0:c569d820861b 68
wue 0:c569d820861b 69 // function sets the matrix A to the identity matrix
wue 0:c569d820861b 70 void fmatrixAeqI(float **A, int32 rc)
wue 0:c569d820861b 71 {
wue 0:c569d820861b 72 // rc = rows and columns in A
wue 0:c569d820861b 73
wue 0:c569d820861b 74 int32 i, j; // loop counters
wue 0:c569d820861b 75
wue 0:c569d820861b 76 for (i = 0; i < rc; i++)
wue 0:c569d820861b 77 {
wue 0:c569d820861b 78 for (j = 0; j < rc; j++)
wue 0:c569d820861b 79 {
wue 0:c569d820861b 80 A[i][j] = 0.0F;
wue 0:c569d820861b 81 }
wue 0:c569d820861b 82 A[i][i] = 1.0F;
wue 0:c569d820861b 83 }
wue 0:c569d820861b 84 return;
wue 0:c569d820861b 85 }
wue 0:c569d820861b 86
wue 0:c569d820861b 87
wue 0:c569d820861b 88 /* function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ */
wue 0:c569d820861b 89 /* on exit, A is replaced with its inverse */
wue 0:c569d820861b 90 void fmatrixAeqInvA(float **A, int32 isize, int32 **icolind, int32 **irowind, int32 **ipivot)
wue 0:c569d820861b 91 {
wue 0:c569d820861b 92 int32 i, j, k, l, m; // index counters
wue 0:c569d820861b 93 int32 ipivotrow, ipivotcol; // row and column of pivot element
wue 0:c569d820861b 94 float largest; // largest element used for pivoting
wue 0:c569d820861b 95 float scaling; // scaling factor in pivoting
wue 0:c569d820861b 96 float recippiv; // reciprocal of pivot element
wue 0:c569d820861b 97 float ftmp; // temporary variable used in swaps
wue 0:c569d820861b 98
wue 0:c569d820861b 99 // initialize the pivot array to 0
wue 0:c569d820861b 100 for (j = 0; j < isize; j++)
wue 0:c569d820861b 101 {
wue 0:c569d820861b 102 ipivot[j][0] = 0;
wue 0:c569d820861b 103 }
wue 0:c569d820861b 104
wue 0:c569d820861b 105 // main loop i over the dimensions of the square matrix A
wue 0:c569d820861b 106 for (i = 0; i < isize; i++)
wue 0:c569d820861b 107 {
wue 0:c569d820861b 108 // zero the largest element found for pivoting
wue 0:c569d820861b 109 largest = 0.0F;
wue 0:c569d820861b 110 // loop over candidate rows j
wue 0:c569d820861b 111 for (j = 0; j < isize; j++)
wue 0:c569d820861b 112 {
wue 0:c569d820861b 113 // check if row j has been previously pivoted
wue 0:c569d820861b 114 if (ipivot[j][0] != 1)
wue 0:c569d820861b 115 {
wue 0:c569d820861b 116 // loop over candidate columns k
wue 0:c569d820861b 117 for (k = 0; k < isize; k++)
wue 0:c569d820861b 118 {
wue 0:c569d820861b 119 // check if column k has previously been pivoted
wue 0:c569d820861b 120 if (ipivot[k][0] == 0)
wue 0:c569d820861b 121 {
wue 0:c569d820861b 122 // check if the pivot element is the largest found so far
wue 0:c569d820861b 123 if (fabs(A[j][k]) >= largest)
wue 0:c569d820861b 124 {
wue 0:c569d820861b 125 // and store this location as the current best candidate for pivoting
wue 0:c569d820861b 126 ipivotrow = j;
wue 0:c569d820861b 127 ipivotcol = k;
wue 0:c569d820861b 128 largest = (float) fabs(A[ipivotrow][ipivotcol]);
wue 0:c569d820861b 129 }
wue 0:c569d820861b 130 }
wue 0:c569d820861b 131 else if (ipivot[k][0] > 1)
wue 0:c569d820861b 132 {
wue 0:c569d820861b 133 // zero determinant situation: exit with identity matrix
wue 0:c569d820861b 134 fmatrixAeqI(A, 3);
wue 0:c569d820861b 135 return;
wue 0:c569d820861b 136 }
wue 0:c569d820861b 137 }
wue 0:c569d820861b 138 }
wue 0:c569d820861b 139 }
wue 0:c569d820861b 140 // increment the entry in ipivot to denote it has been selected for pivoting
wue 0:c569d820861b 141 ipivot[ipivotcol][0]++;
wue 0:c569d820861b 142
wue 0:c569d820861b 143 // check the pivot rows ipivotrow and ipivotcol are not the same before swapping
wue 0:c569d820861b 144 if (ipivotrow != ipivotcol)
wue 0:c569d820861b 145 {
wue 0:c569d820861b 146 // loop over columns l
wue 0:c569d820861b 147 for (l = 0; l < isize; l++)
wue 0:c569d820861b 148 {
wue 0:c569d820861b 149 // and swap all elements of rows ipivotrow and ipivotcol
wue 0:c569d820861b 150 ftmp = A[ipivotrow][l];
wue 0:c569d820861b 151 A[ipivotrow][l] = A[ipivotcol][l];
wue 0:c569d820861b 152 A[ipivotcol][l] = ftmp;
wue 0:c569d820861b 153 }
wue 0:c569d820861b 154 }
wue 0:c569d820861b 155
wue 0:c569d820861b 156 // record that on the i-th iteration rows ipivotrow and ipivotcol were swapped
wue 0:c569d820861b 157 irowind[i][0] = ipivotrow;
wue 0:c569d820861b 158 icolind[i][0] = ipivotcol;
wue 0:c569d820861b 159
wue 0:c569d820861b 160 // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
wue 0:c569d820861b 161 if (A[ipivotcol][ipivotcol] == 0.0F)
wue 0:c569d820861b 162 {
wue 0:c569d820861b 163 // zero determinant situation: exit with identity matrix
wue 0:c569d820861b 164 fmatrixAeqI(A, 3);
wue 0:c569d820861b 165 return;
wue 0:c569d820861b 166 }
wue 0:c569d820861b 167
wue 0:c569d820861b 168 // calculate the reciprocal of the pivot element knowing it's non-zero
wue 0:c569d820861b 169 recippiv = 1.0F / A[ipivotcol][ipivotcol];
wue 0:c569d820861b 170 // by definition, the diagonal element normalizes to 1
wue 0:c569d820861b 171 A[ipivotcol][ipivotcol] = 1.0F;
wue 0:c569d820861b 172 // multiply all of row ipivotcol by the reciprocal of the pivot element including the diagonal element
wue 0:c569d820861b 173 // the diagonal element A[ipivotcol][ipivotcol] now has value equal to the reciprocal of its previous value
wue 0:c569d820861b 174 for (l = 0; l < isize; l++)
wue 0:c569d820861b 175 {
wue 0:c569d820861b 176 A[ipivotcol][l] *= recippiv;
wue 0:c569d820861b 177 }
wue 0:c569d820861b 178 // loop over all rows m of A
wue 0:c569d820861b 179 for (m = 0; m < isize; m++)
wue 0:c569d820861b 180 {
wue 0:c569d820861b 181 if (m != ipivotcol)
wue 0:c569d820861b 182 {
wue 0:c569d820861b 183 // scaling factor for this row m is in column ipivotcol
wue 0:c569d820861b 184 scaling = A[m][ipivotcol];
wue 0:c569d820861b 185 // zero this element
wue 0:c569d820861b 186 A[m][ipivotcol] = 0.0F;
wue 0:c569d820861b 187 // loop over all columns l of A and perform elimination
wue 0:c569d820861b 188 for (l = 0; l < isize; l++)
wue 0:c569d820861b 189 {
wue 0:c569d820861b 190 A[m][l] -= A[ipivotcol][l] * scaling;
wue 0:c569d820861b 191 }
wue 0:c569d820861b 192 }
wue 0:c569d820861b 193 }
wue 0:c569d820861b 194 } // end of loop i over the matrix dimensions
wue 0:c569d820861b 195
wue 0:c569d820861b 196 // finally, loop in inverse order to apply the missing column swaps
wue 0:c569d820861b 197 for (l = isize - 1; l >= 0; l--)
wue 0:c569d820861b 198 {
wue 0:c569d820861b 199 // set i and j to the two columns to be swapped
wue 0:c569d820861b 200 i = irowind[l][0];
wue 0:c569d820861b 201 j = icolind[l][0];
wue 0:c569d820861b 202
wue 0:c569d820861b 203 // check that the two columns i and j to be swapped are not the same
wue 0:c569d820861b 204 if (i != j)
wue 0:c569d820861b 205 {
wue 0:c569d820861b 206 // loop over all rows k to swap columns i and j of A
wue 0:c569d820861b 207 for (k = 0; k < isize; k++)
wue 0:c569d820861b 208 {
wue 0:c569d820861b 209 ftmp = A[k][i];
wue 0:c569d820861b 210 A[k][i] = A[k][j];
wue 0:c569d820861b 211 A[k][j] = ftmp;
wue 0:c569d820861b 212 }
wue 0:c569d820861b 213 }
wue 0:c569d820861b 214 }
wue 0:c569d820861b 215
wue 0:c569d820861b 216 return;
wue 0:c569d820861b 217 }
wue 0:c569d820861b 218
wue 0:c569d820861b 219
wue 0:c569d820861b 220 void ResetMagCalibration(struct MagCalibration *pthisMagCal/*, struct MagneticBuffer *pthisMagneticBuffer*/)
wue 0:c569d820861b 221 {
wue 0:c569d820861b 222 int32 i, j, k, l; // loop counters
wue 0:c569d820861b 223
wue 0:c569d820861b 224 for (i = 0; i < 3; i++) {
wue 0:c569d820861b 225 pthisMagCal->finvW[i] = pthisMagCal->xfinvW[i];
wue 0:c569d820861b 226 pthisMagCal->ftrinvW[i] = pthisMagCal->xftrinvW[i];
wue 0:c569d820861b 227 pthisMagCal->fA[i] = pthisMagCal->xfA[i];
wue 0:c569d820861b 228 pthisMagCal->finvA[i] = pthisMagCal->xinvA[i];
wue 0:c569d820861b 229 };
wue 0:c569d820861b 230
wue 0:c569d820861b 231 // initialize the calibration hard and soft iron estimate to null
wue 0:c569d820861b 232 fmatrixAeqI(pthisMagCal->finvW, 3);
wue 0:c569d820861b 233 pthisMagCal->fVx = pthisMagCal->fVy = pthisMagCal->fVz = 0.0F;
wue 0:c569d820861b 234 pthisMagCal->fB = 0.0F;
wue 0:c569d820861b 235 pthisMagCal->fFitErrorpc = 1000.0F;
wue 0:c569d820861b 236 pthisMagCal->iValidMagCal = 0;
wue 0:c569d820861b 237
wue 0:c569d820861b 238 // set magnetic buffer index to invalid value -1 to denote invalid
wue 0:c569d820861b 239 /*pthisMagneticBuffer->iMagBufferCount = 0;
wue 0:c569d820861b 240 for (j = 0; j < MAGBUFFSIZE; j++)
wue 0:c569d820861b 241 {
wue 0:c569d820861b 242 for (k = 0; k < MAGBUFFSIZE; k++)
wue 0:c569d820861b 243 {
wue 0:c569d820861b 244 for (l = 0; l < MAGBUFFSIZE; l++)
wue 0:c569d820861b 245 {
wue 0:c569d820861b 246 pthisMagneticBuffer->index[j][k][l] = -1;
wue 0:c569d820861b 247 }
wue 0:c569d820861b 248 }
wue 0:c569d820861b 249 }*/
wue 0:c569d820861b 250
wue 0:c569d820861b 251 return;
wue 0:c569d820861b 252 }
wue 0:c569d820861b 253
wue 0:c569d820861b 254 // 4 element calibration using 4x4 matrix inverse
wue 0:c569d820861b 255 void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal,
wue 0:c569d820861b 256 struct MagneticBuffer *pthisMagneticBuffer,
wue 0:c569d820861b 257 float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1,
wue 0:c569d820861b 258 float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot)
wue 0:c569d820861b 259 {
wue 0:c569d820861b 260 int32 i, j, /*k, l,*/ m, n; // loop counters
wue 0:c569d820861b 261 int32 ilocalMagBufferCount; // local count of measurements for this process
wue 0:c569d820861b 262 float fOffsetx, fOffsety, fOffsetz; // offset to remove large DC hard iron bias in matrix
wue 0:c569d820861b 263 float ftmpBpx, ftmpBpy, ftmpBpz; // x, y, z magnetometer readings (uT)
wue 0:c569d820861b 264 float ftmpBpxsq, ftmpBpysq, ftmpBpzsq; // squares of x, y, z magnetometer readings (uT)
wue 0:c569d820861b 265 float fy; // dependent variable
wue 0:c569d820861b 266 float fYTY; // scalar equal to Y^T.Y
wue 0:c569d820861b 267 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
wue 0:c569d820861b 268 float fP; // performance function = r^T.r
wue 0:c569d820861b 269
wue 0:c569d820861b 270 // compute fscaling to reduce multiplications later
wue 0:c569d820861b 271 fscaling = FUTPERCOUNT * FMATRIXSCALING;
wue 0:c569d820861b 272
wue 0:c569d820861b 273 // set trial inverse soft iron matrix invW to the identity matrix for 4 element calibration
wue 0:c569d820861b 274 pthisMagCal->ftrinvW[0][0] = pthisMagCal->ftrinvW[1][1] = pthisMagCal->ftrinvW[2][2] = 1.0F;
wue 0:c569d820861b 275 pthisMagCal->ftrinvW[0][1] = pthisMagCal->ftrinvW[0][2] = pthisMagCal->ftrinvW[1][0] = 0.0F;
wue 0:c569d820861b 276 pthisMagCal->ftrinvW[1][2] = pthisMagCal->ftrinvW[2][0] = pthisMagCal->ftrinvW[2][1] = 0.0F;
wue 0:c569d820861b 277
wue 0:c569d820861b 278 // zero fYTY=Y^T.Y, ftmpA4x1=X^T.Y and on and above diagonal elements of ftmpA4x4=X^T*X
wue 0:c569d820861b 279 fYTY = 0.0F;
wue 0:c569d820861b 280 for (m = 0; m < 4; m++)
wue 0:c569d820861b 281 {
wue 0:c569d820861b 282 ftmpA4x1[m][0] = 0.0F;
wue 0:c569d820861b 283 for (n = m; n < 4; n++)
wue 0:c569d820861b 284 {
wue 0:c569d820861b 285 ftmpA4x4[m][n] = 0.0F;
wue 0:c569d820861b 286 }
wue 0:c569d820861b 287 }
wue 0:c569d820861b 288
wue 0:c569d820861b 289 // the offsets are guaranteed to be set from the first element but to avoid compiler error
wue 0:c569d820861b 290 fOffsetx = fOffsety = fOffsetz = 0.0F;
wue 0:c569d820861b 291
wue 0:c569d820861b 292 // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices
wue 0:c569d820861b 293 i = 0;
wue 0:c569d820861b 294 for (j = 0; j < MAGBUFFSIZE; j++)
wue 0:c569d820861b 295 {
wue 0:c569d820861b 296 //for (k = 0; k < MAGBUFFSIZE; k++)
wue 0:c569d820861b 297 //{
wue 0:c569d820861b 298 //for (l = 0; l < MAGBUFFSIZE; l++)
wue 0:c569d820861b 299 //{
wue 0:c569d820861b 300 //if (pthisMagneticBuffer->index[j][k][l] != -1)
wue 0:c569d820861b 301 //{
wue 0:c569d820861b 302 // use first valid magnetic buffer entry as estimate (in counts) for offset
wue 0:c569d820861b 303 printf(".");
wue 0:c569d820861b 304 if (i == 0)
wue 0:c569d820861b 305 {
wue 0:c569d820861b 306 fOffsetx = (float)pthisMagneticBuffer->iBx[j]/*[k][l]*/;
wue 0:c569d820861b 307 fOffsety = (float)pthisMagneticBuffer->iBy[j]/*[k][l]*/;
wue 0:c569d820861b 308 fOffsetz = (float)pthisMagneticBuffer->iBz[j]/*[k][l]*/;
wue 0:c569d820861b 309 }
wue 0:c569d820861b 310
wue 0:c569d820861b 311 // calculate offset and scaled magnetic buffer vector data Bx, By, Bz (scaled uT)
wue 0:c569d820861b 312 ftmpBpx = ((float)pthisMagneticBuffer->iBx[j]/*[k][l]*/ - fOffsetx) * fscaling;
wue 0:c569d820861b 313 ftmpBpy = ((float)pthisMagneticBuffer->iBy[j]/*[k][l]*/ - fOffsety) * fscaling;
wue 0:c569d820861b 314 ftmpBpz = ((float)pthisMagneticBuffer->iBz[j]/*[k][l]*/ - fOffsetz) * fscaling;
wue 0:c569d820861b 315
wue 0:c569d820861b 316 // calculate y = Bx^2 + By^2 + Bz^2 (scaled uT^2) and accumulate Y^T.Y
wue 0:c569d820861b 317 ftmpBpxsq = ftmpBpx * ftmpBpx;
wue 0:c569d820861b 318 ftmpBpysq = ftmpBpy * ftmpBpy;
wue 0:c569d820861b 319 ftmpBpzsq = ftmpBpz * ftmpBpz;
wue 0:c569d820861b 320 fy = ftmpBpxsq + ftmpBpysq + ftmpBpzsq;
wue 0:c569d820861b 321 fYTY += fy * fy;
wue 0:c569d820861b 322
wue 0:c569d820861b 323 // accumulate ftmpA4x1 = X^T.Y
wue 0:c569d820861b 324 ftmpA4x1[0][0] += ftmpBpx * fy;
wue 0:c569d820861b 325 ftmpA4x1[1][0] += ftmpBpy * fy;
wue 0:c569d820861b 326 ftmpA4x1[2][0] += ftmpBpz * fy;
wue 0:c569d820861b 327 ftmpA4x1[3][0] += fy;
wue 0:c569d820861b 328
wue 0:c569d820861b 329 // accumulate on and above-diagonal terms of ftmpA4x4 = X^T.X
wue 0:c569d820861b 330 ftmpA4x4[0][0] += ftmpBpxsq;
wue 0:c569d820861b 331 ftmpA4x4[0][1] += ftmpBpx * ftmpBpy;
wue 0:c569d820861b 332 ftmpA4x4[0][2] += ftmpBpx * ftmpBpz;
wue 0:c569d820861b 333 ftmpA4x4[0][3] += ftmpBpx;
wue 0:c569d820861b 334 ftmpA4x4[1][1] += ftmpBpysq;
wue 0:c569d820861b 335 ftmpA4x4[1][2] += ftmpBpy * ftmpBpz;
wue 0:c569d820861b 336 ftmpA4x4[1][3] += ftmpBpy;
wue 0:c569d820861b 337 ftmpA4x4[2][2] += ftmpBpzsq;
wue 0:c569d820861b 338 ftmpA4x4[2][3] += ftmpBpz;
wue 0:c569d820861b 339
wue 0:c569d820861b 340 // increment the counter for next iteration
wue 0:c569d820861b 341 i++;
wue 0:c569d820861b 342 //}
wue 0:c569d820861b 343 //}
wue 0:c569d820861b 344 //}
wue 0:c569d820861b 345 }
wue 0:c569d820861b 346 //printf("[dbg1]");
wue 0:c569d820861b 347
wue 0:c569d820861b 348
wue 0:c569d820861b 349 // store the number of measurements accumulated
wue 0:c569d820861b 350 ilocalMagBufferCount = i;
wue 0:c569d820861b 351
wue 0:c569d820861b 352 // set the last element of the measurement matrix to the number of buffer elements used
wue 0:c569d820861b 353 ftmpA4x4[3][3] = (float) i;
wue 0:c569d820861b 354
wue 0:c569d820861b 355 // use above diagonal elements of symmetric matrix ftmpA4x4 to create ftmpB4x4 = ftmpA4x4 = X^T.X
wue 0:c569d820861b 356 for (m = 0; m < 4; m++)
wue 0:c569d820861b 357 {
wue 0:c569d820861b 358 for (n = m; n < 4; n++)
wue 0:c569d820861b 359 {
wue 0:c569d820861b 360 ftmpB4x4[m][n] = ftmpB4x4[n][m] = ftmpA4x4[n][m] = ftmpA4x4[m][n];
wue 0:c569d820861b 361 }
wue 0:c569d820861b 362 }
wue 0:c569d820861b 363
wue 0:c569d820861b 364 //printf("[dbg2]");
wue 0:c569d820861b 365 // calculate in situ inverse of ftmpB4x4 = inv(X^T.X) (4x4)
wue 0:c569d820861b 366 fmatrixAeqInvA(ftmpB4x4, 4, icolind, irowind, ipivot);
wue 0:c569d820861b 367
wue 0:c569d820861b 368 // calculate ftmpB4x1 = solution vector beta (4x1) = inv(X^T.X).X^T.Y = ftmpB4x4 * ftmpA4x1
wue 0:c569d820861b 369 fmatrixAeqBxC(ftmpB4x1, ftmpB4x4, ftmpA4x1, 4, 4, 1);
wue 0:c569d820861b 370
wue 0:c569d820861b 371 // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
wue 0:c569d820861b 372 // = fYTY - 2 * ftmpB4x1^T.ftmpA4x1 + ftmpB4x1^T.ftmpA4x4.ftmpB4x1
wue 0:c569d820861b 373 // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fYTY - 2 * ftmpB4x1^T.ftmpA4x1
wue 0:c569d820861b 374 fP = fYTY - 2.0F * (ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] +
wue 0:c569d820861b 375 ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0]);
wue 0:c569d820861b 376 // set ftmpA4x1 = (X^T.X).beta = ftmpA4x4.ftmpB4x1
wue 0:c569d820861b 377 fmatrixAeqBxC(ftmpA4x1, ftmpA4x4, ftmpB4x1, 4, 4, 1);
wue 0:c569d820861b 378 // add beta^T.(X^T.X).beta = ftmpB4x1^T * ftmpA4x1 to P
wue 0:c569d820861b 379 fP += ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] +
wue 0:c569d820861b 380 ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0];
wue 0:c569d820861b 381
wue 0:c569d820861b 382 // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
wue 0:c569d820861b 383 pthisMagCal->ftrVx = 0.5F * ftmpB4x1[0][0];
wue 0:c569d820861b 384 pthisMagCal->ftrVy = 0.5F * ftmpB4x1[1][0];
wue 0:c569d820861b 385 pthisMagCal->ftrVz = 0.5F * ftmpB4x1[2][0];
wue 0:c569d820861b 386
wue 0:c569d820861b 387 // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
wue 0:c569d820861b 388 pthisMagCal->ftrB = (float)sqrt(ftmpB4x1[3][0] + pthisMagCal->ftrVx * pthisMagCal->ftrVx +
wue 0:c569d820861b 389 pthisMagCal->ftrVy * pthisMagCal->ftrVy + pthisMagCal->ftrVz * pthisMagCal->ftrVz);
wue 0:c569d820861b 390
wue 0:c569d820861b 391 // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
wue 0:c569d820861b 392 pthisMagCal->ftrFitErrorpc = (float)sqrt(fP / (float) ilocalMagBufferCount) * 100.0F /
wue 0:c569d820861b 393 (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
wue 0:c569d820861b 394 //printf("\n\nTrial new calibration fit error=%9.4f%% versus previous %9.4f%%", pthisMagCal->ftrFitErrorpc, pthisMagCal->fFitErrorpc);
wue 0:c569d820861b 395
wue 0:c569d820861b 396 // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
wue 0:c569d820861b 397 pthisMagCal->ftrVx = pthisMagCal->ftrVx * FINVMATRIXSCALING + fOffsetx * FUTPERCOUNT;
wue 0:c569d820861b 398 pthisMagCal->ftrVy = pthisMagCal->ftrVy * FINVMATRIXSCALING + fOffsety * FUTPERCOUNT;
wue 0:c569d820861b 399 pthisMagCal->ftrVz = pthisMagCal->ftrVz * FINVMATRIXSCALING + fOffsetz * FUTPERCOUNT;
wue 0:c569d820861b 400 //printf("\n\nTrial new calibration hard iron (uT) Vx=%9.3f Vy=%9.3f Vz=%9.3f", pthisMagCal->ftrVx, pthisMagCal->ftrVy, pthisMagCal->ftrVz);
wue 0:c569d820861b 401
wue 0:c569d820861b 402 // correct the geomagnetic field strength B to correct scaling (result in uT)
wue 0:c569d820861b 403 pthisMagCal->ftrB *= FINVMATRIXSCALING;
wue 0:c569d820861b 404 //printf("\n\nTrial new calibration geomagnetic field (uT) B=%9.3f", pthisMagCal->ftrB);
wue 0:c569d820861b 405
wue 0:c569d820861b 406 // set the valid calibration flag to true
wue 0:c569d820861b 407 pthisMagCal->iValidMagCal = 1;
wue 0:c569d820861b 408
wue 0:c569d820861b 409 return;
wue 0:c569d820861b 410 }