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