| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

matrix.c
Go to the documentation of this file.
1 // Copyright (c) 2015, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains matrix manipulation functions.
27 //
28
29 #include "math.h"
30 #include "matrix.h"
31
32 // compile time constants that are private to this file
33 #define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
34
35 // function sets the 3x3 matrix A to the identity matrix
36 void f3x3matrixAeqI(float A[][3])
37 {
38 float *pAij; // pointer to A[i][j]
39 int8 i, j; // loop counters
40
41 for (i = 0; i < 3; i++)
42 {
43 // set pAij to &A[i][j=0]
44 pAij = A[i];
45 for (j = 0; j < 3; j++)
46 {
47 *(pAij++) = 0.0F;
48 }
49 A[i][i] = 1.0F;
50 }
51 return;
52 }
53
54 // function sets 3x3 matrix A to 3x3 matrix B
55 void f3x3matrixAeqB(float A[][3], float B[][3])
56 {
57 float *pAij; // pointer to A[i][j]
58 float *pBij; // pointer to B[i][j]
59 int8 i, j; // loop counters
60
61 for (i = 0; i < 3; i++)
62 {
63 // set pAij to &A[i][j=0] and pBij to &B[i][j=0]
64 pAij = A[i];
65 pBij = B[i];
66 for (j = 0; j < 3; j++)
67 {
68 *(pAij++) = *(pBij++);
69 }
70 }
71 return;
72 }
73
74 // function sets the matrix A to the identity matrix
75 void fmatrixAeqI(float *A[], int16 rc)
76 {
77 // rc = rows and columns in A
78
79 float *pAij; // pointer to A[i][j]
80 int8 i, j; // loop counters
81
82 for (i = 0; i < rc; i++)
83 {
84 // set pAij to &A[i][j=0]
85 pAij = A[i];
86 for (j = 0; j < rc; j++)
87 {
88 *(pAij++) = 0.0F;
89 }
90 A[i][i] = 1.0F;
91 }
92 return;
93 }
94
95 // function sets every entry in the 3x3 matrix A to a constant scalar
96 void f3x3matrixAeqScalar(float A[][3], float Scalar)
97 {
98 float *pAij; // pointer to A[i][j]
99 int8 i, j; // counters
100
101 for (i = 0; i < 3; i++)
102 {
103 // set pAij to &A[i][j=0]
104 pAij = A[i];
105 for (j = 0; j < 3; j++)
106 {
107 *(pAij++) = Scalar;
108 }
109 }
110 return;
111 }
112
113 // function multiplies all elements of 3x3 matrix A by the specified scalar
114 void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
115 {
116 float *pAij; // pointer to A[i][j]
117 int8 i, j; // loop counters
118
119 for (i = 0; i < 3; i++)
120 {
121 // set pAij to &A[i][j=0]
122 pAij = A[i];
123 for (j = 0; j < 3; j++)
124 {
125 *(pAij++) *= Scalar;
126 }
127 }
128
129 return;
130 }
131
132 // function negates all elements of 3x3 matrix A
133 void f3x3matrixAeqMinusA(float A[][3])
134 {
135 float *pAij; // pointer to A[i][j]
136 int8 i, j; // loop counters
137
138 for (i = 0; i < 3; i++)
139 {
140 // set pAij to &A[i][j=0]
141 pAij = A[i];
142 for (j = 0; j < 3; j++)
143 {
144 *pAij = -*pAij;
145 pAij++;
146 }
147 }
148
149 return;
150 }
151
152 // function directly calculates the symmetric inverse of a symmetric 3x3 matrix
153 // only the on and above diagonal terms in B are used and need to be specified
154 void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
155 {
156 float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
157 float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
158 float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
159 float ftmp; // determinant and then reciprocal
160
161 // calculate useful products
162 fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
163 fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
164 fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
165
166 // set ftmp to the determinant of the input matrix B
167 ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
168
169 // set A to the inverse of B for any determinant except zero
170 if (ftmp != 0.0F)
171 {
172 ftmp = 1.0F / ftmp;
173 A[0][0] = fB11B22mB12B12 * ftmp;
174 A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
175 A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
176 A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
177 A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
178 A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
179 }
180 else
181 {
182 // provide the identity matrix if the determinant is zero
183 f3x3matrixAeqI(A);
184 }
185 return;
186 }
187
188 // function calculates the determinant of a 3x3 matrix
189 float f3x3matrixDetA(float A[][3])
190 {
191 return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
192 A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
193 A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
194 }
195
196 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
197 // stored in the top left of a 10x10 array A[10][10]
198 // A[][] is changed on output.
199 // eigval[0..n-1] returns the eigenvalues of A[][].
200 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
201 // the eigenvectors are not sorted by value
202 // n can vary up to and including 10 but the matrices A and eigvec must have 10 columns.
203 void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
204 {
205 // maximum number of iterations to achieve convergence: in practice 6 is typical
206 #define NITERATIONS 15
207
208 // various trig functions of the jacobi rotation angle phi
209 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
210 // scratch variable to prevent over-writing during rotations
211 float ftmp;
212 // residue from remaining non-zero above diagonal terms
213 float residue;
214 // matrix row and column indices
215 int8 ir, ic;
216 // general loop counter
217 int8 j;
218 // timeout ctr for number of passes of the algorithm
219 int8 ctr;
220
221 // initialize eigenvectors matrix and eigenvalues array
222 for (ir = 0; ir < n; ir++)
223 {
224 // loop over all columns
225 for (ic = 0; ic < n; ic++)
226 {
227 // set on diagonal and off-diagonal elements to zero
228 eigvec[ir][ic] = 0.0F;
229 }
230
231 // correct the diagonal elements to 1.0
232 eigvec[ir][ir] = 1.0F;
233
234 // initialize the array of eigenvalues to the diagonal elements of m
235 eigval[ir] = A[ir][ir];
236 }
237
238 // initialize the counter and loop until converged or NITERATIONS reached
239 ctr = 0;
240 do
241 {
242 // compute the absolute value of the above diagonal elements as exit criterion
243 residue = 0.0F;
244 // loop over rows excluding last row
245 for (ir = 0; ir < n - 1; ir++)
246 {
247 // loop over above diagonal columns
248 for (ic = ir + 1; ic < n; ic++)
249 {
250 // accumulate the residual off diagonal terms which are being driven to zero
251 residue += fabsf(A[ir][ic]);
252 }
253 }
254
255 // check if we still have work to do
256 if (residue > 0.0F)
257 {
258 // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
259 for (ir = 0; ir < n - 1; ir++)
260 {
261 // loop over columns ic (where ic is always greater than ir since above diagonal)
262 for (ic = ir + 1; ic < n; ic++)
263 {
264 // only continue with this element if the element is non-zero
265 if (fabsf(A[ir][ic]) > 0.0F)
266 {
267 // calculate cot(2*phi) where phi is the Jacobi rotation angle
268 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
269
270 // calculate tan(phi) correcting sign to ensure the smaller solution is used
271 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
272 if (cot2phi < 0.0F)
273 {
274 tanphi = -tanphi;
275 }
276
277 // calculate the sine and cosine of the Jacobi rotation angle phi
278 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
279 sinphi = tanphi * cosphi;
280
281 // calculate tan(phi/2)
282 tanhalfphi = sinphi / (1.0F + cosphi);
283
284 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
285 ftmp = tanphi * A[ir][ic];
286
287 // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
288 // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
289 eigval[ir] -= ftmp;
290 // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
291 eigval[ic] += ftmp;
292
293 // by definition, applying the jacobi rotation on element ir, ic results in 0.0
294 A[ir][ic] = 0.0F;
295
296 // apply the jacobi rotation to all elements of the eigenvector matrix
297 for (j = 0; j < n; j++)
298 {
299 // store eigvec[j][ir]
300 ftmp = eigvec[j][ir];
301 // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
302 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
303 // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
304 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
305 }
306
307 // apply the jacobi rotation only to those elements of matrix m that can change
308 for (j = 0; j <= ir - 1; j++)
309 {
310 // store A[j][ir]
311 ftmp = A[j][ir];
312 // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
313 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
314 // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
315 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
316 }
317 for (j = ir + 1; j <= ic - 1; j++)
318 {
319 // store A[ir][j]
320 ftmp = A[ir][j];
321 // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
322 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
323 // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
324 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
325 }
326 for (j = ic + 1; j < n; j++)
327 {
328 // store A[ir][j]
329 ftmp = A[ir][j];
330 // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
331 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
332 // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
333 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
334 }
335 } // end of test for matrix element already zero
336 } // end of loop over columns
337 } // end of loop over rows
338 } // end of test for non-zero residue
339 } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
340
341 return;
342 }
343
344 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
345 // stored in the top left of a 4x4 array A[4][4]
346 // A[][] is changed on output.
347 // eigval[0..n-1] returns the eigenvalues of A[][].
348 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
349 // the eigenvectors are not sorted by value
350 // n can vary up to and including 4 but the matrices A and eigvec must have 4 columns.
351 // this function is identical to eigencompute10 except for the workaround for 4x4 matrices since C cannot
352 // handle functions accepting matrices with variable numbers of columns.
353 void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
354 {
355 // maximum number of iterations to achieve convergence: in practice 6 is typical
356 #define NITERATIONS 15
357
358 // various trig functions of the jacobi rotation angle phi
359 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
360 // scratch variable to prevent over-writing during rotations
361 float ftmp;
362 // residue from remaining non-zero above diagonal terms
363 float residue;
364 // matrix row and column indices
365 int8 ir, ic;
366 // general loop counter
367 int8 j;
368 // timeout ctr for number of passes of the algorithm
369 int8 ctr;
370
371 // initialize eigenvectors matrix and eigenvalues array
372 for (ir = 0; ir < n; ir++)
373 {
374 // loop over all columns
375 for (ic = 0; ic < n; ic++)
376 {
377 // set on diagonal and off-diagonal elements to zero
378 eigvec[ir][ic] = 0.0F;
379 }
380
381 // correct the diagonal elements to 1.0
382 eigvec[ir][ir] = 1.0F;
383
384 // initialize the array of eigenvalues to the diagonal elements of m
385 eigval[ir] = A[ir][ir];
386 }
387
388 // initialize the counter and loop until converged or NITERATIONS reached
389 ctr = 0;
390 do
391 {
392 // compute the absolute value of the above diagonal elements as exit criterion
393 residue = 0.0F;
394 // loop over rows excluding last row
395 for (ir = 0; ir < n - 1; ir++)
396 {
397 // loop over above diagonal columns
398 for (ic = ir + 1; ic < n; ic++)
399 {
400 // accumulate the residual off diagonal terms which are being driven to zero
401 residue += fabsf(A[ir][ic]);
402 }
403 }
404
405 // check if we still have work to do
406 if (residue > 0.0F)
407 {
408 // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
409 for (ir = 0; ir < n - 1; ir++)
410 {
411 // loop over columns ic (where ic is always greater than ir since above diagonal)
412 for (ic = ir + 1; ic < n; ic++)
413 {
414 // only continue with this element if the element is non-zero
415 if (fabsf(A[ir][ic]) > 0.0F)
416 {
417 // calculate cot(2*phi) where phi is the Jacobi rotation angle
418 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
419
420 // calculate tan(phi) correcting sign to ensure the smaller solution is used
421 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
422 if (cot2phi < 0.0F)
423 {
424 tanphi = -tanphi;
425 }
426
427 // calculate the sine and cosine of the Jacobi rotation angle phi
428 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
429 sinphi = tanphi * cosphi;
430
431 // calculate tan(phi/2)
432 tanhalfphi = sinphi / (1.0F + cosphi);
433
434 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
435 ftmp = tanphi * A[ir][ic];
436
437 // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
438 // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
439 eigval[ir] -= ftmp;
440 // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
441 eigval[ic] += ftmp;
442
443 // by definition, applying the jacobi rotation on element ir, ic results in 0.0
444 A[ir][ic] = 0.0F;
445
446 // apply the jacobi rotation to all elements of the eigenvector matrix
447 for (j = 0; j < n; j++)
448 {
449 // store eigvec[j][ir]
450 ftmp = eigvec[j][ir];
451 // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
452 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
453 // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
454 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
455 }
456
457 // apply the jacobi rotation only to those elements of matrix m that can change
458 for (j = 0; j <= ir - 1; j++)
459 {
460 // store A[j][ir]
461 ftmp = A[j][ir];
462 // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
463 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
464 // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
465 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
466 }
467 for (j = ir + 1; j <= ic - 1; j++)
468 {
469 // store A[ir][j]
470 ftmp = A[ir][j];
471 // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
472 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
473 // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
474 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
475 }
476 for (j = ic + 1; j < n; j++)
477 {
478 // store A[ir][j]
479 ftmp = A[ir][j];
480 // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
481 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
482 // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
483 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
484 }
485 } // end of test for matrix element already zero
486 } // end of loop over columns
487 } // end of loop over rows
488 } // end of test for non-zero residue
489 } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
490
491 return;
492 }
493
494 // function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ
495 // on exit, A is replaced with its inverse
496 void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
497 {
498 float largest; // largest element used for pivoting
499 float scaling; // scaling factor in pivoting
500 float recippiv; // reciprocal of pivot element
501 float ftmp; // temporary variable used in swaps
502 int8 i, j, k, l, m; // index counters
503 int8 iPivotRow, iPivotCol; // row and column of pivot element
504
505 // to avoid compiler warnings
506 iPivotRow = iPivotCol = 0;
507
508 // default to successful inversion
509 *pierror = false;
510
511 // initialize the pivot array to 0
512 for (j = 0; j < isize; j++)
513 {
514 iPivot[j] = 0;
515 }
516
517 // main loop i over the dimensions of the square matrix A
518 for (i = 0; i < isize; i++)
519 {
520 // zero the largest element found for pivoting
521 largest = 0.0F;
522 // loop over candidate rows j
523 for (j = 0; j < isize; j++)
524 {
525 // check if row j has been previously pivoted
526 if (iPivot[j] != 1)
527 {
528 // loop over candidate columns k
529 for (k = 0; k < isize; k++)
530 {
531 // check if column k has previously been pivoted
532 if (iPivot[k] == 0)
533 {
534 // check if the pivot element is the largest found so far
535 if (fabsf(A[j][k]) >= largest)
536 {
537 // and store this location as the current best candidate for pivoting
538 iPivotRow = j;
539 iPivotCol = k;
540 largest = (float) fabsf(A[iPivotRow][iPivotCol]);
541 }
542 }
543 else if (iPivot[k] > 1)
544 {
545 // zero determinant situation: exit with identity matrix and set error flag
546 fmatrixAeqI(A, isize);
547 *pierror = true;
548 return;
549 }
550 }
551 }
552 }
553 // increment the entry in iPivot to denote it has been selected for pivoting
554 iPivot[iPivotCol]++;
555
556 // check the pivot rows iPivotRow and iPivotCol are not the same before swapping
557 if (iPivotRow != iPivotCol)
558 {
559 // loop over columns l
560 for (l = 0; l < isize; l++)
561 {
562 // and swap all elements of rows iPivotRow and iPivotCol
563 ftmp = A[iPivotRow][l];
564 A[iPivotRow][l] = A[iPivotCol][l];
565 A[iPivotCol][l] = ftmp;
566 }
567 }
568
569 // record that on the i-th iteration rows iPivotRow and iPivotCol were swapped
570 iRowInd[i] = iPivotRow;
571 iColInd[i] = iPivotCol;
572
573 // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
574 if (A[iPivotCol][iPivotCol] == 0.0F)
575 {
576 // zero determinant situation: exit with identity matrix and set error flag
577 fmatrixAeqI(A, isize);
578 *pierror = true;
579 return;
580 }
581
582 // calculate the reciprocal of the pivot element knowing it's non-zero
583 recippiv = 1.0F / A[iPivotCol][iPivotCol];
584 // by definition, the diagonal element normalizes to 1
585 A[iPivotCol][iPivotCol] = 1.0F;
586 // multiply all of row iPivotCol by the reciprocal of the pivot element including the diagonal element
587 // the diagonal element A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its previous value
588 for (l = 0; l < isize; l++)
589 {
590 if (A[iPivotCol][l] != 0.0F)
591 A[iPivotCol][l] *= recippiv;
592 }
593 // loop over all rows m of A
594 for (m = 0; m < isize; m++)
595 {
596 if (m != iPivotCol)
597 {
598 // scaling factor for this row m is in column iPivotCol
599 scaling = A[m][iPivotCol];
600 // zero this element
601 A[m][iPivotCol] = 0.0F;
602 // loop over all columns l of A and perform elimination
603 for (l = 0; l < isize; l++)
604 {
605 if ((A[iPivotCol][l] != 0.0F) && (scaling != 0.0F))
606 A[m][l] -= A[iPivotCol][l] * scaling;
607 }
608 }
609 }
610 } // end of loop i over the matrix dimensions
611
612 // finally, loop in inverse order to apply the missing column swaps
613 for (l = isize - 1; l >= 0; l--)
614 {
615 // set i and j to the two columns to be swapped
616 i = iRowInd[l];
617 j = iColInd[l];
618
619 // check that the two columns i and j to be swapped are not the same
620 if (i != j)
621 {
622 // loop over all rows k to swap columns i and j of A
623 for (k = 0; k < isize; k++)
624 {
625 ftmp = A[k][i];
626 A[k][i] = A[k][j];
627 A[k][j] = ftmp;
628 }
629 }
630 }
631
632 return;
633 }
634
635 // function re-orthonormalizes a 3x3 rotation matrix
636 void fmatrixAeqRenormRotA(float A[][3])
637 {
638 float ftmp; // scratch variable
639
640 // normalize the X column of the low pass filtered orientation matrix
641 ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
642 if (ftmp > CORRUPTMATRIX)
643 {
644 // normalize the x column vector
645 ftmp = 1.0F / ftmp;
646 A[X][X] *= ftmp;
647 A[Y][X] *= ftmp;
648 A[Z][X] *= ftmp;
649 }
650 else
651 {
652 // set x column vector to {1, 0, 0}
653 A[X][X] = 1.0F;
654 A[Y][X] = A[Z][X] = 0.0F;
655 }
656
657 // force the y column vector to be orthogonal to x using y = y-(x.y)x
658 ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
659 A[X][Y] -= ftmp * A[X][X];
660 A[Y][Y] -= ftmp * A[Y][X];
661 A[Z][Y] -= ftmp * A[Z][X];
662
663 // normalize the y column vector
664 ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
665 if (ftmp > CORRUPTMATRIX)
666 {
667 // normalize the y column vector
668 ftmp = 1.0F / ftmp;
669 A[X][Y] *= ftmp;
670 A[Y][Y] *= ftmp;
671 A[Z][Y] *= ftmp;
672 }
673 else
674 {
675 // set y column vector to {0, 1, 0}
676 A[Y][Y] = 1.0F;
677 A[X][Y] = A[Z][Y] = 0.0F;
678 }
679
680 // finally set the z column vector to x vector cross y vector (automatically normalized)
681 A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
682 A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
683 A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
684
685 return;
686 }
fmatrixAeqInvA
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
Definition: matrix.c:496
f3x3matrixAeqB
void f3x3matrixAeqB(float A[][3], float B[][3])
Definition: matrix.c:55
f3x3matrixAeqMinusA
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:133
f3x3matrixAeqAxScalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:114
X
Definition: matrix.h:38
f3x3matrixDetA
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:189
CORRUPTMATRIX
#define CORRUPTMATRIX
Definition: matrix.c:33
fmatrixAeqI
void fmatrixAeqI(float *A[], int16 rc)
Definition: matrix.c:75
NITERATIONS
#define NITERATIONS
int16
signed short int int16
Definition: isf_types.h:73
f3x3matrixAeqInvSymB
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:154
matrix.h
Z
Definition: matrix.h:40
Y
Definition: matrix.h:39
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
eigencompute4
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
Definition: matrix.c:353
f3x3matrixAeqScalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:96
fmatrixAeqRenormRotA
void fmatrixAeqRenormRotA(float A[][3])
Definition: matrix.c:636
eigencompute10
void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:203
int8
signed char int8
Definition: isf_types.h:72
- © Freescale Semiconductor, Inc. 2016. All Rights Reserved.