| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
matrix.c
Go to the documentation of this file.
1 // Copyright (c) 2014, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains 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 the matrix A to the identity matrix
55 void fmatrixAeqI(float *A[], int16 rc)
56 {
57 // rc = rows and columns in A
58
59 float *pAij; // pointer to A[i][j]
60 int8 i, j; // loop counters
61
62 for (i = 0; i < rc; i++)
63 {
64 // set pAij to &A[i][j=0]
65 pAij = A[i];
66 for (j = 0; j < rc; j++)
67 {
68 *(pAij++) = 0.0F;
69 }
70 A[i][i] = 1.0F;
71 }
72 return;
73 }
74
75 // function sets every entry in the 3x3 matrix A to a constant scalar
76 void f3x3matrixAeqScalar(float A[][3], float Scalar)
77 {
78 float *pAij; // pointer to A[i][j]
79 int8 i, j; // counters
80
81 for (i = 0; i < 3; i++)
82 {
83 // set pAij to &A[i][j=0]
84 pAij = A[i];
85 for (j = 0; j < 3; j++)
86 {
87 *(pAij++) = Scalar;
88 }
89 }
90 return;
91 }
92
93 // function multiplies all elements of 3x3 matrix A by the specified scalar
94 void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
95 {
96 float *pAij; // pointer to A[i][j]
97 int8 i, j; // loop counters
98
99 for (i = 0; i < 3; i++)
100 {
101 // set pAij to &A[i][j=0]
102 pAij = A[i];
103 for (j = 0; j < 3; j++)
104 {
105 *(pAij++) *= Scalar;
106 }
107 }
108
109 return;
110 }
111
112 // function negates all elements of 3x3 matrix A
113 void f3x3matrixAeqMinusA(float A[][3])
114 {
115 float *pAij; // pointer to A[i][j]
116 int8 i, j; // loop counters
117
118 for (i = 0; i < 3; i++)
119 {
120 // set pAij to &A[i][j=0]
121 pAij = A[i];
122 for (j = 0; j < 3; j++)
123 {
124 *pAij = -*pAij;
125 pAij++;
126 }
127 }
128
129 return;
130 }
131
132 // function directly calculates the symmetric inverse of a symmetric 3x3 matrix
133 // only the on and above diagonal terms in B are used and need to be specified
134 void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
135 {
136 float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
137 float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
138 float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
139 float ftmp; // determinant and then reciprocal
140
141 // calculate useful products
142 fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
143 fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
144 fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
145
146 // set ftmp to the determinant of the input matrix B
147 ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
148
149 // set A to the inverse of B for any determinant except zero
150 if (ftmp != 0.0F)
151 {
152 ftmp = 1.0F / ftmp;
153 A[0][0] = fB11B22mB12B12 * ftmp;
154 A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
155 A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
156 A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
157 A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
158 A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
159 }
160 else
161 {
162 // provide the identity matrix if the determinant is zero
163 f3x3matrixAeqI(A);
164 }
165 return;
166 }
167
168 // function calculates the determinant of a 3x3 matrix
169 float f3x3matrixDetA(float A[][3])
170 {
171 return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
172 A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
173 A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
174 }
175
176 // function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1]
177 // stored in the top left of a 10x10 array A[10][10]
178 // A[][] is changed on output.
179 // eigval[0..n-1] returns the eigenvalues of A[][].
180 // eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
181 // the eigenvectors are not sorted by value
182 void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
183 {
184 // maximum number of iterations to achieve convergence: in practice 6 is typical
185 #define NITERATIONS 15
186
187 // various trig functions of the jacobi rotation angle phi
188 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
189 // scratch variable to prevent over-writing during rotations
190 float ftmp;
191 // residue from remaining non-zero above diagonal terms
192 float residue;
193 // matrix row and column indices
194 int8 ir, ic;
195 // general loop counter
196 int8 j;
197 // timeout ctr for number of passes of the algorithm
198 int8 ctr;
199
200 // initialize eigenvectors matrix and eigenvalues array
201 for (ir = 0; ir < n; ir++)
202 {
203 // loop over all columns
204 for (ic = 0; ic < n; ic++)
205 {
206 // set on diagonal and off-diagonal elements to zero
207 eigvec[ir][ic] = 0.0F;
208 }
209
210 // correct the diagonal elements to 1.0
211 eigvec[ir][ir] = 1.0F;
212
213 // initialize the array of eigenvalues to the diagonal elements of m
214 eigval[ir] = A[ir][ir];
215 }
216
217 // initialize the counter and loop until converged or NITERATIONS reached
218 ctr = 0;
219 do
220 {
221 // compute the absolute value of the above diagonal elements as exit criterion
222 residue = 0.0F;
223 // loop over rows excluding last row
224 for (ir = 0; ir < n - 1; ir++)
225 {
226 // loop over above diagonal columns
227 for (ic = ir + 1; ic < n; ic++)
228 {
229 // accumulate the residual off diagonal terms which are being driven to zero
230 residue += fabs(A[ir][ic]);
231 }
232 }
233
234 // check if we still have work to do
235 if (residue > 0.0F)
236 {
237 // loop over all rows with the exception of the last row (since only rotating above diagonal elements)
238 for (ir = 0; ir < n - 1; ir++)
239 {
240 // loop over columns ic (where ic is always greater than ir since above diagonal)
241 for (ic = ir + 1; ic < n; ic++)
242 {
243 // only continue with this element if the element is non-zero
244 if (fabs(A[ir][ic]) > 0.0F)
245 {
246 // calculate cot(2*phi) where phi is the Jacobi rotation angle
247 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
248
249 // calculate tan(phi) correcting sign to ensure the smaller solution is used
250 tanphi = 1.0F / (fabs(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
251 if (cot2phi < 0.0F)
252 {
253 tanphi = -tanphi;
254 }
255
256 // calculate the sine and cosine of the Jacobi rotation angle phi
257 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
258 sinphi = tanphi * cosphi;
259
260 // calculate tan(phi/2)
261 tanhalfphi = sinphi / (1.0F + cosphi);
262
263 // set tmp = tan(phi) times current matrix element used in update of leading diagonal elements
264 ftmp = tanphi * A[ir][ic];
265
266 // apply the jacobi rotation to diagonal elements [ir][ir] and [ic][ic] stored in the eigenvalue array
267 // eigval[ir] = eigval[ir] - tan(phi) * A[ir][ic]
268 eigval[ir] -= ftmp;
269 // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
270 eigval[ic] += ftmp;
271
272 // by definition, applying the jacobi rotation on element ir, ic results in 0.0
273 A[ir][ic] = 0.0F;
274
275 // apply the jacobi rotation to all elements of the eigenvector matrix
276 for (j = 0; j < n; j++)
277 {
278 // store eigvec[j][ir]
279 ftmp = eigvec[j][ir];
280 // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] + tan(phi/2) * eigvec[j][ir])
281 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
282 // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] - tan(phi/2) * eigvec[j][ic])
283 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
284 }
285
286 // apply the jacobi rotation only to those elements of matrix m that can change
287 for (j = 0; j <= ir - 1; j++)
288 {
289 // store A[j][ir]
290 ftmp = A[j][ir];
291 // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) * A[j][ir])
292 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
293 // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) * A[j][ic])
294 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
295 }
296 for (j = ir + 1; j <= ic - 1; j++)
297 {
298 // store A[ir][j]
299 ftmp = A[ir][j];
300 // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) * A[ir][j])
301 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
302 // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) * A[j][ic])
303 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
304 }
305 for (j = ic + 1; j < n; j++)
306 {
307 // store A[ir][j]
308 ftmp = A[ir][j];
309 // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) * A[ir][j])
310 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
311 // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) * A[ic][j])
312 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
313 }
314 } // end of test for matrix element already zero
315 } // end of loop over columns
316 } // end of loop over rows
317 } // end of test for non-zero residue
318 } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
319
320 return;
321 }
322
323 // function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ
324 // on exit, A is replaced with its inverse
325 void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
326 {
327 float largest; // largest element used for pivoting
328 float scaling; // scaling factor in pivoting
329 float recippiv; // reciprocal of pivot element
330 float ftmp; // temporary variable used in swaps
331 int8 i, j, k, l, m; // index counters
332 int8 iPivotRow, iPivotCol; // row and column of pivot element
333
334 // to avoid compiler warnings
335 iPivotRow = iPivotCol = 0;
336
337 // initialize the pivot array to 0
338 for (j = 0; j < isize; j++)
339 {
340 iPivot[j] = 0;
341 }
342
343 // main loop i over the dimensions of the square matrix A
344 for (i = 0; i < isize; i++)
345 {
346 // zero the largest element found for pivoting
347 largest = 0.0F;
348 // loop over candidate rows j
349 for (j = 0; j < isize; j++)
350 {
351 // check if row j has been previously pivoted
352 if (iPivot[j] != 1)
353 {
354 // loop over candidate columns k
355 for (k = 0; k < isize; k++)
356 {
357 // check if column k has previously been pivoted
358 if (iPivot[k] == 0)
359 {
360 // check if the pivot element is the largest found so far
361 if (fabs(A[j][k]) >= largest)
362 {
363 // and store this location as the current best candidate for pivoting
364 iPivotRow = j;
365 iPivotCol = k;
366 largest = (float) fabs(A[iPivotRow][iPivotCol]);
367 }
368 }
369 else if (iPivot[k] > 1)
370 {
371 // zero determinant situation: exit with identity matrix
372 fmatrixAeqI(A, isize);
373 return;
374 }
375 }
376 }
377 }
378 // increment the entry in iPivot to denote it has been selected for pivoting
379 iPivot[iPivotCol]++;
380
381 // check the pivot rows iPivotRow and iPivotCol are not the same before swapping
382 if (iPivotRow != iPivotCol)
383 {
384 // loop over columns l
385 for (l = 0; l < isize; l++)
386 {
387 // and swap all elements of rows iPivotRow and iPivotCol
388 ftmp = A[iPivotRow][l];
389 A[iPivotRow][l] = A[iPivotCol][l];
390 A[iPivotCol][l] = ftmp;
391 }
392 }
393
394 // record that on the i-th iteration rows iPivotRow and iPivotCol were swapped
395 iRowInd[i] = iPivotRow;
396 iColInd[i] = iPivotCol;
397
398 // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
399 if (A[iPivotCol][iPivotCol] == 0.0F)
400 {
401 // zero determinant situation: exit with identity matrix
402 fmatrixAeqI(A, isize);
403 return;
404 }
405
406 // calculate the reciprocal of the pivot element knowing it's non-zero
407 recippiv = 1.0F / A[iPivotCol][iPivotCol];
408 // by definition, the diagonal element normalizes to 1
409 A[iPivotCol][iPivotCol] = 1.0F;
410 // multiply all of row iPivotCol by the reciprocal of the pivot element including the diagonal element
411 // the diagonal element A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its previous value
412 for (l = 0; l < isize; l++)
413 {
414 A[iPivotCol][l] *= recippiv;
415 }
416 // loop over all rows m of A
417 for (m = 0; m < isize; m++)
418 {
419 if (m != iPivotCol)
420 {
421 // scaling factor for this row m is in column iPivotCol
422 scaling = A[m][iPivotCol];
423 // zero this element
424 A[m][iPivotCol] = 0.0F;
425 // loop over all columns l of A and perform elimination
426 for (l = 0; l < isize; l++)
427 {
428 A[m][l] -= A[iPivotCol][l] * scaling;
429 }
430 }
431 }
432 } // end of loop i over the matrix dimensions
433
434 // finally, loop in inverse order to apply the missing column swaps
435 for (l = isize - 1; l >= 0; l--)
436 {
437 // set i and j to the two columns to be swapped
438 i = iRowInd[l];
439 j = iColInd[l];
440
441 // check that the two columns i and j to be swapped are not the same
442 if (i != j)
443 {
444 // loop over all rows k to swap columns i and j of A
445 for (k = 0; k < isize; k++)
446 {
447 ftmp = A[k][i];
448 A[k][i] = A[k][j];
449 A[k][j] = ftmp;
450 }
451 }
452 }
453
454 return;
455 }
456
457 // function re-orthonormalizes a 3x3 rotation matrix
458 void fmatrixAeqRenormRotA(float A[][3])
459 {
460 float ftmp; // scratch variable
461
462 // normalize the X column of the low pass filtered orientation matrix
463 ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
464 if (ftmp > CORRUPTMATRIX)
465 {
466 // normalize the x column vector
467 ftmp = 1.0F / ftmp;
468 A[X][X] *= ftmp;
469 A[Y][X] *= ftmp;
470 A[Z][X] *= ftmp;
471 }
472 else
473 {
474 // set x column vector to {1, 0, 0}
475 A[X][X] = 1.0F;
476 A[Y][X] = A[Z][X] = 0.0F;
477 }
478
479 // force the y column vector to be orthogonal to x using y = y-(x.y)x
480 ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
481 A[X][Y] -= ftmp * A[X][X];
482 A[Y][Y] -= ftmp * A[Y][X];
483 A[Z][Y] -= ftmp * A[Z][X];
484
485 // normalize the y column vector
486 ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
487 if (ftmp > CORRUPTMATRIX)
488 {
489 // normalize the y column vector
490 ftmp = 1.0F / ftmp;
491 A[X][Y] *= ftmp;
492 A[Y][Y] *= ftmp;
493 A[Z][Y] *= ftmp;
494 }
495 else
496 {
497 // set y column vector to {0, 1, 0}
498 A[Y][Y] = 1.0F;
499 A[X][Y] = A[Z][Y] = 0.0F;
500 }
501
502 // finally set the z column vector to x vector cross y vector (automatically normalized)
503 A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
504 A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
505 A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
506
507 return;
508 }
eigencompute
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:182
f3x3matrixAeqMinusA
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:113
f3x3matrixAeqAxScalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:94
f3x3matrixDetA
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:169
CORRUPTMATRIX
#define CORRUPTMATRIX
Definition: matrix.c:33
X
Definition: matrix.h:33
int8
signed char int8
Definition: basic_types.h:12
fmatrixAeqI
void fmatrixAeqI(float *A[], int16 rc)
Definition: matrix.c:55
NITERATIONS
#define NITERATIONS
f3x3matrixAeqInvSymB
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:134
matrix.h
Y
Definition: matrix.h:34
int16
short int16
This defines int16 as short.
Definition: isf_types.h:23
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
Z
Definition: matrix.h:35
f3x3matrixAeqScalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:76
fmatrixAeqRenormRotA
void fmatrixAeqRenormRotA(float A[][3])
Definition: matrix.c:458
fmatrixAeqInvA
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325
- © Freescale Semiconductor, Inc. 2015. All Rights Reserved.