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 #include <stdio.h>
10 #include "sensor_fusion.h"
11 #include "fusion.h"
12
13 /*! \file precisionAccelerometer.c
14 \brief Implements accelerometer calibration routines
15 */
16
17 // function resets the accelerometer buffer and accelerometer calibration
fInitializeAccelCalibration(struct AccelCalibration * pthisAccelCal,struct AccelBuffer * pthisAccelBuffer,volatile int8 * AccelCalPacketOn)18 void fInitializeAccelCalibration(struct AccelCalibration *pthisAccelCal,
19 struct AccelBuffer *pthisAccelBuffer,
20 volatile int8 *AccelCalPacketOn)
21 {
22 float *pFlash; // pointer into flash memory
23 int8_t i,
24 j; // loop counters
25
26 // set flags to false to denote no precision accelerometer measurements
27 pthisAccelBuffer->iStoreFlags = 0;
28
29 // perform one transmission of the precision calibration (using invalid value MAX_ACCEL_CAL_ORIENTATIONS for calibration only)
30 *AccelCalPacketOn = MAX_ACCEL_CAL_ORIENTATIONS;
31
32 // check to see if the stored accelerometer calibration has been erased
33 // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
34 #ifndef SIMULATION
35 pFlash = (float *) (CALIBRATION_NVM_ADDR + ACCEL_NVM_OFFSET);
36 if (*((uint32 *) pFlash++) == 0x12345678)
37 {
38 // a precision accelerometer calibration is present in flash
39 // copy accelerometer calibration elements (21x float total 84 bytes) from flash to RAM
40 for (i = CHX; i <= CHZ; i++) pthisAccelCal->fV[i] = *(pFlash++);
41 for (i = CHX; i <= CHZ; i++)
42 for (j = CHX; j <= CHZ; j++)
43 pthisAccelCal->finvW[i][j] = *(pFlash++);
44 for (i = CHX; i <= CHZ; i++)
45 for (j = CHX; j <= CHZ; j++)
46 pthisAccelCal->fR0[i][j] = *(pFlash++);
47 }
48 else
49 {
50 #endif
51 // flash has been erased and no accelerometer calibration is present
52 // initialize the precision accelerometer calibration in RAM to null default
53 pthisAccelCal->fV[CHX] = pthisAccelCal->fV[CHY] = pthisAccelCal->fV[CHZ] = 0.0F;
54 f3x3matrixAeqI(pthisAccelCal->finvW);
55 f3x3matrixAeqI(pthisAccelCal->fR0);
56 #ifndef SIMULATION
57 }
58 #endif
59
60 // set current averaging location and counter to -1 for invalid
61 pthisAccelBuffer->iStoreLocation = pthisAccelBuffer->iStoreCounter = -1;
62
63 return;
64 }
65
fUpdateAccelBuffer(struct AccelCalibration * pthisAccelCal,struct AccelBuffer * pthisAccelBuffer,struct AccelSensor * pthisAccel,volatile int8 * AccelCalPacketOn)66 void fUpdateAccelBuffer(struct AccelCalibration *pthisAccelCal,
67 struct AccelBuffer *pthisAccelBuffer,
68 struct AccelSensor *pthisAccel, volatile int8 *AccelCalPacketOn)
69 {
70 int16_t i; // loop counter
71
72 // iStoreCounter > 0: precision measurements are still on-going
73 // iStoreCounter = 0: the precision measurement has just finished
74 // iStoreCounter = -1: no precision measurement is in progress
75 // check if a new precision measurement has started and zero sums if one has
76 if (pthisAccelBuffer->iStoreCounter == (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ))
77 {
78 for (i = CHX; i <= CHZ; i++) pthisAccelBuffer->fSumGs[i] = 0.0F;
79 }
80
81 // accumulate sum if averaging not yet complete
82 if (pthisAccelBuffer->iStoreCounter > 0)
83 {
84 for (i = CHX; i <= CHZ; i++)
85 pthisAccelBuffer->fSumGs[i] += pthisAccel->fGs[i];
86 pthisAccelBuffer->iStoreCounter--;
87 }
88
89 // check if the measurement accumulation is complete and, if so, store average and set packet transmit flag
90 if (pthisAccelBuffer->iStoreCounter == 0)
91 {
92 // store the measurement, set the relevant flag and decrement the counter down to -1 denoting completion
93 for (i = CHX; i <= CHZ; i++)
94 {
95 pthisAccelBuffer->fGsStored[pthisAccelBuffer->iStoreLocation][i] =
96 pthisAccelBuffer->fSumGs[i] /
97 (float) (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
98 }
99
100 pthisAccelBuffer->iStoreFlags |=
101 (
102 1 <<
103 pthisAccelBuffer->iStoreLocation
104 );
105 pthisAccelBuffer->iStoreCounter--;
106
107 // compute the new precision accelerometer calibration including rotation using all measurements
108 fRunAccelCalibration(pthisAccelCal, pthisAccelBuffer, pthisAccel);
109
110 // and make one packet transmission of this measurement with the new calibration
111 *AccelCalPacketOn = pthisAccelBuffer->iStoreLocation;
112 }
113
114 return;
115 }
116
117 // function maps the accelerometer data fGs (g) onto precision calibrated and de-rotated data fGc (g), iGc (counts)
fInvertAccelCal(struct AccelSensor * pthisAccel,struct AccelCalibration * pthisAccelCal)118 void fInvertAccelCal(struct AccelSensor *pthisAccel,
119 struct AccelCalibration *pthisAccelCal)
120 {
121 // local variables
122 float ftmp[3]; // temporary array
123 int8_t i; // loop counter
124
125 //subtract the offset vector fV (g): ftmp[]=fGs[]-V[]
126 for (i = CHX; i <= CHZ; i++)
127 ftmp[i] = pthisAccel->fGs[i] - pthisAccelCal->fV[i];
128
129 // apply the inverse rotation correction matrix finvW: fGc=inv(W)*(fGs[]-V[])
130 for (i = CHX; i <= CHZ; i++)
131 {
132 pthisAccel->fGc[i] = pthisAccelCal->finvW[i][CHX] *
133 ftmp[CHX] +
134 pthisAccelCal->finvW[i][CHY] *
135 ftmp[CHY] +
136 pthisAccelCal->finvW[i][CHZ] *
137 ftmp[CHZ];
138 }
139
140 // apply the inverse of the forward rotation matrix fR0: fGc=inv(R).inv(W)*(fGs[]-V[])
141 for (i = CHX; i <= CHZ; i++) ftmp[i] = pthisAccel->fGc[i];
142 for (i = CHX; i <= CHZ; i++)
143 {
144 pthisAccel->fGc[i] = pthisAccelCal->fR0[CHX][i] *
145 ftmp[CHX] +
146 pthisAccelCal->fR0[CHY][i] *
147 ftmp[CHY] +
148 pthisAccelCal->fR0[CHZ][i] *
149 ftmp[CHZ];
150 pthisAccel->iGc[i] = (int16_t) (pthisAccel->fGc[i] * pthisAccel->iCountsPerg);
151 }
152
153 return;
154 }
155
156 // function runs the precision accelerometer calibration
fRunAccelCalibration(struct AccelCalibration * pthisAccelCal,struct AccelBuffer * pthisAccelBuffer,struct AccelSensor * pthisAccel)157 void fRunAccelCalibration(struct AccelCalibration *pthisAccelCal,
158 struct AccelBuffer *pthisAccelBuffer,
159 struct AccelSensor *pthisAccel)
160 {
161 float fGc0[3]; // calibrated but not de-rotated measurement 0
162 uint8_t iMeasurements; // number of stored measurements
163 int8_t i; // loop counters
164
165 // calculate how many measurements are present in the accelerometer measurement buffer
166 iMeasurements = 0;
167 for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
168 {
169 if (pthisAccelBuffer->iStoreFlags & (1 << i)) iMeasurements++;
170 }
171
172 // perform the highest quality calibration possible given this number
173 if (iMeasurements >= 9)
174 {
175 // perform the 10 element calibration
176 fComputeAccelCalibration10(pthisAccelBuffer, pthisAccelCal, pthisAccel);
177 }
178 else if (iMeasurements >= 6)
179 {
180 // perform the 7 element calibration
181 fComputeAccelCalibration7(pthisAccelBuffer, pthisAccelCal, pthisAccel);
182 }
183 else if (iMeasurements >= 4)
184 {
185 // perform the 4 element calibration
186 fComputeAccelCalibration4(pthisAccelBuffer, pthisAccelCal, pthisAccel);
187 }
188
189 // calculate the rotation correction matrix to rotate calibrated measurement 0 to flat
190 if (pthisAccelBuffer->iStoreFlags & 1)
191 {
192 // apply offset and gain calibration but not rotation to measurement 0 (flat) if present
193 // set ftmpA3x1 = invW . (fGs - fV)
194 for (i = CHX; i <= CHZ; i++)
195 fGc0[i] = pthisAccelBuffer->fGsStored[0][i] - pthisAccelCal->fV[i];
196 fVeq3x3AxV(fGc0, pthisAccelCal->finvW);
197
198 // compute the new final rotation matrix if rotation 0 (flat) is present.
199 // multiplying by the transpose of this matrix therefore forces measurement zero to flat.
200 #if THISCOORDSYSTEM == NED
201 f3DOFTiltNED(pthisAccelCal->fR0, fGc0);
202 #endif
203 #if THISCOORDSYSTEM == ANDROID
204 f3DOFTiltAndroid(pthisAccelCal->fR0, fGc0);
205 #endif
206 #if THISCOORDSYSTEM == WIN8
207 f3DOFTiltWin8(pthisAccelCal->fR0, fGc0);
208 #endif
209 }
210
211 return;
212 }
213
214 // calculate the 4 element calibration from the available measurements
fComputeAccelCalibration4(struct AccelBuffer * pthisAccelBuffer,struct AccelCalibration * pthisAccelCal,struct AccelSensor * pthisAccel)215 void fComputeAccelCalibration4(struct AccelBuffer *pthisAccelBuffer,
216 struct AccelCalibration *pthisAccelCal,
217 struct AccelSensor *pthisAccel)
218 {
219 int32 i,
220 j; // loop counters
221 float ftmp; // scratch
222 int8_t ierror; // flag from matrix inversion
223
224 // working arrays for 4x4 matrix inversion
225 float *pfRows[4];
226 int8_t iColInd[4];
227 int8_t iRowInd[4];
228 int8_t iPivot[4];
229
230 // zero the 4x4 matrix XTX (in upper left of fmatA) and 4x1 vector XTY (in upper fvecA)
231 for (i = 0; i < 4; i++)
232 {
233 pthisAccelCal->fvecA[i] = 0.0F;
234 for (j = 0; j < 4; j++)
235 {
236 pthisAccelCal->fmatA[i][j] = 0.0F;
237 }
238 }
239
240 // accumulate fXTY (in fvecA) and fXTX4x4 (in fmatA)
241 for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
242 {
243 // accumulate vector X^T.Y if this entry is valid
244 if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
245 {
246 ftmp = pthisAccelBuffer->fGsStored[i][CHX] *
247 pthisAccelBuffer->fGsStored[i][CHX] +
248 pthisAccelBuffer->fGsStored[i][CHY] *
249 pthisAccelBuffer->fGsStored[i][CHY] +
250 pthisAccelBuffer->fGsStored[i][CHZ] *
251 pthisAccelBuffer->fGsStored[i][CHZ];
252 pthisAccelCal->fvecA[0] += pthisAccelBuffer->fGsStored[i][CHX] * ftmp;
253 pthisAccelCal->fvecA[1] += pthisAccelBuffer->fGsStored[i][CHY] * ftmp;
254 pthisAccelCal->fvecA[2] += pthisAccelBuffer->fGsStored[i][CHZ] * ftmp;
255 pthisAccelCal->fvecA[3] += ftmp;
256
257 // accumulate above diagonal terms of matrix X^T.X
258 pthisAccelCal->fmatA[CHX][CHX] += pthisAccelBuffer->fGsStored[i][
259 CHX] *
260 pthisAccelBuffer->fGsStored[i][CHX];
261 pthisAccelCal->fmatA[CHX][CHY] += pthisAccelBuffer->fGsStored[i][
262 CHX] *
263 pthisAccelBuffer->fGsStored[i][CHY];
264 pthisAccelCal->fmatA[CHX][CHZ] += pthisAccelBuffer->fGsStored[i][
265 CHX] *
266 pthisAccelBuffer->fGsStored[i][CHZ];
267 pthisAccelCal->fmatA[CHX][3] += pthisAccelBuffer->fGsStored[i][CHX];
268 pthisAccelCal->fmatA[CHY][CHY] += pthisAccelBuffer->fGsStored[i][
269 CHY] *
270 pthisAccelBuffer->fGsStored[i][CHY];
271 pthisAccelCal->fmatA[CHY][CHZ] += pthisAccelBuffer->fGsStored[i][
272 CHY] *
273 pthisAccelBuffer->fGsStored[i][CHZ];;
274 pthisAccelCal->fmatA[CHY][3] += pthisAccelBuffer->fGsStored[i][CHY];
275 pthisAccelCal->fmatA[CHZ][CHZ] += pthisAccelBuffer->fGsStored[i][
276 CHZ] *
277 pthisAccelBuffer->fGsStored[i][CHZ];;
278 pthisAccelCal->fmatA[CHZ][3] += pthisAccelBuffer->fGsStored[i][CHZ];
279 pthisAccelCal->fmatA[3][3] += 1.0F;
280 }
281 }
282
283 // copy above diagonal elements of fXTX4x4 = X^T.X to below diagonal elements
284 pthisAccelCal->fmatA[CHY][CHX] = pthisAccelCal->fmatA[CHX][CHY];
285 pthisAccelCal->fmatA[CHZ][CHX] = pthisAccelCal->fmatA[CHX][CHZ];
286 pthisAccelCal->fmatA[CHZ][CHY] = pthisAccelCal->fmatA[CHY][CHZ];
287 pthisAccelCal->fmatA[3][CHX] = pthisAccelCal->fmatA[CHX][3];
288 pthisAccelCal->fmatA[3][CHY] = pthisAccelCal->fmatA[CHY][3];
289 pthisAccelCal->fmatA[3][CHZ] = pthisAccelCal->fmatA[CHZ][3];
290
291 // calculate in situ inverse of X^T.X
292 for (i = 0; i < 4; i++)
293 {
294 pfRows[i] = pthisAccelCal->fmatA[i];
295 }
296
297 fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
298
299 // calculate the solution vector fvecB = inv(X^T.X).X^T.Y
300 for (i = 0; i < 4; i++)
301 {
302 pthisAccelCal->fvecB[i] = 0.0F;
303 for (j = 0; j < 4; j++)
304 {
305 pthisAccelCal->fvecB[i] += pthisAccelCal->fmatA[i][j] * pthisAccelCal->fvecA[j];
306 }
307 }
308
309 // extract the offset vector
310 pthisAccelCal->fV[CHX] = 0.5F * pthisAccelCal->fvecB[CHX];
311 pthisAccelCal->fV[CHY] = 0.5F * pthisAccelCal->fvecB[CHY];
312 pthisAccelCal->fV[CHZ] = 0.5F * pthisAccelCal->fvecB[CHZ];
313
314 // set ftmp to 1/W where W is the forward gain to fit the 1g sphere
315 ftmp = 1.0F / sqrtf(fabsf(pthisAccelCal->fvecB[3] + pthisAccelCal->fV[CHX] *
316 pthisAccelCal->fV[CHX] + pthisAccelCal->fV[CHY] *
317 pthisAccelCal->fV[CHY] + pthisAccelCal->fV[CHZ] *
318 pthisAccelCal->fV[CHZ]));
319
320 // copy the inverse gain 1/W to the inverse gain matrix
321 pthisAccelCal->finvW[CHX][CHY] = pthisAccelCal->finvW[CHY][CHX] = 0.0F;
322 pthisAccelCal->finvW[CHX][CHZ] = pthisAccelCal->finvW[CHZ][CHX] = 0.0F;
323 pthisAccelCal->finvW[CHY][CHZ] = pthisAccelCal->finvW[CHZ][CHY] = 0.0F;
324 pthisAccelCal->finvW[CHX][CHX] = pthisAccelCal->finvW[CHY][CHY] = pthisAccelCal->finvW[CHZ][CHZ] = ftmp;
325
326 return;
327 }
328
329 // calculate the 7 element calibration from the available measurements
fComputeAccelCalibration7(struct AccelBuffer * pthisAccelBuffer,struct AccelCalibration * pthisAccelCal,struct AccelSensor * pthisAccel)330 void fComputeAccelCalibration7(struct AccelBuffer *pthisAccelBuffer,
331 struct AccelCalibration *pthisAccelCal,
332 struct AccelSensor *pthisAccel)
333 {
334 int32 i,
335 j,
336 m,
337 n; // loop counters
338 float det; // matrix determinant
339 float fg0; // fitted local gravity magnitude
340
341 // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
342 for (i = 0; i < 7; i++)
343 for (j = i; j < 7; j++) pthisAccelCal->fmatA[i][j] = 0.0F;
344
345 // last entry of vector fvecA is always 1.0 so move assignment outside the loop
346 pthisAccelCal->fvecA[6] = 1.0F;
347
348 // loop over all orientations
349 for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
350 {
351 // accumulate fvecA if this entry is valid
352 if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
353 {
354 // compute the remaining members of the measurement vector fvecA
355 pthisAccelCal->fvecA[0] = pthisAccelBuffer->fGsStored[i][CHX] * pthisAccelBuffer->fGsStored[i][CHX];
356 pthisAccelCal->fvecA[1] = pthisAccelBuffer->fGsStored[i][CHY] * pthisAccelBuffer->fGsStored[i][CHY];
357 pthisAccelCal->fvecA[2] = pthisAccelBuffer->fGsStored[i][CHZ] * pthisAccelBuffer->fGsStored[i][CHZ];
358 pthisAccelCal->fvecA[3] = pthisAccelBuffer->fGsStored[i][CHX];
359 pthisAccelCal->fvecA[4] = pthisAccelBuffer->fGsStored[i][CHY];
360 pthisAccelCal->fvecA[5] = pthisAccelBuffer->fGsStored[i][CHZ];
361
362 // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
363 for (m = 0; m < 7; m++)
364 for (n = m; n < 7; n++)
365 pthisAccelCal->fmatA[m][n] += pthisAccelCal->fvecA[m] * pthisAccelCal->fvecA[n];
366 } // end of test for stored data
367 } // end of loop over orientations
368
369 // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
370 for (m = 1; m < 7; m++)
371 for (n = 0; n < m; n++)
372 pthisAccelCal->fmatA[m][n] = pthisAccelCal->fmatA[n][m];
373
374 // set fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
375 fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
376 pthisAccelCal->fmatB, 7);
377
378 // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
379 j = 0;
380 for (i = 1; i < 7; i++)
381 if (pthisAccelCal->fvecA[i] < pthisAccelCal->fvecA[j]) j = i;
382
383 // negate the entire solution vector if ellipsoid matrix has negative determinant
384 det = pthisAccelCal->fmatB[0][j] *
385 pthisAccelCal->fmatB[1][j] *
386 pthisAccelCal->fmatB[2][j];
387 if (det < 0.0F)
388 {
389 for (i = 0; i < 7; i++)
390 {
391 pthisAccelCal->fmatB[i][j] = -pthisAccelCal->fmatB[i][j];
392 }
393 }
394
395 // compute invW and V and fitted gravity g0 from solution vector j
396 f3x3matrixAeqScalar(pthisAccelCal->finvW, 0.0F);
397 pthisAccelCal->finvW[CHX][CHX] = sqrtf(fabsf(pthisAccelCal->fmatB[0][j]));
398 pthisAccelCal->finvW[CHY][CHY] = sqrtf(fabsf(pthisAccelCal->fmatB[1][j]));
399 pthisAccelCal->finvW[CHZ][CHZ] = sqrtf(fabsf(pthisAccelCal->fmatB[2][j]));
400 pthisAccelCal->fV[CHX] = -0.5F *
401 pthisAccelCal->fmatB[3][j] /
402 pthisAccelCal->fmatB[0][j];
403 pthisAccelCal->fV[CHY] = -0.5F *
404 pthisAccelCal->fmatB[4][j] /
405 pthisAccelCal->fmatB[1][j];
406 pthisAccelCal->fV[CHZ] = -0.5F *
407 pthisAccelCal->fmatB[5][j] /
408 pthisAccelCal->fmatB[2][j];
409 fg0 = sqrtf(fabsf(pthisAccelCal->fmatB[0][j] * pthisAccelCal->fV[CHX] *
410 pthisAccelCal->fV[CHX] + pthisAccelCal->fmatB[1][j] *
411 pthisAccelCal->fV[CHY] * pthisAccelCal->fV[CHY] +
412 pthisAccelCal->fmatB[2][j] * pthisAccelCal->fV[CHZ] *
413 pthisAccelCal->fV[CHZ] - pthisAccelCal->fmatB[6][j]));
414
415 // normalize invW to fit the 1g sphere
416 pthisAccelCal->finvW[CHX][CHX] /= fg0;
417 pthisAccelCal->finvW[CHY][CHY] /= fg0;
418 pthisAccelCal->finvW[CHZ][CHZ] /= fg0;
419
420 return;
421 }
422
423 // calculate the 10 element calibration from the available measurements
fComputeAccelCalibration10(struct AccelBuffer * pthisAccelBuffer,struct AccelCalibration * pthisAccelCal,struct AccelSensor * pthisAccel)424 void fComputeAccelCalibration10(struct AccelBuffer *pthisAccelBuffer,
425 struct AccelCalibration *pthisAccelCal,
426 struct AccelSensor *pthisAccel)
427 {
428 int32 i,
429 j,
430 k,
431 l,
432 m,
433 n; // loop counters
434 float det; // matrix determinant
435 float ftmp; // scratch
436 float fg0; // fitted local gravity magnitude
437
438 // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
439 for (i = 0; i < 10; i++)
440 for (j = i; j < 10; j++) pthisAccelCal->fmatA[i][j] = 0.0F;
441
442 // last entry of vector fvecA is always 1.0 so move assignment outside the loop
443 pthisAccelCal->fvecA[9] = 1.0F;
444
445 // loop over all orientations
446 for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
447 {
448 // accumulate fvecA if this entry is valid
449 if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
450 {
451 // compute the remaining members of the measurement vector fvecA
452 pthisAccelCal->fvecA[6] = pthisAccelBuffer->fGsStored[i][CHX];
453 pthisAccelCal->fvecA[7] = pthisAccelBuffer->fGsStored[i][CHY];
454 pthisAccelCal->fvecA[8] = pthisAccelBuffer->fGsStored[i][CHZ];
455 pthisAccelCal->fvecA[0] = pthisAccelCal->fvecA[6] * pthisAccelCal->fvecA[6];
456 pthisAccelCal->fvecA[1] = 2.0F *
457 pthisAccelCal->fvecA[6] *
458 pthisAccelCal->fvecA[7];
459 pthisAccelCal->fvecA[2] = 2.0F *
460 pthisAccelCal->fvecA[6] *
461 pthisAccelCal->fvecA[8];
462 pthisAccelCal->fvecA[3] = pthisAccelCal->fvecA[7] * pthisAccelCal->fvecA[7];
463 pthisAccelCal->fvecA[4] = 2.0F *
464 pthisAccelCal->fvecA[7] *
465 pthisAccelCal->fvecA[8];
466 pthisAccelCal->fvecA[5] = pthisAccelCal->fvecA[8] * pthisAccelCal->fvecA[8];
467
468 // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
469 for (m = 0; m < 10; m++)
470 {
471 for (n = m; n < 10; n++)
472 {
473 pthisAccelCal->fmatA[m][n] += pthisAccelCal->fvecA[m] * pthisAccelCal->fvecA[n];
474 }
475 }
476 } // end of test for stored data
477 } // end of loop over orientations
478
479 // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
480 for (m = 1; m < 10; m++)
481 for (n = 0; n < m; n++)
482 pthisAccelCal->fmatA[m][n] = pthisAccelCal->fmatA[n][m];
483
484 // set fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
485 fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
486 pthisAccelCal->fmatB, 10);
487
488 // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
489 j = 0;
490 for (i = 1; i < 10; i++)
491 {
492 if (pthisAccelCal->fvecA[i] < pthisAccelCal->fvecA[j])
493 {
494 j = i;
495 }
496 }
497
498 pthisAccelCal->fA[0][0] = pthisAccelCal->fmatB[0][j];
499 pthisAccelCal->fA[0][1] = pthisAccelCal->fA[1][0] = pthisAccelCal->fmatB[1][j];
500 pthisAccelCal->fA[0][2] = pthisAccelCal->fA[2][0] = pthisAccelCal->fmatB[2][j];
501 pthisAccelCal->fA[1][1] = pthisAccelCal->fmatB[3][j];
502 pthisAccelCal->fA[1][2] = pthisAccelCal->fA[2][1] = pthisAccelCal->fmatB[4][j];
503 pthisAccelCal->fA[2][2] = pthisAccelCal->fmatB[5][j];
504
505 // negate entire solution if A has negative determinant
506 det = f3x3matrixDetA(pthisAccelCal->fA);
507 if (det < 0.0F)
508 {
509 f3x3matrixAeqMinusA(pthisAccelCal->fA);
510 pthisAccelCal->fmatB[6][j] = -pthisAccelCal->fmatB[6][j];
511 pthisAccelCal->fmatB[7][j] = -pthisAccelCal->fmatB[7][j];
512 pthisAccelCal->fmatB[8][j] = -pthisAccelCal->fmatB[8][j];
513 pthisAccelCal->fmatB[9][j] = -pthisAccelCal->fmatB[9][j];
514 det = -det;
515 }
516
517 // compute the inverse of the ellipsoid matrix
518 f3x3matrixAeqInvSymB(pthisAccelCal->finvA, pthisAccelCal->fA);
519
520 // compute the offset vector V
521 for (l = CHX; l <= CHZ; l++)
522 {
523 pthisAccelCal->fV[l] = 0.0F;
524 for (m = CHX; m <= CHZ; m++)
525 {
526 pthisAccelCal->fV[l] += pthisAccelCal->finvA[l][m] * pthisAccelCal->fmatB[m + 6][j];
527 }
528
529 pthisAccelCal->fV[l] *= -0.5F;
530 }
531
532 // compute the local gravity fit to these calibration coefficients
533 fg0 = sqrtf(fabsf(pthisAccelCal->fA[0][0] * pthisAccelCal->fV[CHX] *
534 pthisAccelCal->fV[CHX] + 2.0F * pthisAccelCal->fA[0][1] *
535 pthisAccelCal->fV[CHX] * pthisAccelCal->fV[CHY] + 2.0F *
536 pthisAccelCal->fA[0][2] * pthisAccelCal->fV[CHX] *
537 pthisAccelCal->fV[CHZ] + pthisAccelCal->fA[1][1] *
538 pthisAccelCal->fV[CHY] * pthisAccelCal->fV[CHY] + 2.0F *
539 pthisAccelCal->fA[1][2] * pthisAccelCal->fV[CHY] *
540 pthisAccelCal->fV[CHZ] + pthisAccelCal->fA[2][2] *
541 pthisAccelCal->fV[CHZ] * pthisAccelCal->fV[CHZ] -
542 pthisAccelCal->fmatB[9][j]));
543
544 // compute trial invW from the square root of fA
545 // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
546 // where fmatA holds the 3x3 matrix fA in its top left elements
547 for (i = 0; i < 3; i++)
548 for (j = 0; j < 3; j++)
549 pthisAccelCal->fmatA[i][j] = pthisAccelCal->fA[i][j];
550 fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
551 pthisAccelCal->fmatB, 3);
552
553 // set fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
554 for (j = 0; j < 3; j++) // loop over columns j
555 {
556 ftmp = sqrtf(sqrtf(fabsf(pthisAccelCal->fvecA[j])));
557 for (i = 0; i < 3; i++) // loop over rows i
558 {
559 pthisAccelCal->fmatB[i][j] *= ftmp;
560 }
561 }
562
563 // set finvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
564 // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
565 // loop over rows
566 for (i = 0; i < 3; i++)
567 {
568 // loop over on and above diagonal columns
569 for (j = i; j < 3; j++)
570 {
571 pthisAccelCal->finvW[i][j] = 0.0F;
572
573 // accumulate the matrix product
574 for (k = 0; k < 3; k++)
575 {
576 pthisAccelCal->finvW[i][j] += pthisAccelCal->fmatB[i][k] * pthisAccelCal->fmatB[j][k];
577 }
578
579 // copy to below diagonal element
580 pthisAccelCal->finvW[j][i] = pthisAccelCal->finvW[i][j];
581 }
582 }
583
584 // scale finvW so that the calibrated measurements fit on the 1g sphere
585 for (i = CHX; i <= CHZ; i++)
586 {
587 for (j = CHX; j <= CHZ; j++)
588 {
589 pthisAccelCal->finvW[i][j] /= fg0;
590 }
591 }
592
593 return;
594 }
595
