ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
magnetic.c
Go to the documentation of this file.
1 // Copyright (c) 2014, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains magnetic calibration functions. It is STRONGLY RECOMMENDED
27 // that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file
28 // is extremely complex, and it will be very easy (almost inevitable) that you screw it
29 // up.
30 //
31 
32 #include "math.h"
33 #include "stdlib.h"
34 #include "matrix.h"
35 #include "math_constants.h"
36 #include "magnetic.h"
37 
38 #define ONETHIRD 0.33333333F // one third
39 #define ONESIXTH 0.166666667F // one sixth
40 
41 // function resets the magnetometer buffer and magnetic calibration
42 void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
43 {
44  int8 j, k; // loop counters
45 
46  // initialize the calibration hard and soft iron estimate to null
47  f3x3matrixAeqI(pthisMagCal->finvW);
48  pthisMagCal->fV[X] = pthisMagCal->fV[Y] = pthisMagCal->fV[Z] = 0.0F;
49  pthisMagCal->fB = DEFAULTB;
50  pthisMagCal->fFourBsq = 4.0F * pthisMagCal->fB * pthisMagCal->fB;
51  pthisMagCal->fFitErrorpc = 1000.0F;
52  pthisMagCal->iValidMagCal = 0;
53  pthisMagCal->iCalInProgress = 0;
54  pthisMagCal->iMagCalHasRun = 0;
55 
56  // set magnetic buffer index to invalid value -1 to denote invalid
57  pthisMagBuffer->iMagBufferCount = 0;
58  for (j = 0; j < MAGBUFFSIZEX; j++)
59  {
60  for (k = 0; k < MAGBUFFSIZEY; k++)
61  {
62  pthisMagBuffer->index[j][k] = -1;
63  }
64  }
65 
66  // initialize the array of (MAGBUFFSIZEX - 1) elements of 100 * tangents used for buffer indexing
67  // entries cover the range 100 * tan(-PI/2 + PI/MAGBUFFSIZEX), 100 * tan(-PI/2 + 2*PI/MAGBUFFSIZEX) to
68  // 100 * tan(-PI/2 + (MAGBUFFSIZEX - 1) * PI/MAGBUFFSIZEX).
69  // for MAGBUFFSIZEX=12, the entries range in value from -373 to +373
70  for (j = 0; j < (MAGBUFFSIZEX - 1); j++)
71  {
72  pthisMagBuffer->tanarray[j] = (int16) (100.0F * tanf(PI * (-0.5F + (float) (j + 1) / MAGBUFFSIZEX)));
73  }
74 
75  return;
76 }
77 
78 // function updates the magnetic measurement buffer with most recent magnetic data (typically 25Hz)
79 void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel,
80  struct MagSensor *pthisMag, int32 loopcounter)
81 {
82  // local variables
83  int32 idelta; // absolute vector distance
84  int32 i; // counter
85  int16 itanj, itank; // indexing accelerometer ratios
86  int8 j, k, l, m; // counters
87  int8 itooclose; // flag denoting measurement is too close to existing ones
88 
89  // calculate the magnetometer buffer bins from the accelerometer tangent ratios
90  if (pthisAccel->iGp[X] == 0) return;
91  itanj = (100 * (int32)pthisAccel->iGp[Y]) / ((int32)pthisAccel->iGp[X]);
92  itank = (100 * (int32)pthisAccel->iGp[Z]) / ((int32)pthisAccel->iGp[X]);
93  // map tangent ratios to bins j and k using equal angle bins: C guarantees left to right execution of the test
94  // and add an offset of MAGBUFFSIZEX bins to k to mimic atan2 on this ratio
95  // j will vary from 0 to MAGBUFFSIZEX - 1 and k from 0 to 2 * MAGBUFFSIZEX - 1
96  j = k = 0;
97  while ((j < (MAGBUFFSIZEX - 1) && (itanj >= pthisMagBuffer->tanarray[j]))) j++;
98  while ((k < (MAGBUFFSIZEX - 1) && (itank >= pthisMagBuffer->tanarray[k]))) k++;
99  if (pthisAccel->iGp[X] < 0) k += MAGBUFFSIZEX;
100 
101  // case 1: buffer is full and this bin has a measurement: over-write without increasing number of measurements
102  // this is the most common option at run time
103  if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] != -1))
104  {
105  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
106  for (i = X; i <= Z; i++)
107  {
108  pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
109  }
110  pthisMagBuffer->index[j][k] = loopcounter;
111  return;
112  } // end case 1
113 
114  // case 2: the buffer is full and this bin does not have a measurement: store and retire the oldest
115  // this is the second most common option at run time
116  if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] == -1))
117  {
118  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
119  for (i = X; i <= Z; i++)
120  {
121  pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
122  }
123  pthisMagBuffer->index[j][k] = loopcounter;
124 
125  // set l and m to the oldest active entry and disable it
126  i = loopcounter;
127  l = m = 0; // to avoid compiler complaint
128  for (j = 0; j < MAGBUFFSIZEX; j++)
129  {
130  for (k = 0; k < MAGBUFFSIZEY; k++)
131  {
132  // check if the time stamp is older than the oldest found so far (normally fails this test)
133  if (pthisMagBuffer->index[j][k] < i)
134  {
135  // check if this bin is active (normally passes this test)
136  if (pthisMagBuffer->index[j][k] != -1)
137  {
138  // set l and m to the indices of the oldest entry found so far
139  l = j;
140  m = k;
141  // set i to the time stamp of the oldest entry found so far
142  i = pthisMagBuffer->index[l][m];
143  } // end of test for active
144  } // end of test for older
145  } // end of loop over k
146  } // end of loop over j
147 
148  // deactivate the oldest measurement (no need to zero the measurement data)
149  pthisMagBuffer->index[l][m] = -1;
150  return;
151  } // end case 2
152 
153  // case 3: buffer is not full and this bin is empty: store and increment number of measurements
154  if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] == -1))
155  {
156  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
157  for (i = X; i <= Z; i++)
158  {
159  pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
160  }
161  pthisMagBuffer->index[j][k] = loopcounter;
162  (pthisMagBuffer->iMagBufferCount)++;
163  return;
164  } // end case 3
165 
166  // case 4: buffer is not full and this bin has a measurement: over-write if close or try to slot in
167  // elsewhere if not close to the other measurements so as to create a mesh at power up
168  if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) && (pthisMagBuffer->index[j][k] != -1))
169  {
170  // calculate the vector difference between current measurement and the buffer entry
171  idelta = 0;
172  for (i = X; i <= Z; i++)
173  {
174  idelta += abs((int32)pthisMag->iBp[i] - (int32)pthisMagBuffer->iBp[i][j][k]);
175  }
176  // check to see if the current reading is close to this existing magnetic buffer entry
177  if (idelta < MESHDELTACOUNTS)
178  {
179  // simply over-write the measurement and return
180  for (i = X; i <= Z; i++)
181  {
182  pthisMagBuffer->iBp[i][j][k] = pthisMag->iBp[i];
183  }
184  pthisMagBuffer->index[j][k] = loopcounter;
185  }
186  else
187  {
188  // reset the flag denoting that the current measurement is close to any measurement in the buffer
189  itooclose = 0;
190  // to avoid compiler warning
191  l = m = 0;
192  // loop over the buffer j from 0 potentially up to MAGBUFFSIZEX - 1
193  j = 0;
194  while (!itooclose && (j < MAGBUFFSIZEX))
195  {
196  // loop over the buffer k from 0 potentially up to MAGBUFFSIZEY - 1
197  k = 0;
198  while (!itooclose && (k < MAGBUFFSIZEY))
199  {
200  // check whether this buffer entry already has a measurement or not
201  if (pthisMagBuffer->index[j][k] != -1)
202  {
203  // calculate the vector difference between current measurement and the buffer entry
204  idelta = 0;
205  for (i = X; i <= Z; i++)
206  {
207  idelta += abs((int32)pthisMag->iBp[i] - (int32)pthisMagBuffer->iBp[i][j][k]);
208  }
209  // check to see if the current reading is close to this existing magnetic buffer entry
210  if (idelta < MESHDELTACOUNTS)
211  {
212  // set the flag to abort the search
213  itooclose = 1;
214  }
215  }
216  else
217  {
218  // store the location of this empty bin for future use
219  l = j;
220  m = k;
221  } // end of test for valid measurement in this bin
222  k++;
223  } // end of k loop
224  j++;
225  } // end of j loop
226 
227  // if none too close, store the measurement in the last empty bin found and return
228  // l and m are guaranteed to be set if no entries too close are detected
229  if (!itooclose)
230  {
231  for (i = X; i <= Z; i++)
232  {
233  pthisMagBuffer->iBp[i][l][m] = pthisMag->iBp[i];
234  }
235  pthisMagBuffer->index[l][m] = loopcounter;
236  (pthisMagBuffer->iMagBufferCount)++;
237  }
238  } // end of test for closeness to current buffer entry
239  return;
240  } // end case 4
241 
242  // this line should be unreachable
243  return;
244 }
245 
246 // function maps the uncalibrated magnetometer data Bp (uT) onto calibrated data Bc (uT)
247 void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
248 {
249  // local variables
250  float ftmp[3]; // temporary array
251  int8 i; // loop counter
252 
253  // calculate fBc and iBc
254  // remove the computed hard iron offsets (uT): ftmp[]=fBp[]-V[]
255  for (i = X; i <= Z; i++)
256  {
257  ftmp[i] = pthisMag->fBp[i] - pthisMagCal->fV[i];
258  }
259  // remove the computed soft iron offsets (uT and counts): fBc=inv(W)*(fBp[]-V[])
260  for (i = X; i <= Z; i++)
261  {
262  pthisMag->fBc[i] = pthisMagCal->finvW[i][X] * ftmp[X] + pthisMagCal->finvW[i][Y] * ftmp[Y] + pthisMagCal->finvW[i][Z] * ftmp[Z];
263  //pthisMag->iBc[i] = (int16) (pthisMag->fBc[i] * pthisMag->fCountsPeruT); // [JNM 2-9-15] This quantity appears to be unnused.
264  }
265 
266  return;
267 }
268 
269 // function runs the magnetic calibration
270 void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag )
271 {
272  int8 i, j; // loop counters
273  int8 isolver; // magnetic solver used
274 
275  // 4 element calibration case
276  if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS7CAL)
277  {
278  // age the existing fit error to avoid one good calibration locking out future updates
279  if (pthisMagCal->iValidMagCal)
280  {
281  pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL4CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
282  }
283  // call the 4 element matrix inversion calibration
284  isolver = 4;
285  fUpdateCalibration4INV(pthisMagCal, pthisMagBuffer, pthisMag);
286  }
287  // 7 element calibration case
288  else if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS10CAL)
289  {
290  // age the existing fit error to avoid one good calibration locking out future updates
291  if (pthisMagCal->iValidMagCal)
292  {
293  pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL7CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
294  }
295  // call the 7 element eigenpair calibration
296  isolver = 7;
297  fUpdateCalibration7EIG(pthisMagCal, pthisMagBuffer, pthisMag);
298  }
299  // 10 element calibration case
300  else
301  {
302  // age the existing fit error to avoid one good calibration locking out future updates
303  if (pthisMagCal->iValidMagCal)
304  {
305  pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL10CAL * (float) MAG_OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
306  }
307  // call the 10 element eigenpair calibration
308  isolver = 10;
309  fUpdateCalibration10EIG(pthisMagCal, pthisMagBuffer, pthisMag);
310  }
311 
312  // the trial geomagnetic field must be in range (earth is 22uT to 67uT)
313  if ((pthisMagCal->ftrB >= MINBFITUT) && (pthisMagCal->ftrB <= MAXBFITUT))
314  {
315  // always accept the calibration if i) no previous calibration exists ii) the calibration fit is reduced or
316  // an improved solver was used giving a good trial calibration (4% or under)
317  if ((pthisMagCal->iValidMagCal == 0) ||
318  (pthisMagCal->ftrFitErrorpc <= pthisMagCal->fFitErrorpc) ||
319  ((isolver > pthisMagCal->iValidMagCal) && (pthisMagCal->ftrFitErrorpc <= 4.0F)))
320  {
321  // accept the new calibration solution
322  pthisMagCal->iValidMagCal = isolver;
323  pthisMagCal->fFitErrorpc = pthisMagCal->ftrFitErrorpc;
324  pthisMagCal->fB = pthisMagCal->ftrB;
325  pthisMagCal->fFourBsq = 4.0F * pthisMagCal->ftrB * pthisMagCal->ftrB;
326  for (i = X; i <= Z; i++)
327  {
328  pthisMagCal->fV[i] = pthisMagCal->ftrV[i];
329  for (j = X; j <= Z; j++)
330  {
331  pthisMagCal->finvW[i][j] = pthisMagCal->ftrinvW[i][j];
332  }
333  }
334  } // end of test to accept the new calibration
335  } // end of test for geomagenetic field strength in range
336 
337  // reset the calibration in progress flag to allow writing to the magnetic buffer
338  pthisMagCal->iCalInProgress = 0;
339 
340  return;
341 }
342 
343 // 4 element calibration using 4x4 matrix inverse
344 void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
345 {
346  // local variables
347  float fBp2; // fBp[X]^2+fBp[Y]^2+fBp[Z]^2
348  float fSumBp4; // sum of fBp2
349  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
350  float fE; // error function = r^T.r
351  int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
352  int16 iCount; // number of measurements counted
353  int8 i, j, k, l; // loop counters
354 
355  // working arrays for 4x4 matrix inversion
356  float *pfRows[4];
357  int8 iColInd[4];
358  int8 iRowInd[4];
359  int8 iPivot[4];
360 
361  // compute fscaling to reduce multiplications later
362  fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
363 
364  // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
365  f3x3matrixAeqI(pthisMagCal->ftrinvW);
366 
367  // zero fSumBp4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above diagonal elements of fmatA=X^T*X (4x4)
368  fSumBp4 = 0.0F;
369  for (i = 0; i < 4; i++)
370  {
371  pthisMagCal->fvecB[i] = 0.0F;
372  for (j = i; j < 4; j++)
373  {
374  pthisMagCal->fmatA[i][j] = 0.0F;
375  }
376  }
377 
378  // the offsets are guaranteed to be set from the first element but to avoid compiler error
379  iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
380 
381  // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices
382  iCount = 0;
383  for (j = 0; j < MAGBUFFSIZEX; j++)
384  {
385  for (k = 0; k < MAGBUFFSIZEY; k++)
386  {
387  if (pthisMagBuffer->index[j][k] != -1)
388  {
389  // use first valid magnetic buffer entry as estimate (in counts) for offset
390  if (iCount == 0)
391  {
392  for (l = X; l <= Z; l++)
393  {
394  iOffset[l] = pthisMagBuffer->iBp[l][j][k];
395  }
396  }
397 
398  // store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
399  for (l = X; l <= Z; l++)
400  {
401  pthisMagCal->fvecA[l] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
402  pthisMagCal->fvecA[l + 3] = pthisMagCal->fvecA[l] * pthisMagCal->fvecA[l];
403  }
404 
405  // calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
406  fBp2 = pthisMagCal->fvecA[3] + pthisMagCal->fvecA[4] + pthisMagCal->fvecA[5];
407 
408  // accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
409  fSumBp4 += fBp2 * fBp2;
410 
411  // now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
412  for (l = X; l <= Z; l++)
413  {
414  pthisMagCal->fvecB[l] += pthisMagCal->fvecA[l] * fBp2;
415  }
416 
417  //accumulate fvecB[3] = X^T.Y =sum(fBp2)
418  pthisMagCal->fvecB[3] += fBp2;
419 
420  // accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
421  pthisMagCal->fmatA[0][0] += pthisMagCal->fvecA[X + 3];
422  pthisMagCal->fmatA[0][1] += pthisMagCal->fvecA[X] * pthisMagCal->fvecA[Y];
423  pthisMagCal->fmatA[0][2] += pthisMagCal->fvecA[X] * pthisMagCal->fvecA[Z];
424  pthisMagCal->fmatA[0][3] += pthisMagCal->fvecA[X];
425  pthisMagCal->fmatA[1][1] += pthisMagCal->fvecA[Y + 3];
426  pthisMagCal->fmatA[1][2] += pthisMagCal->fvecA[Y] * pthisMagCal->fvecA[Z];
427  pthisMagCal->fmatA[1][3] += pthisMagCal->fvecA[Y];
428  pthisMagCal->fmatA[2][2] += pthisMagCal->fvecA[Z + 3];
429  pthisMagCal->fmatA[2][3] += pthisMagCal->fvecA[Z];
430 
431  // increment the counter for next iteration
432  iCount++;
433  }
434  }
435  }
436 
437  // set the last element of the measurement matrix to the number of buffer elements used
438  pthisMagCal->fmatA[3][3] = (float) iCount;
439 
440  // store the number of measurements accumulated (defensive programming, should never be needed)
441  pthisMagBuffer->iMagBufferCount = iCount;
442 
443  // use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
444  for (i = 0; i < 4; i++)
445  {
446  for (j = i; j < 4; j++)
447  {
448  pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[j][i] = pthisMagCal->fmatA[i][j];
449  }
450  }
451 
452  // calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
453  for (i = 0; i < 4; i++)
454  {
455  pfRows[i] = pthisMagCal->fmatB[i];
456  }
457  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4);
458 
459  // calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
460  for (i = 0; i < 4; i++)
461  {
462  pthisMagCal->fvecA[i] = 0.0F;
463  for (k = 0; k < 4; k++)
464  {
465  pthisMagCal->fvecA[i] += pthisMagCal->fmatB[i][k] * pthisMagCal->fvecB[k];
466  }
467  }
468 
469  // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
470  // = fSumBp4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
471  // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBp4 - 2 * fvecA^T.fvecB
472  fE = 0.0F;
473  for (i = 0; i < 4; i++)
474  {
475  fE += pthisMagCal->fvecA[i] * pthisMagCal->fvecB[i];
476  }
477  fE = fSumBp4 - 2.0F * fE;
478 
479  // set fvecB = (X^T.X).beta = fmatA.fvecA
480  for (i = 0; i < 4; i++)
481  {
482  pthisMagCal->fvecB[i] = 0.0F;
483  for (k = 0; k < 4; k++)
484  {
485  pthisMagCal->fvecB[i] += pthisMagCal->fmatA[i][k] * pthisMagCal->fvecA[k];
486  }
487  }
488 
489  // complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
490  for (i = 0; i < 4; i++)
491  {
492  fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
493  }
494 
495  // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
496  for (l = X; l <= Z; l++)
497  {
498  pthisMagCal->ftrV[l] = 0.5F * pthisMagCal->fvecA[l];
499  }
500 
501  // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
502  pthisMagCal->ftrB = sqrtf(pthisMagCal->fvecA[3] + pthisMagCal->ftrV[X] * pthisMagCal->ftrV[X] +
503  pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Y] + pthisMagCal->ftrV[Z] * pthisMagCal->ftrV[Z]);
504 
505  // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
506  pthisMagCal->ftrFitErrorpc = sqrtf(fE / (float) pthisMagBuffer->iMagBufferCount) * 100.0F /
507  (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
508 
509  // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
510  for (l = X; l <= Z; l++)
511  {
512  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]/256.0*FUTPERCOUNT;
513  }
514 
515  // correct the geomagnetic field strength B to correct scaling (result in uT)
516  pthisMagCal->ftrB *= DEFAULTB;
517 
518  return;
519 }
520 
521 // 7 element calibration using direct eigen-decomposition
522 void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
523 {
524  // local variables
525  float det; // matrix determinant
526  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
527  float ftmp; // scratch variable
528  int16 iOffset[3]; // offset to remove large DC hard iron bias
529  int16 iCount; // number of measurements counted
530  int8 i, j, k, l, m, n; // loop counters
531 
532  // compute fscaling to reduce multiplications later
533  fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
534 
535  // the offsets are guaranteed to be set from the first element but to avoid compiler error
536  iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
537 
538  // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
539  for (m = 0; m < 7; m++)
540  {
541  for (n = m; n < 7; n++)
542  {
543  pthisMagCal->fmatA[m][n] = 0.0F;
544  }
545  }
546 
547  // place from MINEQUATIONS to MAXEQUATIONS entries into product matrix fmatA
548  iCount = 0;
549  for (j = 0; j < MAGBUFFSIZEX; j++)
550  {
551  for (k = 0; k < MAGBUFFSIZEY; k++)
552  {
553  if (pthisMagBuffer->index[j][k] != -1)
554  {
555  // use first valid magnetic buffer entry as offset estimate (bit counts)
556  if (iCount == 0)
557  {
558  for (l = X; l <= Z; l++)
559  {
560  iOffset[l] = pthisMagBuffer->iBp[l][j][k];
561  }
562  }
563 
564  // apply the offset and scaling and store in fvecA
565  for (l = X; l <= Z; l++)
566  {
567  pthisMagCal->fvecA[l + 3] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
568  pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3];
569  }
570 
571  // accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA}
572  // with the exception of fmatA[6][6] which will sum to the number of measurements
573  // and remembering that fvecA[6] equals 1.0F
574  // update the right hand column [6] of fmatA except for fmatA[6][6]
575  for (m = 0; m < 6; m++)
576  {
577  pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m];
578  }
579  // update the on and above diagonal terms except for right hand column 6
580  for (m = 0; m < 6; m++)
581  {
582  for (n = m; n < 6; n++)
583  {
584  pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
585  }
586  }
587 
588  // increment the measurement counter for the next iteration
589  iCount++;
590  }
591  }
592  }
593 
594  // finally set the last element fmatA[6][6] to the number of measurements
595  pthisMagCal->fmatA[6][6] = (float) iCount;
596 
597  // store the number of measurements accumulated (defensive programming, should never be needed)
598  pthisMagBuffer->iMagBufferCount = iCount;
599 
600  // copy the above diagonal elements of fmatA to below the diagonal
601  for (m = 1; m < 7; m++)
602  {
603  for (n = 0; n < m; n++)
604  {
605  pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
606  }
607  }
608 
609  // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
610  eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 7);
611 
612  // find the smallest eigenvalue
613  j = 0;
614  for (i = 1; i < 7; i++)
615  {
616  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
617  {
618  j = i;
619  }
620  }
621 
622  // set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
623  // and the hard iron offset (scaled and offset)
624  f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F);
625  det = 1.0F;
626  for (l = X; l <= Z; l++)
627  {
628  pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j];
629  det *= pthisMagCal->fA[l][l];
630  pthisMagCal->ftrV[l] = -0.5F * pthisMagCal->fmatB[l + 3][j] / pthisMagCal->fA[l][l];
631  }
632 
633  // negate A if it has negative determinant
634  if (det < 0.0F)
635  {
636  f3x3matrixAeqMinusA(pthisMagCal->fA);
637  pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
638  det = -det;
639  }
640 
641  // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
642  ftmp = -pthisMagCal->fmatB[6][j];
643  for (l = X; l <= Z; l++)
644  {
645  ftmp += pthisMagCal->fA[l][l] * pthisMagCal->ftrV[l] * pthisMagCal->ftrV[l];
646  }
647 
648  // calculate the trial normalized fit error as a percentage
649  pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / fabs(ftmp);
650 
651  // normalize the ellipsoid matrix A to unit determinant
652  f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
653 
654  // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
655  pthisMagCal->ftrB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
656 
657  // compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
658  f3x3matrixAeqI(pthisMagCal->ftrinvW);
659  for (l = X; l <= Z; l++)
660  {
661  pthisMagCal->ftrinvW[l][l] = sqrtf(fabs(pthisMagCal->fA[l][l]));
662  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]*FUTPERCOUNT;
663  }
664 
665  return;
666 }
667 
668 // 10 element calibration using direct eigen-decomposition
669 void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
670 {
671  // local variables
672  float det; // matrix determinant
673  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
674  float ftmp; // scratch variable
675  int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
676  int16 iCount; // number of measurements counted
677  int8 i, j, k, l, m, n; // loop counters
678 
679  // compute fscaling to reduce multiplications later
680  fscaling = FUTPERCOUNT * INV_DEFAULTB; // was pthisMag->fuTPerCount / DEFAULTB;
681 
682  // the offsets are guaranteed to be set from the first element but to avoid compiler error
683  iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
684 
685  // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
686  for (m = 0; m < 10; m++)
687  {
688  for (n = m; n < 10; n++)
689  {
690  pthisMagCal->fmatA[m][n] = 0.0F;
691  }
692  }
693 
694  // sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix fmatA
695  iCount = 0;
696  for (j = 0; j < MAGBUFFSIZEX; j++)
697  {
698  for (k = 0; k < MAGBUFFSIZEY; k++)
699  {
700  if (pthisMagBuffer->index[j][k] != -1)
701  {
702  // use first valid magnetic buffer entry as estimate for offset to help solution (bit counts)
703  if (iCount == 0)
704  {
705  for (l = X; l <= Z; l++)
706  {
707  iOffset[l] = pthisMagBuffer->iBp[l][j][k];
708  }
709  }
710 
711  // apply the fixed offset and scaling and enter into fvecA[6-8]
712  for (l = X; l <= Z; l++)
713  {
714  pthisMagCal->fvecA[l + 6] = (float)((int32)pthisMagBuffer->iBp[l][j][k] - (int32)iOffset[l]) * fscaling;
715  }
716 
717  // compute measurement vector elements fvecA[0-5] from fvecA[6-8]
718  pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
719  pthisMagCal->fvecA[1] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[7];
720  pthisMagCal->fvecA[2] = 2.0F * pthisMagCal->fvecA[6] * pthisMagCal->fvecA[8];
721  pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
722  pthisMagCal->fvecA[4] = 2.0F * pthisMagCal->fvecA[7] * pthisMagCal->fvecA[8];
723  pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
724 
725  // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
726  // with the exception of fmatA[9][9] which equals the number of measurements
727  // update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
728  for (m = 0; m < 9; m++)
729  {
730  pthisMagCal->fmatA[m][9] += pthisMagCal->fvecA[m];
731  }
732  // update the on and above diagonal terms of fmatA ignoring right hand column 9
733  for (m = 0; m < 9; m++)
734  {
735  for (n = m; n < 9; n++)
736  {
737  pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
738  }
739  }
740 
741  // increment the measurement counter for the next iteration
742  iCount++;
743  }
744  }
745  }
746 
747  // set the last element fmatA[9][9] to the number of measurements
748  pthisMagCal->fmatA[9][9] = (float) iCount;
749 
750  // store the number of measurements accumulated (defensive programming, should never be needed)
751  pthisMagBuffer->iMagBufferCount = iCount;
752 
753  // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
754  for (m = 1; m < 10; m++)
755  {
756  for (n = 0; n < m; n++)
757  {
758  pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
759  }
760  }
761 
762  // set pthisMagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
763  eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 10);
764 
765  // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
766  j = 0;
767  for (i = 1; i < 10; i++)
768  {
769  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
770  {
771  j = i;
772  }
773  }
774  pthisMagCal->fA[0][0] = pthisMagCal->fmatB[0][j];
775  pthisMagCal->fA[0][1] = pthisMagCal->fA[1][0] = pthisMagCal->fmatB[1][j];
776  pthisMagCal->fA[0][2] = pthisMagCal->fA[2][0] = pthisMagCal->fmatB[2][j];
777  pthisMagCal->fA[1][1] = pthisMagCal->fmatB[3][j];
778  pthisMagCal->fA[1][2] = pthisMagCal->fA[2][1] = pthisMagCal->fmatB[4][j];
779  pthisMagCal->fA[2][2] = pthisMagCal->fmatB[5][j];
780 
781  // negate entire solution if A has negative determinant
782  det = f3x3matrixDetA(pthisMagCal->fA);
783  if (det < 0.0F)
784  {
785  f3x3matrixAeqMinusA(pthisMagCal->fA);
786  pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
787  pthisMagCal->fmatB[7][j] = -pthisMagCal->fmatB[7][j];
788  pthisMagCal->fmatB[8][j] = -pthisMagCal->fmatB[8][j];
789  pthisMagCal->fmatB[9][j] = -pthisMagCal->fmatB[9][j];
790  det = -det;
791  }
792 
793  // compute the inverse of the ellipsoid matrix
794  f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
795 
796  // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
797  for (l = X; l <= Z; l++)
798  {
799  pthisMagCal->ftrV[l] = 0.0F;
800  for (m = X; m <= Z; m++)
801  {
802  pthisMagCal->ftrV[l] += pthisMagCal->finvA[l][m] * pthisMagCal->fmatB[m + 6][j];
803  }
804  pthisMagCal->ftrV[l] *= -0.5F;
805  }
806 
807  // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
808  pthisMagCal->ftrB = sqrtf(fabs(pthisMagCal->fA[0][0] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[X] +
809  2.0F * pthisMagCal->fA[0][1] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Y] +
810  2.0F * pthisMagCal->fA[0][2] * pthisMagCal->ftrV[X] * pthisMagCal->ftrV[Z] +
811  pthisMagCal->fA[1][1] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Y] +
812  2.0F * pthisMagCal->fA[1][2] * pthisMagCal->ftrV[Y] * pthisMagCal->ftrV[Z] +
813  pthisMagCal->fA[2][2] * pthisMagCal->ftrV[Z] * pthisMagCal->ftrV[Z] - pthisMagCal->fmatB[9][j]));
814 
815  // calculate the trial normalized fit error as a percentage
816  pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) /
817  (pthisMagCal->ftrB * pthisMagCal->ftrB);
818 
819  // correct for the measurement matrix offset and scaling and get the computed hard iron offset in uT
820  for (l = X; l <= Z; l++)
821  {
822  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l]*FUTPERCOUNT;
823  }
824 
825  // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A
826  pthisMagCal->ftrB *= DEFAULTB;
827 
828  // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor
829  f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
830  pthisMagCal->ftrB *= powf(det, -(ONESIXTH));
831 
832  // compute trial invW from the square root of fA (both with normalized determinant)
833  // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
834  // where fmatA holds the 3x3 matrix fA in its top left elements
835  for (i = 0; i < 3; i++)
836  {
837  for (j = 0; j < 3; j++)
838  {
839  pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
840  }
841  }
842  eigencompute(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 3);
843 
844  // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
845  for (j = 0; j < 3; j++) // loop over columns j
846  {
847  ftmp = sqrtf(sqrtf(fabs(pthisMagCal->fvecA[j])));
848  for (i = 0; i < 3; i++) // loop over rows i
849  {
850  pthisMagCal->fmatB[i][j] *= ftmp;
851  }
852  }
853 
854  // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
855  // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
856  // loop over rows
857  for (i = 0; i < 3; i++)
858  {
859  // loop over on and above diagonal columns
860  for (j = i; j < 3; j++)
861  {
862  pthisMagCal->ftrinvW[i][j] = 0.0F;
863  // accumulate the matrix product
864  for (k = 0; k < 3; k++)
865  {
866  pthisMagCal->ftrinvW[i][j] += pthisMagCal->fmatB[i][k] * pthisMagCal->fmatB[j][k];
867  }
868  // copy to below diagonal element
869  pthisMagCal->ftrinvW[j][i] = pthisMagCal->ftrinvW[i][j];
870  }
871  }
872 
873  return;
874 }
875 
876 
int16 iBp[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
Definition: magnetic.h:35
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:270
float fvecA[10]
Definition: magnetic.h:57
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:182
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:344
float fBp[3]
float fV[3]
Definition: magnetic.h:44
int16 tanarray[MAGBUFFSIZEX-1]
Definition: magnetic.h:37
#define SENSORFS
Definition: fusion_config.h:21
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:522
#define ONETHIRD
Definition: magnetic.c:38
float ftrinvW[3][3]
Definition: magnetic.h:50
#define MAXMEASUREMENTS
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:113
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:94
#define INTERVAL7CAL
float fmatA[10][10]
Definition: magnetic.h:55
#define INV_DEFAULTB
#define INTERVAL4CAL
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:169
Definition: matrix.h:33
signed char int8
Definition: basic_types.h:12
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:42
int8 iValidMagCal
Definition: magnetic.h:61
float fA[3][3]
Definition: magnetic.h:53
int32 index[MAGBUFFSIZEX][MAGBUFFSIZEY]
Definition: magnetic.h:36
#define MESHDELTACOUNTS
#define FITERRORAGINGSECS
int8 iCalInProgress
Definition: magnetic.h:59
float ftrV[3]
Definition: magnetic.h:49
#define MAG_OVERSAMPLE_RATIO
Definition: fusion_config.h:23
float ftrFitErrorpc
Definition: magnetic.h:52
#define MAGBUFFSIZEX
#define MAGBUFFSIZEY
float fFitErrorpc
Definition: magnetic.h:48
int8 iMagCalHasRun
Definition: magnetic.h:60
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:79
#define PI
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:134
float fvecB[4]
Definition: magnetic.h:58
float finvA[3][3]
Definition: magnetic.h:54
float fFourBsq
Definition: magnetic.h:47
#define FUTPERCOUNT
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:669
long int32
This defines int32 as long.
Definition: isf_types.h:32
Definition: matrix.h:34
short int16
This defines int16 as short.
Definition: isf_types.h:23
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
float fBc[3]
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:247
#define INTERVAL10CAL
Definition: matrix.h:35
float fmatB[10][10]
Definition: magnetic.h:56
float ftrB
Definition: magnetic.h:51
#define ONESIXTH
Definition: magnetic.c:39
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:76
#define MINBFITUT
#define DEFAULTB
int16 iMagBufferCount
Definition: magnetic.h:38
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325
#define MAXBFITUT
#define MINMEASUREMENTS10CAL
float finvW[3][3]
Definition: magnetic.h:45
#define MINMEASUREMENTS7CAL
int16 iBp[3]