1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /*! \file magnetic.c
10 \brief Lower level magnetic calibration interface
11
12 Many developers can utilize the NXP Sensor Fusion Library without ever
13 making any adjustment to the lower level magnetic calibration functions
14 defined in this file.
15 */
16
17 #include "sensor_fusion.h"
18 #include "math.h"
19 #include "stdlib.h"
20 #include "time.h"
21
22 #if F_USING_MAG
23 // function resets the magnetometer buffer and magnetic calibration
fInitializeMagCalibration(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer)24 void fInitializeMagCalibration(struct MagCalibration *pthisMagCal,
25 struct MagBuffer *pthisMagBuffer)
26 {
27 float *pFlash; // pointer into flash memory
28 int8 i,
29 j; // loop counters
30
31 // set magnetic buffer index to invalid value -1 to denote no measurement present
32 pthisMagBuffer->iMagBufferCount = 0;
33 for (i = 0; i < MAGBUFFSIZEX; i++)
34 for (j = 0; j < MAGBUFFSIZEY; j++) pthisMagBuffer->index[i][j] = -1;
35
36 // initialize the array of (MAGBUFFSIZEX - 1) elements of 100 * tangents used for buffer indexing
37 // entries cover the range 100 * tan(-PI/2 + PI/MAGBUFFSIZEX), 100 * tan(-PI/2 + 2*PI/MAGBUFFSIZEX) to
38 // 100 * tan(-PI/2 + (MAGBUFFSIZEX - 1) * PI/MAGBUFFSIZEX).
39 // for MAGBUFFSIZEX=12, the entries range in value from -373 to +373
40 for (i = 0; i < (MAGBUFFSIZEX - 1); i++)
41 pthisMagBuffer->tanarray[i] = (int16) (100.0F * tanf(PI * (-0.5F + (float) (i + 1) / MAGBUFFSIZEX)));
42
43 // check to see if the stored magnetic calibration has been erased
44 // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
45 #ifndef SIMULATION
46
47 pFlash = (float *) (CALIBRATION_NVM_ADDR + MAG_NVM_OFFSET);
48 if (*((uint32 *) pFlash++) == 0x12345678)
49 {
50 // a magnetic calibration is present in flash
51 // copy magnetic calibration elements (15x float + 1x int32 total 64 bytes) from flash to RAM
52 for (i = CHX; i <= CHZ; i++) pthisMagCal->fV[i] = *(pFlash++);
53 for (i = CHX; i <= CHZ; i++)
54 for (j = CHX; j <= CHZ; j++)
55 pthisMagCal->finvW[i][j] = *(pFlash++);
56 pthisMagCal->fB = *(pFlash++);
57 pthisMagCal->fBSq = *(pFlash++);
58 pthisMagCal->fFitErrorpc = *(pFlash++);
59 pthisMagCal->iValidMagCal = *((int32 *) pFlash);
60 }
61 else
62 {
63 #endif
64 // flash has been erased and no magnetic calibration is present
65 // initialize the magnetic calibration in RAM to null default
66 pthisMagCal->fV[CHX] = pthisMagCal->fV[CHY] = pthisMagCal->fV[CHZ] = 0.0F;
67 f3x3matrixAeqI(pthisMagCal->finvW);
68 pthisMagCal->fB = DEFAULTB;
69 pthisMagCal->fBSq = DEFAULTB * DEFAULTB;
70 pthisMagCal->fFitErrorpc = 100.0F;
71 pthisMagCal->iValidMagCal = 0;
72 #ifndef SIMULATION
73 }
74 #endif
75
76 // initialize remaining elements of the magnetic calibration structure
77 pthisMagCal->iCalInProgress = 0;
78 pthisMagCal->iInitiateMagCal = 0;
79 pthisMagCal->iNewCalibrationAvailable = 0;
80 pthisMagCal->iMagBufferReadOnly = false;
81 pthisMagCal->i4ElementSolverTried = false;
82 pthisMagCal->i7ElementSolverTried = false;
83 pthisMagCal->i10ElementSolverTried = false;
84
85 return;
86 }
87
88 // function updates the magnetic measurement buffer with most recent magnetic data (typically 200Hz)
89
90 // the uncalibrated measurements iBs are stored in the buffer but the calibrated measurements iBc are used for indexing.
iUpdateMagBuffer(struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag,int32 loopcounter)91 void iUpdateMagBuffer(struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag,
92 int32 loopcounter)
93 {
94 // local variables
95 int32 idelta; // absolute vector distance
96 int32 i; // counter
97 int16 itanj,
98 itank; // indexing accelerometer ratios
99 int8 j,
100 k,
101 l,
102 m; // counters
103 int8 itooclose; // flag denoting measurement is too close to existing ones
104
105 // calculate the magnetometer buffer bins from the tangent ratios
106 if (pthisMag->iBc[CHZ] == 0) return;
107 itanj = (100 * (int32) pthisMag->iBc[CHX]) / ((int32) pthisMag->iBc[CHZ]);
108 itank = (100 * (int32) pthisMag->iBc[CHY]) / ((int32) pthisMag->iBc[CHZ]);
109
110 // map tangent ratios to bins j and k using equal angle bins: C guarantees left to right execution of the test
111 // and add an offset of MAGBUFFSIZEX bins to k to mimic atan2 on this ratio
112 // j will vary from 0 to MAGBUFFSIZEX - 1 and k from 0 to 2 * MAGBUFFSIZEX - 1
113 j = k = 0;
114 while ((j < (MAGBUFFSIZEX - 1) && (itanj >= pthisMagBuffer->tanarray[j])))
115 j++;
116 while ((k < (MAGBUFFSIZEX - 1) && (itank >= pthisMagBuffer->tanarray[k])))
117 k++;
118 if (pthisMag->iBc[CHX] < 0) k += MAGBUFFSIZEX;
119
120 // case 1: buffer is full and this bin has a measurement: over-write without increasing number of measurements
121 // this is the most common option at run time
122 if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) &&
123 (pthisMagBuffer->index[j][k] != -1))
124 {
125 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
126 for (i = CHX; i <= CHZ; i++)
127 {
128 pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
129 }
130
131 pthisMagBuffer->index[j][k] = loopcounter;
132 return;
133 } // end case 1
134
135 // case 2: the buffer is full and this bin does not have a measurement: store and retire the oldest
136 // this is the second most common option at run time
137 if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) &&
138 (pthisMagBuffer->index[j][k] == -1))
139 {
140 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
141 for (i = CHX; i <= CHZ; i++)
142 {
143 pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
144 }
145
146 pthisMagBuffer->index[j][k] = loopcounter;
147
148 // set l and m to the oldest active entry and disable it
149 i = loopcounter;
150 l = m = 0; // to avoid compiler complaint
151 for (j = 0; j < MAGBUFFSIZEX; j++)
152 {
153 for (k = 0; k < MAGBUFFSIZEY; k++)
154 {
155 // check if the time stamp is older than the oldest found so far (normally fails this test)
156 if (pthisMagBuffer->index[j][k] < i)
157 {
158 // check if this bin is active (normally passes this test)
159 if (pthisMagBuffer->index[j][k] != -1)
160 {
161 // set l and m to the indices of the oldest entry found so far
162 l = j;
163 m = k;
164
165 // set i to the time stamp of the oldest entry found so far
166 i = pthisMagBuffer->index[l][m];
167 } // end of test for active
168 } // end of test for older
169 } // end of loop over k
170 } // end of loop over j
171
172 // deactivate the oldest measurement (no need to zero the measurement data)
173 pthisMagBuffer->index[l][m] = -1;
174 return;
175 } // end case 2
176
177 // case 3: buffer is not full and this bin is empty: store and increment number of measurements
178 if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) &&
179 (pthisMagBuffer->index[j][k] == -1))
180 {
181 // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
182 for (i = CHX; i <= CHZ; i++)
183 {
184 pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
185 }
186
187 pthisMagBuffer->index[j][k] = loopcounter;
188 (pthisMagBuffer->iMagBufferCount)++;
189 return;
190 } // end case 3
191
192 // case 4: buffer is not full and this bin has a measurement: over-write if close or try to slot in
193 // elsewhere if not close to the other measurements so as to create a mesh at power up
194 if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) &&
195 (pthisMagBuffer->index[j][k] != -1))
196 {
197 // calculate the vector difference between current measurement and the buffer entry
198 idelta = 0;
199 for (i = CHX; i <= CHZ; i++)
200 {
201 idelta += abs((int32) pthisMag->iBs[i] -
202 (int32) pthisMagBuffer->iBs[i][j][k]);
203 }
204
205 // check to see if the current reading is close to this existing magnetic buffer entry
206 if (idelta < MESHDELTACOUNTS)
207 {
208 // simply over-write the measurement and return
209 for (i = CHX; i <= CHZ; i++)
210 {
211 pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
212 }
213
214 pthisMagBuffer->index[j][k] = loopcounter;
215 }
216 else
217 {
218 // reset the flag denoting that the current measurement is close to any measurement in the buffer
219 itooclose = 0;
220
221 // to avoid compiler warning
222 l = m = 0;
223
224 // loop over the buffer j from 0 potentially up to MAGBUFFSIZEX - 1
225 j = 0;
226 while (!itooclose && (j < MAGBUFFSIZEX))
227 {
228 // loop over the buffer k from 0 potentially up to MAGBUFFSIZEY - 1
229 k = 0;
230 while (!itooclose && (k < MAGBUFFSIZEY))
231 {
232 // check whether this buffer entry already has a measurement or not
233 if (pthisMagBuffer->index[j][k] != -1)
234 {
235 // calculate the vector difference between current measurement and the buffer entry
236 idelta = 0;
237 for (i = CHX; i <= CHZ; i++)
238 {
239 idelta += abs((int32) pthisMag->iBs[i] -
240 (int32) pthisMagBuffer->iBs[i][j][k]);
241 }
242
243 // check to see if the current reading is close to this existing magnetic buffer entry
244 if (idelta < MESHDELTACOUNTS)
245 {
246 // set the flag to abort the search
247 itooclose = 1;
248 }
249 }
250 else
251 {
252 // store the location of this empty bin for future use
253 l = j;
254 m = k;
255 } // end of test for valid measurement in this bin
256
257 k++;
258 } // end of k loop
259
260 j++;
261 } // end of j loop
262
263 // if none too close, store the measurement in the last empty bin found and return
264 // l and m are guaranteed to be set if no entries too close are detected
265 if (!itooclose)
266 {
267 for (i = CHX; i <= CHZ; i++)
268 {
269 pthisMagBuffer->iBs[i][l][m] = pthisMag->iBs[i];
270 }
271
272 pthisMagBuffer->index[l][m] = loopcounter;
273 (pthisMagBuffer->iMagBufferCount)++;
274 }
275 } // end of test for closeness to current buffer entry
276
277 return;
278 } // end case 4
279
280 // this line should be unreachable
281 return;
282 }
283
284 // function maps the uncalibrated magnetometer data fBs (uT) onto calibrated averaged data fBc (uT), iBc (counts)
fInvertMagCal(struct MagSensor * pthisMag,struct MagCalibration * pthisMagCal)285 void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
286 {
287 // local variables
288 float ftmp[3]; // temporary array
289 int8 i; // loop counter
290
291 // remove the computed hard iron offsets (uT): ftmp[]=fBs[]-V[]
292 for (i = CHX; i <= CHZ; i++)
293 {
294 ftmp[i] = pthisMag->fBs[i] - pthisMagCal->fV[i];
295 }
296
297 // remove the computed soft iron offsets (uT and counts): fBc=inv(W)*(fBs[]-V[])
298 for (i = CHX; i <= CHZ; i++)
299 {
300 pthisMag->fBc[i] = pthisMagCal->finvW[i][CHX] *
301 ftmp[CHX] +
302 pthisMagCal->finvW[i][CHY] *
303 ftmp[CHY] +
304 pthisMagCal->finvW[i][CHZ] *
305 ftmp[CHZ];
306 pthisMag->iBc[i] = (int16) (pthisMag->fBc[i] * pthisMag->fCountsPeruT);
307 }
308
309 return;
310 }
311
312 // function runs the magnetic calibration
fRunMagCalibration(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag,int32 loopcounter)313 void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagBuffer *pthisMagBuffer,
314 struct MagSensor *pthisMag, int32 loopcounter)
315 {
316 int8 i,
317 j; // loop counters
318
319 // determine whether to initiate a new magnetic calibration
320 if (!pthisMagCal->iCalInProgress)
321 {
322 // clear the flag
323 pthisMagCal->iInitiateMagCal = 0;
324
325 // try one calibration attempt with the best model available given the number of measurements
326 if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL) &&
327 (!pthisMagCal->i10ElementSolverTried))
328 {
329 pthisMagCal->i10ElementSolverTried = true;
330 pthisMagCal->iInitiateMagCal = 10;
331 }
332 else if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL) &&
333 (!pthisMagCal->i7ElementSolverTried))
334 {
335 pthisMagCal->i7ElementSolverTried = true;
336 pthisMagCal->iInitiateMagCal = 7;
337 }
338 else if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL) &&
339 (!pthisMagCal->i4ElementSolverTried))
340 {
341 pthisMagCal->i4ElementSolverTried = true;
342 pthisMagCal->iInitiateMagCal = 4;
343 }
344
345 // otherwise start a calibration at regular interval defined by CAL_INTERVAL_SECS
346 else if (!pthisMagCal->iInitiateMagCal &&
347 !(loopcounter % (CAL_INTERVAL_SECS * FUSION_HZ)))
348 {
349 if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL)
350 {
351 pthisMagCal->i10ElementSolverTried = true;
352 pthisMagCal->iInitiateMagCal = 10;
353 }
354 else if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL)
355 {
356 pthisMagCal->i7ElementSolverTried = true;
357 pthisMagCal->iInitiateMagCal = 7;
358 }
359 else if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL)
360 {
361 pthisMagCal->i4ElementSolverTried = true;
362 pthisMagCal->iInitiateMagCal = 4;
363 }
364 }
365
366 // store the selected calibration model (if any) to be run
367 pthisMagCal->iCalInProgress = pthisMagCal->iInitiateMagCal;
368 }
369
370 // on entry each of the calibration functions resets iInitiateMagCal and on completion sets
371 // iCalInProgress=0 and iNewCalibrationAvailable=4,7,10 according to the solver used
372 switch (pthisMagCal->iCalInProgress)
373 {
374 case 0:
375 break;
376
377 case 4:
378 fUpdateMagCalibration4Slice(pthisMagCal, pthisMagBuffer, pthisMag);
379 break;
380
381 case 7:
382 fUpdateMagCalibration7Slice(pthisMagCal, pthisMagBuffer, pthisMag);
383 break;
384
385 case 10:
386 fUpdateMagCalibration10Slice(pthisMagCal, pthisMagBuffer, pthisMag);
387 break;
388
389 default:
390 break;
391 }
392
393 // evaluate the new calibration to determine whether to accept it
394 if (pthisMagCal->iNewCalibrationAvailable)
395 {
396 // the geomagnetic field strength must be in range (earth is 22uT to 67uT) with reasonable fit error
397 if ((pthisMagCal->ftrB >= MINBFITUT) && (pthisMagCal->ftrB <= MAXBFITUT) &&
398 (pthisMagCal->ftrFitErrorpc <= 15.0F))
399 {
400 // the fit error must be improved or be from a more sophisticated solver but still 5 bars (under 3.5% fit error)
401 if ((pthisMagCal->ftrFitErrorpc <= pthisMagCal->fFitErrorpc) || ((
402 pthisMagCal->iNewCalibrationAvailable > pthisMagCal->
403 iValidMagCal) && (pthisMagCal->ftrFitErrorpc <= 3.5F)))
404 {
405 // accept the new calibration
406 pthisMagCal->iValidMagCal = pthisMagCal->iNewCalibrationAvailable;
407 pthisMagCal->fFitErrorpc = pthisMagCal->ftrFitErrorpc;
408 pthisMagCal->fB = pthisMagCal->ftrB;
409 pthisMagCal->fBSq = pthisMagCal->fB * pthisMagCal->fB;
410 for (i = CHX; i <= CHZ; i++)
411 {
412 pthisMagCal->fV[i] = pthisMagCal->ftrV[i];
413 for (j = CHX; j <= CHZ; j++)
414 pthisMagCal->finvW[i][j] = pthisMagCal->ftrinvW[i][j];
415 }
416 }
417 }
418 else if(pthisMagCal->i10ElementSolverTried)
419 {
420 // the magnetic buffer is presumed corrupted so clear out all measurements and restart calibration attempts
421 pthisMagBuffer->iMagBufferCount = 0;
422 for (i = 0; i < MAGBUFFSIZEX; i++)
423 for (j = 0; j < MAGBUFFSIZEY; j++)
424 pthisMagBuffer->index[i][j] = -1;
425 pthisMagCal->i4ElementSolverTried = false;
426 pthisMagCal->i7ElementSolverTried = false;
427 pthisMagCal->i10ElementSolverTried = false;
428 } // end of test for new calibration within field strength and fit error limits
429
430 // reset the new calibration flag
431 pthisMagCal->iNewCalibrationAvailable = 0;
432 } // end of test for new calibration available
433
434 // age the existing fit error very slowly to avoid one good calibration locking out future updates.
435 // this prevents a calibration remaining for ever if a unit is never powered down
436 if (pthisMagCal->iValidMagCal)
437 pthisMagCal->fFitErrorpc += 1.0F / ((float) FUSION_HZ * FITERRORAGINGSECS);
438
439 return;
440 }
441
442 // 4 element calibration using 4x4 matrix inverse
fUpdateMagCalibration4Slice(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)443 void fUpdateMagCalibration4Slice(struct MagCalibration *pthisMagCal,
444 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
445 {
446 // local variables
447 int8 i,
448 j,
449 k; // loop counters
450
451 // working arrays for 4x4 matrix inversion
452 float *pfRows[4];
453 int8 iColInd[4];
454 int8 iRowInd[4];
455 int8 iPivot[4];
456
457 // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
458 if (pthisMagCal->iInitiateMagCal)
459 {
460 pthisMagCal->itimeslice = 0;
461 pthisMagCal->iInitiateMagCal = false;
462 pthisMagCal->iMagBufferReadOnly = true;
463 }
464
465 // time slice 0: 18.8K ticks = 0.39ms for 300 measurements on KL25Z
466 // initialize matrices and compute average of measurements in magnetic buffer
467 if (pthisMagCal->itimeslice == 0)
468 {
469 int16 iM; // number of measurements in the buffer
470
471 // zero on and above diagonal matrix X^T.X (in fmatA), vector X^T.Y (in fvecA), scalar Y^T.Y (in fYTY)
472 pthisMagCal->fYTY = 0.0F;
473 for (i = 0; i < 4; i++)
474 {
475 pthisMagCal->fvecA[i] = 0.0F;
476 for (j = i; j < 4; j++) pthisMagCal->fmatA[i][j] = 0.0F;
477 }
478
479 // zero total number of measurements and measurement sums
480 iM = 0;
481 for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
482
483 // compute the sum of measurements in the magnetic buffer
484 for (i = 0; i < MAGBUFFSIZEX; i++)
485 {
486 for (j = 0; j < MAGBUFFSIZEY; j++)
487 {
488 if (pthisMagBuffer->index[i][j] != -1)
489 {
490 iM++;
491 for (k = 0; k < 3; k++)
492 pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
493 }
494 }
495 }
496
497 // compute the magnetic buffer measurement averages with rounding
498 for (i = 0; i < 3; i++)
499 {
500 if (pthisMagCal->iSumBs[i] >= 0)
501 pthisMagCal->iMeanBs[i] =
502 (
503 pthisMagCal->iSumBs[i] +
504 ((int32) iM >> 1)
505 ) /
506 (int32) iM;
507 else
508 pthisMagCal->iMeanBs[i] =
509 (
510 pthisMagCal->iSumBs[i] -
511 ((int32) iM >> 1)
512 ) /
513 (int32) iM;
514 }
515
516 // for defensive programming, re-store the number of active measurements in the buffer
517 pthisMagBuffer->iMagBufferCount = iM;
518
519 // increment the time slice
520 (pthisMagCal->itimeslice)++;
521 } // end of time slice 0
522
523 // time slices 1 to MAGBUFFSIZEX: each up to 81K ticks = 1.7ms
524 // accumulate the matrices fmatA=X^T.X and fvecA=X^T.Y and Y^T.Y from the magnetic buffer
525 else if ((pthisMagCal->itimeslice >= 1) &&
526 (pthisMagCal->itimeslice <= MAGBUFFSIZEX))
527 {
528 float fBsZeroMeanSq; // squared magnetic measurement (counts^2)
529 int32 iBsZeroMean[3]; // zero mean magnetic buffer measurement (counts)
530
531 // accumulate the measurement matrix elements XTX (in fmatA), XTY (in fvecA) and YTY on the zero mean measurements
532 i = pthisMagCal->itimeslice - 1;
533 for (j = 0; j < MAGBUFFSIZEY; j++)
534 {
535 if (pthisMagBuffer->index[i][j] != -1)
536 {
537 // compute zero mean measurements
538 for (k = 0; k < 3; k++)
539 iBsZeroMean[k] = (int32) pthisMagBuffer->iBs[k][i][j] - (int32) pthisMagCal->iMeanBs[k];
540
541 // accumulate the non-zero elements of zero mean XTX (in fmatA)
542 pthisMagCal->fmatA[0][0] += (float) (iBsZeroMean[0] * iBsZeroMean[0]);
543 pthisMagCal->fmatA[0][1] += (float) (iBsZeroMean[0] * iBsZeroMean[1]);
544 pthisMagCal->fmatA[0][2] += (float) (iBsZeroMean[0] * iBsZeroMean[2]);
545 pthisMagCal->fmatA[1][1] += (float) (iBsZeroMean[1] * iBsZeroMean[1]);
546 pthisMagCal->fmatA[1][2] += (float) (iBsZeroMean[1] * iBsZeroMean[2]);
547 pthisMagCal->fmatA[2][2] += (float) (iBsZeroMean[2] * iBsZeroMean[2]);
548
549 // accumulate XTY (in fvecA)
550 fBsZeroMeanSq = (float)
551 (
552 iBsZeroMean[CHX] *
553 iBsZeroMean[CHX] +
554 iBsZeroMean[CHY] *
555 iBsZeroMean[CHY] +
556 iBsZeroMean[CHZ] *
557 iBsZeroMean[CHZ]
558 );
559 for (k = 0; k < 3; k++)
560 pthisMagCal->fvecA[k] += (float) iBsZeroMean[k] * fBsZeroMeanSq;
561 pthisMagCal->fvecA[3] += fBsZeroMeanSq;
562
563 // accumulate fYTY
564 pthisMagCal->fYTY += fBsZeroMeanSq * fBsZeroMeanSq;
565 }
566 }
567
568 // increment the time slice
569 (pthisMagCal->itimeslice)++;
570 } // end of time slices 1 to MAGBUFFSIZEX
571
572 // time slice MAGBUFFSIZEX+1: 18.3K ticks = 0.38ms on KL25Z (constant) (stored in systick[2])
573 // re-enable magnetic buffer for writing and invert fmatB = fmatA = X^T.X in situ
574 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX + 1))
575 {
576 int8 ierror; // matrix inversion error flag
577
578 // set fmatA[3][3] = X^T.X[3][3] to number of measurements found
579 pthisMagCal->fmatA[3][3] = (float) pthisMagBuffer->iMagBufferCount;
580
581 // enable the magnetic buffer for writing now that the matrices have been computed
582 pthisMagCal->iMagBufferReadOnly = false;
583
584 // set fmatA and fmatB to above diagonal elements of fmatA
585 for (i = 0; i < 4; i++)
586 {
587 for (j = 0; j <= i; j++)
588 pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
589 }
590
591 // set fmatB = inv(fmatB) = inv(X^T.X)
592 for (i = 0; i < 4; i++) pfRows[i] = pthisMagCal->fmatB[i];
593 fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
594
595 // increment the time slice
596 (pthisMagCal->itimeslice)++;
597 } // // end of time slice MAGBUFFSIZEX+1
598
599 // time slice MAGBUFFSIZEX+2: 17.2K ticks = 0.36ms on KL25Z (constant) (stored in systick[3])
600 // compute the solution vector and the calibration coefficients
601 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX + 2))
602 {
603 float fE; // error function = r^T.r
604 float ftmp; // scratch
605
606 // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
607 f3x3matrixAeqI(pthisMagCal->ftrinvW);
608
609 // calculate solution vector fvecB = beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecA (counts)
610 for (i = 0; i < 4; i++)
611 {
612 pthisMagCal->fvecB[i] = pthisMagCal->fmatB[i][0] * pthisMagCal->fvecA[0];
613 for (j = 1; j < 4; j++)
614 pthisMagCal->fvecB[i] += pthisMagCal->fmatB[i][j] * pthisMagCal->fvecA[j];
615 }
616
617 // compute the hard iron vector (uT) correction for zero mean data
618 ftmp = 0.5F * pthisMag->fuTPerCount;
619 for (i = CHX; i <= CHZ; i++)
620 pthisMagCal->ftrV[i] = ftmp * pthisMagCal->fvecB[i];
621
622 // compute the geomagnetic field strength B (uT)
623 pthisMagCal->ftrB = pthisMagCal->fvecB[3] *
624 pthisMag->fuTPerCount *
625 pthisMag->fuTPerCount;
626 for (i = CHX; i <= CHZ; i++)
627 pthisMagCal->ftrB += pthisMagCal->ftrV[i] * pthisMagCal->ftrV[i];
628 pthisMagCal->ftrB = sqrtf(fabs(pthisMagCal->ftrB));
629
630 // add in the previously subtracted magnetic buffer mean to get true hard iron offset (uT)
631 ftmp = pthisMag->fuTPerCount / (float) pthisMagBuffer->iMagBufferCount;
632 for (i = CHX; i <= CHZ; i++)
633 pthisMagCal->ftrV[i] += (float) pthisMagCal->iSumBs[i] * ftmp;
634
635 // calculate E = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
636 // set E = beta^T.(X^T.Y) = fvecB^T.fvecA
637 fE = pthisMagCal->fvecB[0] * pthisMagCal->fvecA[0];
638 for (i = 1; i < 4; i++)
639 fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
640
641 // set E = YTY - 2 * beta^T.(X^T.Y) = YTY - 2 * E;
642 fE = pthisMagCal->fYTY - 2.0F * fE;
643
644 // set fvecA = (X^T.X).beta = fmatA.fvecB
645 for (i = 0; i < 4; i++)
646 {
647 pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][0] * pthisMagCal->fvecB[0];
648 for (j = 1; j < 4; j++)
649 pthisMagCal->fvecA[i] += pthisMagCal->fmatA[i][j] * pthisMagCal->fvecB[j];
650 }
651
652 // add beta^T.(X^T.X).beta = fvecB^T * fvecA to give fit error E (un-normalized counts^4)
653 for (i = 0; i < 4; i++)
654 fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
655
656 // normalize fit error to the number of measurements and take square root to get error in uT^2
657 fE = sqrtf(fabs(fE) / (float) pthisMagBuffer->iMagBufferCount) *
658 pthisMag->fuTPerCount *
659 pthisMag->fuTPerCount;
660
661 // obtain dimensionless error by dividing square square of the geomagnetic field and convert to percent
662 pthisMagCal->ftrFitErrorpc = 50.0F * fE / (pthisMagCal->ftrB * pthisMagCal->ftrB);
663
664 // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
665 // that a new 4 element calibration is available
666 pthisMagCal->iCalInProgress = 0;
667 pthisMagCal->iNewCalibrationAvailable = 4;
668 } // end of time slice MAGBUFFSIZEX+2
669
670 return;
671 }
672
673 // 7 element calibration using direct eigen-decomposition
fUpdateMagCalibration7Slice(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)674 void fUpdateMagCalibration7Slice(struct MagCalibration *pthisMagCal,
675 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
676 {
677 // local variables
678 float fresidue; // eigen-decomposition residual sum
679 float ftmp; // scratch variable
680 int8 i,
681 j,
682 k,
683 l; // loop counters
684 #define MATRIX_7_SIZE 7
685 // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
686 if (pthisMagCal->iInitiateMagCal)
687 {
688 pthisMagCal->itimeslice = 0;
689 pthisMagCal->iInitiateMagCal = false;
690 pthisMagCal->iMagBufferReadOnly = true;
691 }
692
693 // time slice 0: 18.1K KL25Z ticks for 300 measurements = 0.38ms on KL25Z (variable) stored in systick[0]
694 // zero measurement matrix and calculate the mean values in the magnetic buffer
695 if (pthisMagCal->itimeslice == 0)
696 {
697 int16 iM; // number of measurements in the magnetic buffer
698
699 // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
700 for (i = 0; i < MATRIX_7_SIZE; i++)
701 for (j = i; j < MATRIX_7_SIZE; j++)
702 pthisMagCal->fmatA[i][j] = 0.0F;
703
704 // compute the sum of measurements in the magnetic buffer
705 iM = 0;
706 for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
707 for (i = 0; i < MAGBUFFSIZEX; i++)
708 {
709 for (j = 0; j < MAGBUFFSIZEY; j++)
710 {
711 if (pthisMagBuffer->index[i][j] != -1)
712 {
713 iM++;
714 for (k = 0; k < 3; k++)
715 pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
716 }
717 }
718 }
719
720 // compute the magnetic buffer measurement averages with nearest integer rounding
721 for (i = 0; i < 3; i++)
722 {
723 if (pthisMagCal->iSumBs[i] >= 0)
724 pthisMagCal->iMeanBs[i] =
725 (
726 pthisMagCal->iSumBs[i] +
727 ((int32) iM >> 1)
728 ) /
729 (int32) iM;
730 else
731 pthisMagCal->iMeanBs[i] =
732 (
733 pthisMagCal->iSumBs[i] -
734 ((int32) iM >> 1)
735 ) /
736 (int32) iM;
737 }
738
739 // as defensive programming also ensure the number of measurements found is re-stored
740 pthisMagBuffer->iMagBufferCount = iM;
741
742 // increment the time slice for the next iteration
743 (pthisMagCal->itimeslice)++;
744 } // end of time slice 0
745
746 // time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY: accumulate matrices: 8.6K KL25Z ticks = 0.18ms (with max stored in systick[1])
747 else if ((pthisMagCal->itimeslice >= 1) &&
748 (pthisMagCal->itimeslice <= MAGBUFFSIZEX * MAGBUFFSIZEY))
749 {
750 // accumulate the symmetric matrix fmatA on the zero mean measurements
751 i = (pthisMagCal->itimeslice - 1) / MAGBUFFSIZEY; // matrix row i ranges 0 to MAGBUFFSIZEX-1
752 j = (pthisMagCal->itimeslice - 1) % MAGBUFFSIZEY; // matrix column j ranges 0 to MAGBUFFSIZEY-1
753 if (pthisMagBuffer->index[i][j] != -1)
754 {
755 // set fvecA to be vector of zero mean measurements and their squares
756 for (k = 0; k < 3; k++)
757 {
758 pthisMagCal->fvecA[k + 3] = (float)
759 (
760 (int32) pthisMagBuffer->iBs[k][i][j] -
761 (int32) pthisMagCal->iMeanBs[k]
762 );
763 pthisMagCal->fvecA[k] = pthisMagCal->fvecA[k + 3] * pthisMagCal->fvecA[k + 3];
764 }
765
766 // update non-zero elements fmatA[0-2][6] of fmatA ignoring fmatA[6][6] which is set later.
767 // elements fmatA[3-5][6] are zero as a result of subtracting the mean value
768 for (k = 0; k < 3; k++)
769 pthisMagCal->fmatA[k][6] += pthisMagCal->fvecA[k];
770
771 // update the remaining on and above diagonal elements fmatA[0-5][0-5]
772 for (k = 0; k < (MATRIX_7_SIZE - 1); k++)
773 {
774 for (l = k; l < (MATRIX_7_SIZE - 1); l++)
775 pthisMagCal->fmatA[k][l] += pthisMagCal->fvecA[k] * pthisMagCal->fvecA[l];
776 }
777 }
778
779 // increment the time slice for the next iteration
780 (pthisMagCal->itimeslice)++;
781 } // end of time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY
782
783 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1: 0.8K ticks on KL25Z = 0.02ms on KL25Z (constant) (stored in systick[2])
784 // re-enable magnetic buffer for writing and prepare fmatA, fmatB, fvecA for eigendecomposition
785 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 1))
786 {
787 // set fmatA[6][6] to the number of magnetic measurements found
788 pthisMagCal->fmatA[MATRIX_7_SIZE - 1][MATRIX_7_SIZE - 1] = (float) pthisMagBuffer->iMagBufferCount;
789
790 // clear the magnetic buffer read only flag now that the matrices have been computed
791 pthisMagCal->iMagBufferReadOnly = false;
792
793 // set below diagonal elements of 7x7 matrix fmatA to above diagonal elements
794 for (i = 1; i < MATRIX_7_SIZE; i++)
795 for (j = 0; j < i; j++)
796 pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
797
798 // set matrix of eigenvectors fmatB to identity matrix and eigenvalues vector fvecA to diagonal elements of fmatA
799 for (i = 0; i < MATRIX_7_SIZE; i++)
800 {
801 for (j = 0; j < MATRIX_7_SIZE; j++)
802 pthisMagCal->fmatB[i][j] = 0.0F;
803 pthisMagCal->fmatB[i][i] = 1.0F;
804 pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
805 }
806
807 // increment the time slice for the next iteration
808 (pthisMagCal->itimeslice)++;
809 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1
810
811 // repeating 21 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 22 inclusive
812 // to perform the eigendecomposition of the measurement matrix fmatA.
813 // 19.6k ticks = 0.41ms on KL25Z (with max stored in systick[3]).
814 // for a 7x7 matrix there are 21 above diagonal elements: 6+5+4+3+2+1+0=21
815 else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 2)) &&
816 (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 22)))
817 {
818 // set k to the matrix element in range 0 to 20 to be zeroed and used it to set row i and column j
819 k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 2);
820 if (k < 6)
821 {
822 i = 0;
823 j = k + 1;
824 }
825 else if (k < 11)
826 {
827 i = 1;
828 j = k - 4;
829 }
830 else if (k < 15)
831 {
832 i = 2;
833 j = k - 8;
834 }
835 else if (k < 18)
836 {
837 i = 3;
838 j = k - 11;
839 }
840 else if (k < 20)
841 {
842 i = 4;
843 j = k - 13;
844 }
845 else
846 {
847 i = 5;
848 j = 6;
849 }
850
851 // only continue if matrix element i, j has not already been zeroed
852 if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
853 fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
854 pthisMagCal->fvecA, i, j, MATRIX_7_SIZE);
855
856 // increment the time slice for the next iteration
857 (pthisMagCal->itimeslice)++;
858 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 22 inclusive
859
860 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 23: 2.6k ticks on KL25Z = 0.05ms on KL25Z (constant) (stored in systick[4])
861 // compute the sum of the absolute values of above diagonal elements in fmatA as eigen-decomposition exit criterion
862 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 23))
863 {
864 // sum residue of all above-diagonal elements
865 fresidue = 0.0F;
866 for (i = 0; i < MATRIX_7_SIZE; i++)
867 for (j = i + 1; j < MATRIX_7_SIZE; j++)
868 fresidue += fabsf(pthisMagCal->fmatA[i][j]);
869
870 // determine whether to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
871 if (fresidue > 0.0F)
872 // continue the eigen-decomposition
873 (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 2;
874 else
875 // continue to compute the calibration coefficients since the eigen-decomposition is complete
876 (pthisMagCal->itimeslice)++;
877 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 23
878
879 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 24: 27.8k ticks = 0.58ms on KL25Z (constant) (stored in systick[5])
880 // compute the calibration coefficients from the solution eigenvector
881 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 24))
882 {
883 float fdetA; // determinant of ellipsoid matrix A
884 int8 imin; // column of solution eigenvector with minimum eigenvalue
885
886 // set imin to the index of the smallest eigenvalue in fvecA
887 imin = 0;
888 for (i = 1; i < MATRIX_7_SIZE; i++)
889 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[imin]) imin = i;
890
891 // the ellipsoid fA must have positive determinant but the eigensolver can equally easily return a negated
892 // normalized eigenvector without changing the eigenvalue. compute the determinant of ellipsoid matrix A
893 // from first three elements of eigenvector and negate the eigenvector if negative.
894 fdetA = pthisMagCal->fmatB[CHX][imin] *
895 pthisMagCal->fmatB[CHY][imin] *
896 pthisMagCal->fmatB[CHZ][imin];
897 if (fdetA < 0.0F)
898 {
899 fdetA = fabs(fdetA);
900 for (i = 0; i < MATRIX_7_SIZE; i++)
901 pthisMagCal->fmatB[i][imin] = -pthisMagCal->fmatB[i][imin];
902 }
903
904 // set diagonal elements of ellipsoid matrix A to the solution vector imin with smallest eigenvalue and
905 // zero off-diagonal elements since these are always zero in the 7 element model
906 // compute the hard iron offset fV for zero mean data (counts)
907 for (i = CHX; i <= CHZ; i++)
908 pthisMagCal->ftrV[i] = -0.5F *
909 pthisMagCal->fmatB[i + 3][imin] /
910 pthisMagCal->fmatB[i][imin];
911
912 // set ftmp to the square of the geomagnetic field strength fB (counts) corresponding to current value of fdetA
913 ftmp = -pthisMagCal->fmatB[6][imin];
914 for (i = CHX; i <= CHZ; i++)
915 ftmp += pthisMagCal->fmatB[i][imin] *
916 pthisMagCal->ftrV[i] *
917 pthisMagCal->ftrV[i];
918
919 // calculate the trial normalized fit error as a percentage normalized by the geomagnetic field strength squared
920 pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[imin]) /
921 (float) pthisMagBuffer->iMagBufferCount) / fabsf(ftmp);
922
923 // compute the geomagnetic field strength (uT) for current value of fdetA
924 pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * pthisMag->fuTPerCount;
925
926 // compute the normalized ellipsoid matrix A with unit determinant and derive invW also with unit determinant.
927 ftmp = powf(fabs(fdetA), -(ONETHIRD));
928 for (i = CHX; i <= CHZ; i++)
929 {
930 pthisMagCal->fA[i][i] = pthisMagCal->fmatB[i][imin] * ftmp;
931 pthisMagCal->ftrinvW[i][i] = sqrtf(fabsf(pthisMagCal->fA[i][i]));
932 }
933
934 pthisMagCal->fA[CHX][CHY] = pthisMagCal->fA[CHX][CHZ] = pthisMagCal->fA[CHY][CHZ] = 0.0F;
935 pthisMagCal->ftrinvW[CHX][CHY] = pthisMagCal->ftrinvW[CHX][CHZ] = pthisMagCal->ftrinvW[CHY][CHZ] = 0.0F;
936
937 // each element of fA has been scaled by fdetA^(-1/3) so the square of geomagnetic field strength B^2
938 // must be adjusted by the same amount and B by the square root to keep the ellipsoid equation valid:
939 // (Bk-V)^T.A.(Bk-V) = (Bk-V)^T.(invW)^T.invW.(Bk-V) = B^2
940 pthisMagCal->ftrB *= sqrt(fabs(ftmp));
941
942 // compute the final hard iron offset (uT) by adding in previously subtracted zero mean offset (counts)
943 for (i = CHX; i <= CHZ; i++)
944 pthisMagCal->ftrV[i] =
945 (
946 pthisMagCal->ftrV[i] +
947 (float) pthisMagCal->iMeanBs[i]
948 ) *
949 pthisMag->fuTPerCount;
950
951 // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
952 // that a new 7 element calibration is available
953 pthisMagCal->iCalInProgress = 0;
954 pthisMagCal->iNewCalibrationAvailable = 7;
955 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 24
956
957 return;
958 }
959
960 // 10 element calibration using direct eigen-decomposition
fUpdateMagCalibration10Slice(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)961 void fUpdateMagCalibration10Slice(struct MagCalibration *pthisMagCal,
962 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
963 {
964 // local variables
965 float fresidue; // eigen-decomposition residual sum
966 float ftmp; // scratch variable
967 int8 i,
968 j,
969 k,
970 l; // loop counters
971 #define MATRIX_10_SIZE 10
972 // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
973 if (pthisMagCal->iInitiateMagCal)
974 {
975 pthisMagCal->itimeslice = 0;
976 pthisMagCal->iInitiateMagCal = false;
977 pthisMagCal->iMagBufferReadOnly = true;
978 }
979
980 // time slice 0: 18.7k KL25Z ticks for 300 measurements = 0.39ms on KL25Z (variable) stored in systick[0]
981 // zero measurement matrix fmatA and calculate the mean values in the magnetic buffer
982 if (pthisMagCal->itimeslice == 0)
983 {
984 int16 iM; // number of measurements in the magnetic buffer
985
986 // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
987 for (i = 0; i < MATRIX_10_SIZE; i++)
988 for (j = i; j < MATRIX_10_SIZE; j++)
989 pthisMagCal->fmatA[i][j] = 0.0F;
990
991 // compute the sum of measurements in the magnetic buffer
992 iM = 0;
993 for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
994 for (i = 0; i < MAGBUFFSIZEX; i++)
995 {
996 for (j = 0; j < MAGBUFFSIZEY; j++)
997 {
998 if (pthisMagBuffer->index[i][j] != -1)
999 {
1000 iM++;
1001 for (k = 0; k < 3; k++)
1002 pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
1003 }
1004 }
1005 }
1006
1007 // compute the magnetic buffer measurement averages with nearest integer rounding
1008 for (i = 0; i < 3; i++)
1009 {
1010 if (pthisMagCal->iSumBs[i] >= 0)
1011 pthisMagCal->iMeanBs[i] =
1012 (
1013 pthisMagCal->iSumBs[i] +
1014 ((int32) iM >> 1)
1015 ) /
1016 (int32) iM;
1017 else
1018 pthisMagCal->iMeanBs[i] =
1019 (
1020 pthisMagCal->iSumBs[i] -
1021 ((int32) iM >> 1)
1022 ) /
1023 (int32) iM;
1024 }
1025
1026 // as defensive programming also ensure the number of measurements found is re-stored
1027 pthisMagBuffer->iMagBufferCount = iM;
1028
1029 // increment the time slice for the next iteration
1030 (pthisMagCal->itimeslice)++;
1031 } // end of time slice 0
1032
1033 // time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY: accumulate matrices: 17.9k KL25Z ticks = 0.37ms for 300 measurements
1034 // (with max measured stored in systick[1])
1035 else if ((pthisMagCal->itimeslice >= 1) &&
1036 (pthisMagCal->itimeslice <= MAGBUFFSIZEX * MAGBUFFSIZEY))
1037 {
1038 // accumulate the symmetric matrix fmatA on the zero mean measurements
1039 i = (pthisMagCal->itimeslice - 1) / MAGBUFFSIZEY; // matrix row i ranges 0 to MAGBUFFSIZEX-1
1040 j = (pthisMagCal->itimeslice - 1) % MAGBUFFSIZEY; // matrix column j ranges 0 to MAGBUFFSIZEY-1
1041 if (pthisMagBuffer->index[i][j] != -1)
1042 {
1043 // set fvecA[6-8] to the zero mean measurements
1044 for (k = 0; k < 3; k++)
1045 pthisMagCal->fvecA[k + 6] = (float)
1046 (
1047 (int32) pthisMagBuffer->iBs[k][i][j] -
1048 (int32) pthisMagCal->iMeanBs[k]
1049 );
1050
1051 // compute fvecA[0-5] from fvecA[6-8]
1052 pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
1053 pthisMagCal->fvecA[1] = 2.0F *
1054 pthisMagCal->fvecA[6] *
1055 pthisMagCal->fvecA[7];
1056 pthisMagCal->fvecA[2] = 2.0F *
1057 pthisMagCal->fvecA[6] *
1058 pthisMagCal->fvecA[8];
1059 pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
1060 pthisMagCal->fvecA[4] = 2.0F *
1061 pthisMagCal->fvecA[7] *
1062 pthisMagCal->fvecA[8];
1063 pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
1064
1065 // update non-zero elements fmatA[0-5][9] of fmatA ignoring fmatA[9][9] which is set later.
1066 // elements fmatA[6-8][9] are zero as a result of subtracting the mean value
1067 for (k = 0; k < 6; k++)
1068 pthisMagCal->fmatA[k][9] += pthisMagCal->fvecA[k];
1069
1070 // update the remaining on and above diagonal elements fmatA[0-8][0-8]
1071 for (k = 0; k < (MATRIX_10_SIZE - 1); k++)
1072 {
1073 for (l = k; l < (MATRIX_10_SIZE - 1); l++)
1074 pthisMagCal->fmatA[k][l] += pthisMagCal->fvecA[k] * pthisMagCal->fvecA[l];
1075 }
1076 }
1077
1078 // increment the time slice for the next iteration
1079 (pthisMagCal->itimeslice)++;
1080 } // end of time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY
1081
1082 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1: 1.2k ticks on KL25Z = 0.025ms on KL25Z (constant) (stored in systick[2])
1083 // re-enable magnetic buffer for writing and prepare fmatA, fmatB, fvecA for eigendecomposition
1084 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 1))
1085 {
1086 // set fmatA[9][9] to the number of magnetic measurements found
1087 pthisMagCal->fmatA[MATRIX_10_SIZE - 1][MATRIX_10_SIZE - 1] = (float) pthisMagBuffer->iMagBufferCount;
1088
1089 // clear the magnetic buffer read only flag now that the matrices have been computed
1090 pthisMagCal->iMagBufferReadOnly = false;
1091
1092 // set below diagonal elements of 10x10 matrix fmatA to above diagonal elements
1093 for (i = 1; i < MATRIX_10_SIZE; i++)
1094 for (j = 0; j < i; j++)
1095 pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
1096
1097 // set matrix of eigenvectors fmatB to identity matrix and eigenvalues vector fvecA to diagonal elements of fmatA
1098 for (i = 0; i < MATRIX_10_SIZE; i++)
1099 {
1100 for (j = 0; j < MATRIX_10_SIZE; j++)
1101 pthisMagCal->fmatB[i][j] = 0.0F;
1102 pthisMagCal->fmatB[i][i] = 1.0F;
1103 pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
1104 }
1105
1106 // increment the time slice for the next iteration
1107 (pthisMagCal->itimeslice)++;
1108 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1
1109
1110 // repeating 45 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 46 inclusive
1111 // to perform the eigendecomposition of the measurement matrix fmatA.
1112 // 28.2k ticks = 0.56ms on KL25Z (with max stored in systick[3]).
1113 // for a 10x10 matrix there are 45 above diagonal elements: 9+8+7+6+5+4+3+2+1+0=45
1114 else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 2)) &&
1115 (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 46)))
1116 {
1117 // set k to the matrix element of interest in range 0 to 44 to be zeroed and set row i and column j
1118 k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 2);
1119 if (k < 9)
1120 {
1121 i = 0;
1122 j = k + 1;
1123 }
1124 else if (k < 17)
1125 {
1126 i = 1;
1127 j = k - 7;
1128 }
1129 else if (k < 24)
1130 {
1131 i = 2;
1132 j = k - 14;
1133 }
1134 else if (k < 30)
1135 {
1136 i = 3;
1137 j = k - 20;
1138 }
1139 else if (k < 35)
1140 {
1141 i = 4;
1142 j = k - 25;
1143 }
1144 else if (k < 39)
1145 {
1146 i = 5;
1147 j = k - 29;
1148 }
1149 else if (k < 42)
1150 {
1151 i = 6;
1152 j = k - 32;
1153 }
1154 else if (k < 44)
1155 {
1156 i = 7;
1157 j = k - 34;
1158 }
1159 else
1160 {
1161 i = 8;
1162 j = 9;
1163 }
1164
1165 // only continue if matrix element i, j has not already been zeroed
1166 if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
1167 fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
1168 pthisMagCal->fvecA, i, j, MATRIX_10_SIZE);
1169
1170 // increment the time slice for the next iteration
1171 (pthisMagCal->itimeslice)++;
1172 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 46 inclusive
1173
1174 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 47: 5.6k ticks on KL25Z = 0.12ms on KL25Z (constant) (stored in systick[4])
1175 // compute the sum of the absolute values of above diagonal elements in fmatA as eigen-decomposition exit criterion
1176 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 47))
1177 {
1178 // sum residue of all above-diagonal elements
1179 fresidue = 0.0F;
1180 for (i = 0; i < MATRIX_10_SIZE; i++)
1181 for (j = i + 1; j < MATRIX_10_SIZE; j++)
1182 fresidue += fabsf(pthisMagCal->fmatA[i][j]);
1183
1184 // determine whether to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
1185 if (fresidue > 0.0F)
1186 // continue the eigen-decomposition
1187 (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 2;
1188 else
1189 // continue to compute the calibration coefficients since the eigen-decomposition is complete
1190 (pthisMagCal->itimeslice)++;
1191 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 47
1192
1193 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 48: 38.5k ticks = 0.80ms on KL25Z (constant) (stored in systick[5])
1194 // compute the calibration coefficients (excluding invW) from the solution eigenvector
1195 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 48))
1196 {
1197 float fdetA; // determinant of ellipsoid matrix A
1198 int8 imin; // column of solution eigenvector with minimum eigenvalue
1199
1200 // set imin to the index of the smallest eigenvalue in fvecA
1201 imin = 0;
1202 for (i = 1; i < MATRIX_10_SIZE; i++)
1203 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[imin]) imin = i;
1204
1205 // set the ellipsoid matrix A from elements 0 to 5 of the solution eigenvector.
1206 pthisMagCal->fA[CHX][CHX] = pthisMagCal->fmatB[0][imin];
1207 pthisMagCal->fA[CHX][CHY] = pthisMagCal->fA[CHY][CHX] = pthisMagCal->fmatB[1][imin];
1208 pthisMagCal->fA[CHX][CHZ] = pthisMagCal->fA[CHZ][CHX] = pthisMagCal->fmatB[2][imin];
1209 pthisMagCal->fA[CHY][CHY] = pthisMagCal->fmatB[3][imin];
1210 pthisMagCal->fA[CHY][CHZ] = pthisMagCal->fA[CHZ][CHY] = pthisMagCal->fmatB[4][imin];
1211 pthisMagCal->fA[CHZ][CHZ] = pthisMagCal->fmatB[5][imin];
1212
1213 // negate A and the entire solution vector A has negative determinant
1214 fdetA = f3x3matrixDetA(pthisMagCal->fA);
1215 if (fdetA < 0.0F)
1216 {
1217 f3x3matrixAeqMinusA(pthisMagCal->fA);
1218 fdetA = fabs(fdetA);
1219 for (i = 0; i < MATRIX_10_SIZE; i++)
1220 pthisMagCal->fmatB[i][imin] = -pthisMagCal->fmatB[i][imin];
1221 }
1222
1223 // set finvA to the inverse of the ellipsoid matrix fA
1224 f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
1225
1226 // compute the hard iron offset fV for zero mean data (counts)
1227 for (i = CHX; i <= CHZ; i++)
1228 {
1229 pthisMagCal->ftrV[i] = pthisMagCal->finvA[i][0] *
1230 pthisMagCal->fmatB[6][imin] +
1231 pthisMagCal->finvA[i][1] *
1232 pthisMagCal->fmatB[7][imin] +
1233 pthisMagCal->finvA[i][2] *
1234 pthisMagCal->fmatB[8][imin];
1235 pthisMagCal->ftrV[i] *= -0.5F;
1236 }
1237
1238 // compute the geomagnetic field strength B (counts) for current (un-normalized) ellipsoid matrix determinant.
1239 ftmp = pthisMagCal->fA[CHX][CHY] *
1240 pthisMagCal->ftrV[CHX] *
1241 pthisMagCal->ftrV[CHY] +
1242 pthisMagCal->fA[CHX][CHZ] *
1243 pthisMagCal->ftrV[CHX] *
1244 pthisMagCal->ftrV[CHZ] +
1245 pthisMagCal->fA[CHY][CHZ] *
1246 pthisMagCal->ftrV[CHY] *
1247 pthisMagCal->ftrV[CHZ];
1248 ftmp *= 2.0F;
1249 ftmp -= pthisMagCal->fmatB[9][imin];
1250 ftmp += pthisMagCal->fA[CHX][CHX] *
1251 pthisMagCal->ftrV[CHX] *
1252 pthisMagCal->ftrV[CHX];
1253 ftmp += pthisMagCal->fA[CHY][CHY] *
1254 pthisMagCal->ftrV[CHY] *
1255 pthisMagCal->ftrV[CHY];
1256 ftmp += pthisMagCal->fA[CHZ][CHZ] *
1257 pthisMagCal->ftrV[CHZ] *
1258 pthisMagCal->ftrV[CHZ];
1259 pthisMagCal->ftrB = sqrtf(fabsf(ftmp));
1260
1261 // calculate the normalized fit error as a percentage
1262 pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(
1263 pthisMagCal->fvecA[imin] /
1264 (float) pthisMagBuffer->iMagBufferCount
1265 )) / (pthisMagCal->ftrB * pthisMagCal->ftrB);
1266
1267 // convert the trial geomagnetic field strength B from counts to uT for un-normalized soft iron matrix A
1268 pthisMagCal->ftrB *= pthisMag->fuTPerCount;
1269
1270 // compute the final hard iron offset (uT) by adding in previously subtracted zero mean offset (counts)
1271 for (i = CHX; i <= CHZ; i++)
1272 pthisMagCal->ftrV[i] =
1273 (
1274 pthisMagCal->ftrV[i] +
1275 (float) pthisMagCal->iMeanBs[i]
1276 ) *
1277 pthisMag->fuTPerCount;
1278
1279 // normalize the ellipsoid matrix A to unit determinant
1280 ftmp = powf(fabs(fdetA), -(ONETHIRD));
1281 f3x3matrixAeqAxScalar(pthisMagCal->fA, ftmp);
1282
1283 // each element of fA has been scaled by fdetA^(-1/3) so the square of geomagnetic field strength B^2
1284 // must be adjusted by the same amount and B by the square root to keep the ellipsoid equation valid:
1285 // (Bk-V)^T.A.(Bk-V) = (Bk-V)^T.(invW)^T.invW.(Bk-V) = B^2
1286 pthisMagCal->ftrB *= sqrt(fabs(ftmp));
1287
1288 // prepare for eigendecomposition of fA to compute finvW from the square root of fA by setting
1289 // fmatA (upper left) to the 3x3 matrix fA, set fmatB (eigenvectors to identity matrix and
1290 // fvecA (eigenvalues) to diagonal elements of fmatA = fA
1291 for (i = 0; i < 3; i++)
1292 {
1293 for (j = 0; j < 3; j++)
1294 {
1295 pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
1296 pthisMagCal->fmatB[i][j] = 0.0F;
1297 }
1298
1299 pthisMagCal->fmatB[i][i] = 1.0F;
1300 pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
1301 }
1302
1303 // increment the time slice for the next iteration
1304 (pthisMagCal->itimeslice)++;
1305 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 48
1306
1307 // repeating 3 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 49 to MAGBUFFSIZEX * MAGBUFFSIZEY + 51 inclusive
1308 // 9.7kticks = 0.2ms on KL25Z (with max stored in systick[6]).
1309 // perform the eigendecomposition of the 3x3 ellipsoid matrix fA which has 3 above diagonal elements: 2+1+0=3
1310 else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 49)) &&
1311 (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 51)))
1312 {
1313 // set k to the matrix element of interest in range 0 to 2 to be zeroed and set row i and column j
1314 k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 49);
1315 if (k == 0)
1316 {
1317 i = 0;
1318 j = 1;
1319 }
1320 else if (k == 1)
1321 {
1322 i = 0;
1323 j = 2;
1324 }
1325 else
1326 {
1327 i = 1;
1328 j = 2;
1329 }
1330
1331 // only continue with eigen-decomposition if matrix element i, j has not already been zeroed
1332 if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
1333 fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
1334 pthisMagCal->fvecA, i, j, 3);
1335
1336 // increment the time slice for the next iteration
1337 (pthisMagCal->itimeslice)++;
1338 } // end of time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 49 to MAGBUFFSIZEX * MAGBUFFSIZEY + 51 inclusive
1339
1340 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 52: 0.3k ticks = 0.006ms on KL25Z (constant) (stored in systick[7])
1341 // determine whether the eigen-decomposition of fA is complete
1342 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 52))
1343 {
1344 // sum residue of all above-diagonal elements and use to determine whether
1345 // to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
1346 fresidue = fabsf(pthisMagCal->fmatA[0][1]) + fabsf(pthisMagCal->fmatA[0][2]) + fabsf(pthisMagCal->fmatA[1][2]);
1347 if (fresidue > 0.0F)
1348 // continue the eigen-decomposition
1349 (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 49;
1350 else
1351 // continue to compute the calibration coefficients since the eigen-decomposition is complete
1352 (pthisMagCal->itimeslice)++;
1353 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 52
1354
1355 // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 53: 9.3k ticks = 0.19ms on KL25Z (constant) (stored in systick[8])
1356 // compute the inverse gain matrix invW from the eigendecomposition of the ellipsoid matrix A
1357 else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 53))
1358 {
1359 // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
1360 for (j = 0; j < 3; j++) // loop over columns j
1361 {
1362 ftmp = sqrtf(sqrtf(fabsf(pthisMagCal->fvecA[j])));
1363 for (i = 0; i < 3; i++) // loop over rows i
1364 pthisMagCal->fmatB[i][j] *= ftmp;
1365 }
1366
1367 // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
1368 // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
1369 // loop over on and above diagonal elements
1370 for (i = 0; i < 3; i++)
1371 {
1372 for (j = i; j < 3; j++)
1373 {
1374 pthisMagCal->ftrinvW[i][j] = pthisMagCal->ftrinvW[j][i] =
1375 pthisMagCal->fmatB[i][0] *
1376 pthisMagCal->fmatB[j][0] +
1377 pthisMagCal->fmatB[i][1] *
1378 pthisMagCal->fmatB[j][1] +
1379 pthisMagCal->fmatB[i][2] *
1380 pthisMagCal->fmatB[j][2];
1381 }
1382 }
1383
1384 // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
1385 // that a new 10 element calibration is available
1386 pthisMagCal->iCalInProgress = 0;
1387 pthisMagCal->iNewCalibrationAvailable = 10;
1388 } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 53
1389
1390 return;
1391 }
1392
1393 // 4 element calibration using 4x4 matrix inverse
fComputeMagCalibration4(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)1394 void fComputeMagCalibration4(struct MagCalibration *pthisMagCal,
1395 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
1396 {
1397 // local variables
1398 float fBs2; // fBs[CHX]^2+fBs[CHY]^2+fBs[CHZ]^2
1399 float fSumBs4; // sum of fBs2
1400 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1401 float fE; // error function = r^T.r
1402 int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
1403 int16 iCount; // number of measurements counted
1404 int8 ierror; // matrix inversion error flag
1405 int8 i,
1406 j,
1407 k,
1408 l; // loop counters
1409
1410 // working arrays for 4x4 matrix inversion
1411 float *pfRows[4];
1412 int8 iColInd[4];
1413 int8 iRowInd[4];
1414 int8 iPivot[4];
1415
1416 // compute fscaling to reduce multiplications later
1417 fscaling = pthisMag->fuTPerCount / DEFAULTB;
1418
1419 // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
1420 f3x3matrixAeqI(pthisMagCal->ftrinvW);
1421
1422 // zero fSumBs4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above diagonal elements of fmatA=X^T*X (4x4)
1423 fSumBs4 = 0.0F;
1424 for (i = 0; i < 4; i++)
1425 {
1426 pthisMagCal->fvecB[i] = 0.0F;
1427 for (j = i; j < 4; j++)
1428 {
1429 pthisMagCal->fmatA[i][j] = 0.0F;
1430 }
1431 }
1432
1433 // the offsets are guaranteed to be set from the first element but to avoid compiler error
1434 iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1435
1436 // use entries from magnetic buffer to compute matrices
1437 iCount = 0;
1438 for (j = 0; j < MAGBUFFSIZEX; j++)
1439 {
1440 for (k = 0; k < MAGBUFFSIZEY; k++)
1441 {
1442 if (pthisMagBuffer->index[j][k] != -1)
1443 {
1444 // use first valid magnetic buffer entry as estimate (in counts) for offset
1445 if (iCount == 0)
1446 {
1447 for (l = CHX; l <= CHZ; l++)
1448 {
1449 iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1450 }
1451 }
1452
1453 // store scaled and offset fBs[XYZ] in fvecA[0-2] and fBs[XYZ]^2 in fvecA[3-5]
1454 for (l = CHX; l <= CHZ; l++)
1455 {
1456 pthisMagCal->fvecA[l] = (float)
1457 (
1458 (int32) pthisMagBuffer->iBs[l][j][k] -
1459 (int32) iOffset[l]
1460 ) * fscaling;
1461 pthisMagCal->fvecA[l + 3] = pthisMagCal->fvecA[l] * pthisMagCal->fvecA[l];
1462 }
1463
1464 // calculate fBs2 = fBs[CHX]^2 + fBs[CHY]^2 + fBs[CHZ]^2 (scaled uT^2)
1465 fBs2 = pthisMagCal->fvecA[3] +
1466 pthisMagCal->fvecA[4] +
1467 pthisMagCal->fvecA[5];
1468
1469 // accumulate fBs^4 over all measurements into fSumBs4=Y^T.Y
1470 fSumBs4 += fBs2 * fBs2;
1471
1472 // now we have fBs2, accumulate fvecB[0-2] = X^T.Y =sum(fBs2.fBs[XYZ])
1473 for (l = CHX; l <= CHZ; l++)
1474 {
1475 pthisMagCal->fvecB[l] += pthisMagCal->fvecA[l] * fBs2;
1476 }
1477
1478 //accumulate fvecB[3] = X^T.Y =sum(fBs2)
1479 pthisMagCal->fvecB[3] += fBs2;
1480
1481 // accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
1482 pthisMagCal->fmatA[0][0] += pthisMagCal->fvecA[CHX + 3];
1483 pthisMagCal->fmatA[0][1] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHY];
1484 pthisMagCal->fmatA[0][2] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHZ];
1485 pthisMagCal->fmatA[0][3] += pthisMagCal->fvecA[CHX];
1486 pthisMagCal->fmatA[1][1] += pthisMagCal->fvecA[CHY + 3];
1487 pthisMagCal->fmatA[1][2] += pthisMagCal->fvecA[CHY] * pthisMagCal->fvecA[CHZ];
1488 pthisMagCal->fmatA[1][3] += pthisMagCal->fvecA[CHY];
1489 pthisMagCal->fmatA[2][2] += pthisMagCal->fvecA[CHZ + 3];
1490 pthisMagCal->fmatA[2][3] += pthisMagCal->fvecA[CHZ];
1491
1492 // increment the counter for next iteration
1493 iCount++;
1494 }
1495 }
1496 }
1497
1498 // set the last element of the measurement matrix to the number of buffer elements used
1499 pthisMagCal->fmatA[3][3] = (float) iCount;
1500
1501 // store the number of measurements accumulated (defensive programming, should never be needed)
1502 pthisMagBuffer->iMagBufferCount = iCount;
1503
1504 // use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
1505 for (i = 0; i < 4; i++)
1506 {
1507 for (j = i; j < 4; j++)
1508 {
1509 pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[j][i] = pthisMagCal->fmatA[i][j];
1510 }
1511 }
1512
1513 // calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
1514 for (i = 0; i < 4; i++)
1515 {
1516 pfRows[i] = pthisMagCal->fmatB[i];
1517 }
1518
1519 fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
1520
1521 // calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
1522 for (i = 0; i < 4; i++)
1523 {
1524 pthisMagCal->fvecA[i] = 0.0F;
1525 for (k = 0; k < 4; k++)
1526 {
1527 pthisMagCal->fvecA[i] += pthisMagCal->fmatB[i][k] * pthisMagCal->fvecB[k];
1528 }
1529 }
1530
1531 // calculate E = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
1532 // = fSumBs4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
1533 // first set E = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBs4 - 2 * fvecA^T.fvecB
1534 fE = 0.0F;
1535 for (i = 0; i < 4; i++)
1536 {
1537 fE += pthisMagCal->fvecA[i] * pthisMagCal->fvecB[i];
1538 }
1539
1540 fE = fSumBs4 - 2.0F * fE;
1541
1542 // set fvecB = (X^T.X).beta = fmatA.fvecA
1543 for (i = 0; i < 4; i++)
1544 {
1545 pthisMagCal->fvecB[i] = 0.0F;
1546 for (k = 0; k < 4; k++)
1547 {
1548 pthisMagCal->fvecB[i] += pthisMagCal->fmatA[i][k] * pthisMagCal->fvecA[k];
1549 }
1550 }
1551
1552 // complete calculation of E by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
1553 for (i = 0; i < 4; i++)
1554 {
1555 fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
1556 }
1557
1558 // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
1559 for (l = CHX; l <= CHZ; l++)
1560 {
1561 pthisMagCal->ftrV[l] = 0.5F * pthisMagCal->fvecA[l];
1562 }
1563
1564 // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
1565 pthisMagCal->ftrB = sqrtf(pthisMagCal->fvecA[3] + pthisMagCal->ftrV[CHX] *
1566 pthisMagCal->ftrV[CHX] + pthisMagCal->ftrV[CHY] *
1567 pthisMagCal->ftrV[CHY] + pthisMagCal->ftrV[CHZ] *
1568 pthisMagCal->ftrV[CHZ]);
1569
1570 // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
1571 pthisMagCal->ftrFitErrorpc = sqrtf(fE / (float) pthisMagBuffer->iMagBufferCount) * 100.0F / (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
1572
1573 // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
1574 for (l = CHX; l <= CHZ; l++)
1575 {
1576 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1577 DEFAULTB +
1578 (float) iOffset[l] *
1579 pthisMag->fuTPerCount;
1580 }
1581
1582 // correct the geomagnetic field strength B to correct scaling (result in uT)
1583 pthisMagCal->ftrB *= DEFAULTB;
1584
1585 return;
1586 }
1587
1588 // 7 element calibration using direct eigen-decomposition
fComputeMagCalibration7(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)1589 void fComputeMagCalibration7(struct MagCalibration *pthisMagCal,
1590 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
1591 {
1592 // local variables
1593 float det; // matrix determinant
1594 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1595 float ftmp; // scratch variable
1596 int16 iOffset[3]; // offset to remove large DC hard iron bias
1597 int16 iCount; // number of measurements counted
1598 int8 i,
1599 j,
1600 k,
1601 l,
1602 m,
1603 n; // loop counters
1604
1605 // compute fscaling to reduce multiplications later
1606 fscaling = pthisMag->fuTPerCount / DEFAULTB;
1607
1608 // the offsets are guaranteed to be set from the first element but to avoid compiler error
1609 iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1610
1611 // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
1612 for (m = 0; m < 7; m++)
1613 {
1614 for (n = m; n < 7; n++)
1615 {
1616 pthisMagCal->fmatA[m][n] = 0.0F;
1617 }
1618 }
1619
1620 // add megnetic buffer entries into product matrix fmatA
1621 iCount = 0;
1622 for (j = 0; j < MAGBUFFSIZEX; j++)
1623 {
1624 for (k = 0; k < MAGBUFFSIZEY; k++)
1625 {
1626 if (pthisMagBuffer->index[j][k] != -1)
1627 {
1628 // use first valid magnetic buffer entry as offset estimate (bit counts)
1629 if (iCount == 0)
1630 {
1631 for (l = CHX; l <= CHZ; l++)
1632 {
1633 iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1634 }
1635 }
1636
1637 // apply the offset and scaling and store in fvecA
1638 for (l = CHX; l <= CHZ; l++)
1639 {
1640 pthisMagCal->fvecA[l + 3] = (float)
1641 (
1642 (int32) pthisMagBuffer->iBs[l][j][k] -
1643 (int32) iOffset[l]
1644 ) * fscaling;
1645 pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3];
1646 }
1647
1648 // accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA}
1649 // with the exception of fmatA[6][6] which will sum to the number of measurements
1650 // and remembering that fvecA[6] equals 1.0F
1651 // update the right hand column [6] of fmatA except for fmatA[6][6]
1652 for (m = 0; m < 6; m++)
1653 {
1654 pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m];
1655 }
1656
1657 // update the on and above diagonal terms except for right hand column 6
1658 for (m = 0; m < 6; m++)
1659 {
1660 for (n = m; n < 6; n++)
1661 {
1662 pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
1663 }
1664 }
1665
1666 // increment the measurement counter for the next iteration
1667 iCount++;
1668 }
1669 }
1670 }
1671
1672 // finally set the last element fmatA[6][6] to the number of measurements
1673 pthisMagCal->fmatA[6][6] = (float) iCount;
1674
1675 // store the number of measurements accumulated (defensive programming, should never be needed)
1676 pthisMagBuffer->iMagBufferCount = iCount;
1677
1678 // copy the above diagonal elements of fmatA to below the diagonal
1679 for (m = 1; m < 7; m++)
1680 {
1681 for (n = 0; n < m; n++)
1682 {
1683 pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
1684 }
1685 }
1686
1687 // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
1688 fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1689 7);
1690
1691 // find the smallest eigenvalue
1692 j = 0;
1693 for (i = 1; i < 7; i++)
1694 {
1695 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
1696 {
1697 j = i;
1698 }
1699 }
1700
1701 // set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
1702 // and the hard iron offset (scaled and offset)
1703 f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F);
1704 det = 1.0F;
1705 for (l = CHX; l <= CHZ; l++)
1706 {
1707 pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j];
1708 det *= pthisMagCal->fA[l][l];
1709 pthisMagCal->ftrV[l] = -0.5F *
1710 pthisMagCal->fmatB[l + 3][j] /
1711 pthisMagCal->fA[l][l];
1712 }
1713
1714 // negate A if it has negative determinant
1715 if (det < 0.0F)
1716 {
1717 f3x3matrixAeqMinusA(pthisMagCal->fA);
1718 pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
1719 det = -det;
1720 }
1721
1722 // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
1723 ftmp = -pthisMagCal->fmatB[6][j];
1724 for (l = CHX; l <= CHZ; l++)
1725 {
1726 ftmp += pthisMagCal->fA[l][l] *
1727 pthisMagCal->ftrV[l] *
1728 pthisMagCal->ftrV[l];
1729 }
1730
1731 // normalize the ellipsoid matrix A to unit determinant
1732 f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
1733
1734 // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
1735 pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
1736
1737 // compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
1738 f3x3matrixAeqI(pthisMagCal->ftrinvW);
1739 for (l = CHX; l <= CHZ; l++)
1740 {
1741 pthisMagCal->ftrinvW[l][l] = sqrtf(fabsf(pthisMagCal->fA[l][l]));
1742 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1743 DEFAULTB +
1744 (float) iOffset[l] *
1745 pthisMag->fuTPerCount;
1746 }
1747
1748 return;
1749 }
1750
1751 // 10 element calibration using direct eigen-decomposition
fComputeMagCalibration10(struct MagCalibration * pthisMagCal,struct MagBuffer * pthisMagBuffer,struct MagSensor * pthisMag)1752 void fComputeMagCalibration10(struct MagCalibration *pthisMagCal,
1753 struct MagBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
1754 {
1755 // local variables
1756 float det; // matrix determinant
1757 float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1758 float ftmp; // scratch variable
1759 int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
1760 int16 iCount; // number of measurements counted
1761 int8 i,
1762 j,
1763 k,
1764 l,
1765 m,
1766 n; // loop counters
1767
1768 // compute fscaling to reduce multiplications later
1769 fscaling = pthisMag->fuTPerCount / DEFAULTB;
1770
1771 // the offsets are guaranteed to be set from the first element but to avoid compiler error
1772 iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1773
1774 // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
1775 for (m = 0; m < 10; m++)
1776 {
1777 for (n = m; n < 10; n++)
1778 {
1779 pthisMagCal->fmatA[m][n] = 0.0F;
1780 }
1781 }
1782
1783 // add magnetic buffer entries into the 10x10 product matrix fmatA
1784 iCount = 0;
1785 for (j = 0; j < MAGBUFFSIZEX; j++)
1786 {
1787 for (k = 0; k < MAGBUFFSIZEY; k++)
1788 {
1789 if (pthisMagBuffer->index[j][k] != -1)
1790 {
1791 // use first valid magnetic buffer entry as estimate for offset to help solution (bit counts)
1792 if (iCount == 0)
1793 {
1794 for (l = CHX; l <= CHZ; l++)
1795 {
1796 iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1797 }
1798 }
1799
1800 // apply the fixed offset and scaling and enter into fvecA[6-8]
1801 for (l = CHX; l <= CHZ; l++)
1802 {
1803 pthisMagCal->fvecA[l + 6] = (float)
1804 (
1805 (int32) pthisMagBuffer->iBs[l][j][k] -
1806 (int32) iOffset[l]
1807 ) * fscaling;
1808 }
1809
1810 // compute measurement vector elements fvecA[0-5] from fvecA[6-8]
1811 pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
1812 pthisMagCal->fvecA[1] = 2.0F *
1813 pthisMagCal->fvecA[6] *
1814 pthisMagCal->fvecA[7];
1815 pthisMagCal->fvecA[2] = 2.0F *
1816 pthisMagCal->fvecA[6] *
1817 pthisMagCal->fvecA[8];
1818 pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
1819 pthisMagCal->fvecA[4] = 2.0F *
1820 pthisMagCal->fvecA[7] *
1821 pthisMagCal->fvecA[8];
1822 pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
1823
1824 // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
1825 // with the exception of fmatA[9][9] which equals the number of measurements
1826 // update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
1827 for (m = 0; m < 9; m++)
1828 {
1829 pthisMagCal->fmatA[m][9] += pthisMagCal->fvecA[m];
1830 }
1831
1832 // update the on and above diagonal terms of fmatA ignoring right hand column 9
1833 for (m = 0; m < 9; m++)
1834 {
1835 for (n = m; n < 9; n++)
1836 {
1837 pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
1838 }
1839 }
1840
1841 // increment the measurement counter for the next iteration
1842 iCount++;
1843 }
1844 }
1845 }
1846
1847 // set the last element fmatA[9][9] to the number of measurements
1848 pthisMagCal->fmatA[9][9] = (float) iCount;
1849
1850 // store the number of measurements accumulated (defensive programming, should never be needed)
1851 pthisMagBuffer->iMagBufferCount = iCount;
1852
1853 // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
1854 for (m = 1; m < 10; m++)
1855 {
1856 for (n = 0; n < m; n++)
1857 {
1858 pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
1859 }
1860 }
1861
1862 // set pthisMagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
1863 fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1864 10);
1865
1866 // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
1867 j = 0;
1868 for (i = 1; i < 10; i++)
1869 {
1870 if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
1871 {
1872 j = i;
1873 }
1874 }
1875
1876 pthisMagCal->fA[0][0] = pthisMagCal->fmatB[0][j];
1877 pthisMagCal->fA[0][1] = pthisMagCal->fA[1][0] = pthisMagCal->fmatB[1][j];
1878 pthisMagCal->fA[0][2] = pthisMagCal->fA[2][0] = pthisMagCal->fmatB[2][j];
1879 pthisMagCal->fA[1][1] = pthisMagCal->fmatB[3][j];
1880 pthisMagCal->fA[1][2] = pthisMagCal->fA[2][1] = pthisMagCal->fmatB[4][j];
1881 pthisMagCal->fA[2][2] = pthisMagCal->fmatB[5][j];
1882
1883 // negate entire solution if A has negative determinant
1884 det = f3x3matrixDetA(pthisMagCal->fA);
1885 if (det < 0.0F)
1886 {
1887 f3x3matrixAeqMinusA(pthisMagCal->fA);
1888 pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
1889 pthisMagCal->fmatB[7][j] = -pthisMagCal->fmatB[7][j];
1890 pthisMagCal->fmatB[8][j] = -pthisMagCal->fmatB[8][j];
1891 pthisMagCal->fmatB[9][j] = -pthisMagCal->fmatB[9][j];
1892 det = -det;
1893 }
1894
1895 // compute the inverse of the ellipsoid matrix
1896 f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
1897
1898 // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
1899 for (l = CHX; l <= CHZ; l++)
1900 {
1901 pthisMagCal->ftrV[l] = 0.0F;
1902 for (m = CHX; m <= CHZ; m++)
1903 {
1904 pthisMagCal->ftrV[l] += pthisMagCal->finvA[l][m] * pthisMagCal->fmatB[m + 6][j];
1905 }
1906
1907 pthisMagCal->ftrV[l] *= -0.5F;
1908 }
1909
1910 // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
1911 pthisMagCal->ftrB = sqrtf(fabsf(pthisMagCal->fA[0][0] *
1912 pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHX] +
1913 2.0F * pthisMagCal->fA[0][1] *
1914 pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHY] +
1915 2.0F * pthisMagCal->fA[0][2] *
1916 pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHZ] +
1917 pthisMagCal->fA[1][1] * pthisMagCal->ftrV[CHY] *
1918 pthisMagCal->ftrV[CHY] + 2.0F *
1919 pthisMagCal->fA[1][2] * pthisMagCal->ftrV[CHY] *
1920 pthisMagCal->ftrV[CHZ] + pthisMagCal->fA[2][2] *
1921 pthisMagCal->ftrV[CHZ] * pthisMagCal->ftrV[CHZ] -
1922 pthisMagCal->fmatB[9][j]));
1923
1924 // calculate the trial normalized fit error as a percentage
1925 pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[j]) /
1926 (float) pthisMagBuffer->iMagBufferCount) / (pthisMagCal->ftrB * pthisMagCal->ftrB);
1927
1928 // correct for the measurement matrix offset and scaling and get the computed hard iron offset in uT
1929 for (l = CHX; l <= CHZ; l++)
1930 {
1931 pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1932 DEFAULTB +
1933 (float) iOffset[l] *
1934 pthisMag->fuTPerCount;
1935 }
1936
1937 // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A
1938 pthisMagCal->ftrB *= DEFAULTB;
1939
1940 // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor
1941 f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
1942 pthisMagCal->ftrB *= powf(det, -(ONESIXTH));
1943
1944 // compute trial invW from the square root of fA (both with normalized determinant)
1945 // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
1946 // where fmatA holds the 3x3 matrix fA in its top left elements
1947 for (i = 0; i < 3; i++)
1948 {
1949 for (j = 0; j < 3; j++)
1950 {
1951 pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
1952 }
1953 }
1954
1955 fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1956 3);
1957
1958 // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
1959 for (j = 0; j < 3; j++) // loop over columns j
1960 {
1961 ftmp = sqrtf(sqrtf(fabsf(pthisMagCal->fvecA[j])));
1962 for (i = 0; i < 3; i++) // loop over rows i
1963 {
1964 pthisMagCal->fmatB[i][j] *= ftmp;
1965 }
1966 }
1967
1968 // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
1969 // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
1970 // loop over rows
1971 for (i = 0; i < 3; i++)
1972 {
1973 // loop over on and above diagonal columns
1974 for (j = i; j < 3; j++)
1975 {
1976 pthisMagCal->ftrinvW[i][j] = 0.0F;
1977
1978 // accumulate the matrix product
1979 for (k = 0; k < 3; k++)
1980 {
1981 pthisMagCal->ftrinvW[i][j] += pthisMagCal->fmatB[i][k] * pthisMagCal->fmatB[j][k];
1982 }
1983
1984 // copy to below diagonal element
1985 pthisMagCal->ftrinvW[j][i] = pthisMagCal->ftrinvW[i][j];
1986 }
1987 }
1988
1989 return;
1990 }
1991 #endif
1992
