ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 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 }
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
Definition: matrix.c:182
void f3x3matrixAeqMinusA(float A[][3])
Definition: matrix.c:113
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
Definition: matrix.c:94
float f3x3matrixDetA(float A[][3])
Definition: matrix.c:169
#define CORRUPTMATRIX
Definition: matrix.c:33
Definition: matrix.h:33
signed char int8
Definition: basic_types.h:12
void fmatrixAeqI(float *A[], int16 rc)
Definition: matrix.c:55
#define NITERATIONS
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
Definition: matrix.c:134
Definition: matrix.h:34
short int16
This defines int16 as short.
Definition: isf_types.h:23
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
Definition: matrix.h:35
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:76
void fmatrixAeqRenormRotA(float A[][3])
Definition: matrix.c:458
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
Definition: matrix.c:325