CMSIS DSP Software Library: arm_mat_inverse_f32.c Source File

- Main Page
- Modules
- Data Structures
- Files
- Examples

- File List
- Globals

# arm_mat_inverse_f32.c

Go to the documentation of this file.

    00001 /* ----------------------------------------------------------------------
    00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
    00003 *
    00004 * $Date:        15. July 2011
    00005 * $Revision:    V1.0.10
    00006 *
    00007 * Project:      CMSIS DSP Library
    00008 * Title:        arm_mat_inverse_f32.c
    00009 *
    00010 * Description:  Floating-point matrix inverse.
    00011 *
    00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
    00013 *
    00014 * Version 1.0.10 2011/7/15
    00015 *    Big Endian support added and Merged M0 and M3/M4 Source code.
    00016 *
    00017 * Version 1.0.3 2010/11/29
    00018 *    Re-organized the CMSIS folders and updated documentation.
    00019 *
    00020 * Version 1.0.2 2010/11/11
    00021 *    Documentation updated.
    00022 *
    00023 * Version 1.0.1 2010/10/05
    00024 *    Production release and review comments incorporated.
    00025 *
    00026 * Version 1.0.0 2010/09/20
    00027 *    Production release and review comments incorporated.
    00028 * -------------------------------------------------------------------- */
    00029
    00030 #include "arm_math.h"
    00031
    00074 arm_status arm_mat_inverse_f32(
    00075   const arm_matrix_instance_f32 * pSrc,
    00076   arm_matrix_instance_f32 * pDst)
    00077 {
    00078   float32_t *pIn = pSrc->pData;                  /* input data matrix pointer */
    00079   float32_t *pOut = pDst->pData;                 /* output data matrix pointer */
    00080   float32_t *pInT1, *pInT2;                      /* Temporary input data matrix pointer */
    00081   float32_t *pInT3, *pInT4;                      /* Temporary output data matrix pointer */
    00082   float32_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst;  /* Temporary input and output data matrix pointer */
    00083   uint32_t numRows = pSrc->numRows;              /* Number of rows in the matrix  */
    00084   uint32_t numCols = pSrc->numCols;              /* Number of Cols in the matrix  */
    00085
    00086 #ifndef ARM_MATH_CM0
    00087
    00088   /* Run the below code for Cortex-M4 and Cortex-M3 */
    00089
    00090   float32_t Xchg, in = 0.0f, in1;                /* Temporary input values  */
    00091   uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l;      /* loop counters */
    00092   arm_status status;                             /* status of matrix inverse */
    00093
    00094 #ifdef ARM_MATH_MATRIX_CHECK
    00095
    00096
    00097   /* Check for matrix mismatch condition */
    00098   if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
    00099      || (pSrc->numRows != pDst->numRows))
    00100   {
    00101     /* Set status as ARM_MATH_SIZE_MISMATCH */
    00102     status = ARM_MATH_SIZE_MISMATCH;
    00103   }
    00104   else
    00105 #endif /*    #ifdef ARM_MATH_MATRIX_CHECK    */
    00106
    00107   {
    00108
    00109     /*--------------------------------------------------------------------------------------------------------------
    00110      * Matrix Inverse can be solved using elementary row operations.
    00111      *
    00112      *  Gauss-Jordan Method:
    00113      *
    00114      *     1. First combine the identity matrix and the input matrix separated by a bar to form an
    00115      *        augmented matrix as follows:
    00116      *                      _                  _         _         _
    00117      *                     |  a11  a12 | 1   0  |       |  X11 X12  |
    00118      *                     |           |        |   =   |           |
    00119      *                     |_ a21  a22 | 0   1 _|       |_ X21 X21 _|
    00120      *
    00121      *      2. In our implementation, pDst Matrix is used as identity matrix.
    00122      *
    00123      *      3. Begin with the first row. Let i = 1.
    00124      *
    00125      *      4. Check to see if the pivot for row i is zero.
    00126      *         The pivot is the element of the main diagonal that is on the current row.
    00127      *         For instance, if working with row i, then the pivot element is aii.
    00128      *         If the pivot is zero, exchange that row with a row below it that does not
    00129      *         contain a zero in column i. If this is not possible, then an inverse
    00130      *         to that matrix does not exist.
    00131      *
    00132      *      5. Divide every element of row i by the pivot.
    00133      *
    00134      *      6. For every row below and  row i, replace that row with the sum of that row and
    00135      *         a multiple of row i so that each new element in column i below row i is zero.
    00136      *
    00137      *      7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
    00138      *         for every element below and above the main diagonal.
    00139      *
    00140      *      8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
    00141      *         Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
    00142      *----------------------------------------------------------------------------------------------------------------*/
    00143
    00144     /* Working pointer for destination matrix */
    00145     pInT2 = pOut;
    00146
    00147     /* Loop over the number of rows */
    00148     rowCnt = numRows;
    00149
    00150     /* Making the destination matrix as identity matrix */
    00151     while(rowCnt > 0u)
    00152     {
    00153       /* Writing all zeroes in lower triangle of the destination matrix */
    00154       j = numRows - rowCnt;
    00155       while(j > 0u)
    00156       {
    00157         *pInT2++ = 0.0f;
    00158         j--;
    00159       }
    00160
    00161       /* Writing all ones in the diagonal of the destination matrix */
    00162       *pInT2++ = 1.0f;
    00163
    00164       /* Writing all zeroes in upper triangle of the destination matrix */
    00165       j = rowCnt - 1u;
    00166       while(j > 0u)
    00167       {
    00168         *pInT2++ = 0.0f;
    00169         j--;
    00170       }
    00171
    00172       /* Decrement the loop counter */
    00173       rowCnt--;
    00174     }
    00175
    00176     /* Loop over the number of columns of the input matrix.
    00177        All the elements in each column are processed by the row operations */
    00178     loopCnt = numCols;
    00179
    00180     /* Index modifier to navigate through the columns */
    00181     l = 0u;
    00182
    00183     while(loopCnt > 0u)
    00184     {
    00185       /* Check if the pivot element is zero..
    00186        * If it is zero then interchange the row with non zero row below.
    00187        * If there is no non zero element to replace in the rows below,
    00188        * then the matrix is Singular. */
    00189
    00190       /* Working pointer for the input matrix that points
    00191        * to the pivot element of the particular row  */
    00192       pInT1 = pIn + (l * numCols);
    00193
    00194       /* Working pointer for the destination matrix that points
    00195        * to the pivot element of the particular row  */
    00196       pInT3 = pOut + (l * numCols);
    00197
    00198       /* Temporary variable to hold the pivot value */
    00199       in = *pInT1;
    00200
    00201       /* Destination pointer modifier */
    00202       k = 1u;
    00203
    00204       /* Check if the pivot element is zero */
    00205       if(*pInT1 == 0.0f)
    00206       {
    00207         /* Loop over the number rows present below */
    00208         i = numRows - (l + 1u);
    00209
    00210         while(i > 0u)
    00211         {
    00212           /* Update the input and destination pointers */
    00213           pInT2 = pInT1 + (numCols * l);
    00214           pInT4 = pInT3 + (numCols * k);
    00215
    00216           /* Check if there is a non zero pivot element to
    00217            * replace in the rows below */
    00218           if(*pInT2 != 0.0f)
    00219           {
    00220             /* Loop over number of columns
    00221              * to the right of the pilot element */
    00222             j = numCols - l;
    00223
    00224             while(j > 0u)
    00225             {
    00226               /* Exchange the row elements of the input matrix */
    00227               Xchg = *pInT2;
    00228               *pInT2++ = *pInT1;
    00229               *pInT1++ = Xchg;
    00230
    00231               /* Decrement the loop counter */
    00232               j--;
    00233             }
    00234
    00235             /* Loop over number of columns of the destination matrix */
    00236             j = numCols;
    00237
    00238             while(j > 0u)
    00239             {
    00240               /* Exchange the row elements of the destination matrix */
    00241               Xchg = *pInT4;
    00242               *pInT4++ = *pInT3;
    00243               *pInT3++ = Xchg;
    00244
    00245               /* Decrement the loop counter */
    00246               j--;
    00247             }
    00248
    00249             /* Flag to indicate whether exchange is done or not */
    00250             flag = 1u;
    00251
    00252             /* Break after exchange is done */
    00253             break;
    00254           }
    00255
    00256           /* Update the destination pointer modifier */
    00257           k++;
    00258
    00259           /* Decrement the loop counter */
    00260           i--;
    00261         }
    00262       }
    00263
    00264       /* Update the status if the matrix is singular */
    00265       if((flag != 1u) && (in == 0.0f))
    00266       {
    00267         status = ARM_MATH_SINGULAR;
    00268
    00269         break;
    00270       }
    00271
    00272       /* Points to the pivot row of input and destination matrices */
    00273       pPivotRowIn = pIn + (l * numCols);
    00274       pPivotRowDst = pOut + (l * numCols);
    00275
    00276       /* Temporary pointers to the pivot row pointers */
    00277       pInT1 = pPivotRowIn;
    00278       pInT2 = pPivotRowDst;
    00279
    00280       /* Pivot element of the row */
    00281       in = *(pIn + (l * numCols));
    00282
    00283       /* Loop over number of columns
    00284        * to the right of the pilot element */
    00285       j = (numCols - l);
    00286
    00287       while(j > 0u)
    00288       {
    00289         /* Divide each element of the row of the input matrix
    00290          * by the pivot element */
    00291         in1 = *pInT1;
    00292         *pInT1++ = in1 / in;
    00293
    00294         /* Decrement the loop counter */
    00295         j--;
    00296       }
    00297
    00298       /* Loop over number of columns of the destination matrix */
    00299       j = numCols;
    00300
    00301       while(j > 0u)
    00302       {
    00303         /* Divide each element of the row of the destination matrix
    00304          * by the pivot element */
    00305         in1 = *pInT2;
    00306         *pInT2++ = in1 / in;
    00307
    00308         /* Decrement the loop counter */
    00309         j--;
    00310       }
    00311
    00312       /* Replace the rows with the sum of that row and a multiple of row i
    00313        * so that each new element in column i above row i is zero.*/
    00314
    00315       /* Temporary pointers for input and destination matrices */
    00316       pInT1 = pIn;
    00317       pInT2 = pOut;
    00318
    00319       /* index used to check for pivot element */
    00320       i = 0u;
    00321
    00322       /* Loop over number of rows */
    00323       /*  to be replaced by the sum of that row and a multiple of row i */
    00324       k = numRows;
    00325
    00326       while(k > 0u)
    00327       {
    00328         /* Check for the pivot element */
    00329         if(i == l)
    00330         {
    00331           /* If the processing element is the pivot element,
    00332              only the columns to the right are to be processed */
    00333           pInT1 += numCols - l;
    00334
    00335           pInT2 += numCols;
    00336         }
    00337         else
    00338         {
    00339           /* Element of the reference row */
    00340           in = *pInT1;
    00341
    00342           /* Working pointers for input and destination pivot rows */
    00343           pPRT_in = pPivotRowIn;
    00344           pPRT_pDst = pPivotRowDst;
    00345
    00346           /* Loop over the number of columns to the right of the pivot element,
    00347              to replace the elements in the input matrix */
    00348           j = (numCols - l);
    00349
    00350           while(j > 0u)
    00351           {
    00352             /* Replace the element by the sum of that row
    00353                and a multiple of the reference row  */
    00354             in1 = *pInT1;
    00355             *pInT1++ = in1 - (in * *pPRT_in++);
    00356
    00357             /* Decrement the loop counter */
    00358             j--;
    00359           }
    00360
    00361           /* Loop over the number of columns to
    00362              replace the elements in the destination matrix */
    00363           j = numCols;
    00364
    00365           while(j > 0u)
    00366           {
    00367             /* Replace the element by the sum of that row
    00368                and a multiple of the reference row  */
    00369             in1 = *pInT2;
    00370             *pInT2++ = in1 - (in * *pPRT_pDst++);
    00371
    00372             /* Decrement the loop counter */
    00373             j--;
    00374           }
    00375
    00376         }
    00377
    00378         /* Increment the temporary input pointer */
    00379         pInT1 = pInT1 + l;
    00380
    00381         /* Decrement the loop counter */
    00382         k--;
    00383
    00384         /* Increment the pivot index */
    00385         i++;
    00386       }
    00387
    00388       /* Increment the input pointer */
    00389       pIn++;
    00390
    00391       /* Decrement the loop counter */
    00392       loopCnt--;
    00393
    00394       /* Increment the index modifier */
    00395       l++;
    00396     }
    00397
    00398
    00399 #else
    00400
    00401   /* Run the below code for Cortex-M0 */
    00402
    00403   float32_t Xchg, in = 0.0f;                     /* Temporary input values  */
    00404   uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l;      /* loop counters */
    00405   arm_status status;                             /* status of matrix inverse */
    00406
    00407 #ifdef ARM_MATH_MATRIX_CHECK
    00408
    00409   /* Check for matrix mismatch condition */
    00410   if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
    00411      || (pSrc->numRows != pDst->numRows))
    00412   {
    00413     /* Set status as ARM_MATH_SIZE_MISMATCH */
    00414     status = ARM_MATH_SIZE_MISMATCH;
    00415   }
    00416   else
    00417 #endif /*      #ifdef ARM_MATH_MATRIX_CHECK    */
    00418   {
    00419
    00420     /*--------------------------------------------------------------------------------------------------------------
    00421      * Matrix Inverse can be solved using elementary row operations.
    00422      *
    00423      *  Gauss-Jordan Method:
    00424      *
    00425      *     1. First combine the identity matrix and the input matrix separated by a bar to form an
    00426      *        augmented matrix as follows:
    00427      *                      _  _          _     _      _   _         _         _
    00428      *                     |  |  a11  a12  | | | 1   0  |   |       |  X11 X12  |
    00429      *                     |  |            | | |        |   |   =   |           |
    00430      *                     |_ |_ a21  a22 _| | |_0   1 _|  _|       |_ X21 X21 _|
    00431      *
    00432      *      2. In our implementation, pDst Matrix is used as identity matrix.
    00433      *
    00434      *      3. Begin with the first row. Let i = 1.
    00435      *
    00436      *      4. Check to see if the pivot for row i is zero.
    00437      *         The pivot is the element of the main diagonal that is on the current row.
    00438      *         For instance, if working with row i, then the pivot element is aii.
    00439      *         If the pivot is zero, exchange that row with a row below it that does not
    00440      *         contain a zero in column i. If this is not possible, then an inverse
    00441      *         to that matrix does not exist.
    00442      *
    00443      *      5. Divide every element of row i by the pivot.
    00444      *
    00445      *      6. For every row below and  row i, replace that row with the sum of that row and
    00446      *         a multiple of row i so that each new element in column i below row i is zero.
    00447      *
    00448      *      7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
    00449      *         for every element below and above the main diagonal.
    00450      *
    00451      *      8. Now an identical matrix is formed to the left of the bar(input matrix, src).
    00452      *         Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
    00453      *----------------------------------------------------------------------------------------------------------------*/
    00454
    00455     /* Working pointer for destination matrix */
    00456     pInT2 = pOut;
    00457
    00458     /* Loop over the number of rows */
    00459     rowCnt = numRows;
    00460
    00461     /* Making the destination matrix as identity matrix */
    00462     while(rowCnt > 0u)
    00463     {
    00464       /* Writing all zeroes in lower triangle of the destination matrix */
    00465       j = numRows - rowCnt;
    00466       while(j > 0u)
    00467       {
    00468         *pInT2++ = 0.0f;
    00469         j--;
    00470       }
    00471
    00472       /* Writing all ones in the diagonal of the destination matrix */
    00473       *pInT2++ = 1.0f;
    00474
    00475       /* Writing all zeroes in upper triangle of the destination matrix */
    00476       j = rowCnt - 1u;
    00477       while(j > 0u)
    00478       {
    00479         *pInT2++ = 0.0f;
    00480         j--;
    00481       }
    00482
    00483       /* Decrement the loop counter */
    00484       rowCnt--;
    00485     }
    00486
    00487     /* Loop over the number of columns of the input matrix.
    00488        All the elements in each column are processed by the row operations */
    00489     loopCnt = numCols;
    00490
    00491     /* Index modifier to navigate through the columns */
    00492     l = 0u;
    00493     //for(loopCnt = 0u; loopCnt < numCols; loopCnt++)
    00494     while(loopCnt > 0u)
    00495     {
    00496       /* Check if the pivot element is zero..
    00497        * If it is zero then interchange the row with non zero row below.
    00498        * If there is no non zero element to replace in the rows below,
    00499        * then the matrix is Singular. */
    00500
    00501       /* Working pointer for the input matrix that points
    00502        * to the pivot element of the particular row  */
    00503       pInT1 = pIn + (l * numCols);
    00504
    00505       /* Working pointer for the destination matrix that points
    00506        * to the pivot element of the particular row  */
    00507       pInT3 = pOut + (l * numCols);
    00508
    00509       /* Temporary variable to hold the pivot value */
    00510       in = *pInT1;
    00511
    00512       /* Destination pointer modifier */
    00513       k = 1u;
    00514
    00515       /* Check if the pivot element is zero */
    00516       if(*pInT1 == 0.0f)
    00517       {
    00518         /* Loop over the number rows present below */
    00519         for (i = (l + 1u); i < numRows; i++)
    00520         {
    00521           /* Update the input and destination pointers */
    00522           pInT2 = pInT1 + (numCols * l);
    00523           pInT4 = pInT3 + (numCols * k);
    00524
    00525           /* Check if there is a non zero pivot element to
    00526            * replace in the rows below */
    00527           if(*pInT2 != 0.0f)
    00528           {
    00529             /* Loop over number of columns
    00530              * to the right of the pilot element */
    00531             for (j = 0u; j < (numCols - l); j++)
    00532             {
    00533               /* Exchange the row elements of the input matrix */
    00534               Xchg = *pInT2;
    00535               *pInT2++ = *pInT1;
    00536               *pInT1++ = Xchg;
    00537             }
    00538
    00539             for (j = 0u; j < numCols; j++)
    00540             {
    00541               Xchg = *pInT4;
    00542               *pInT4++ = *pInT3;
    00543               *pInT3++ = Xchg;
    00544             }
    00545
    00546             /* Flag to indicate whether exchange is done or not */
    00547             flag = 1u;
    00548
    00549             /* Break after exchange is done */
    00550             break;
    00551           }
    00552
    00553           /* Update the destination pointer modifier */
    00554           k++;
    00555         }
    00556       }
    00557
    00558       /* Update the status if the matrix is singular */
    00559       if((flag != 1u) && (in == 0.0f))
    00560       {
    00561         status = ARM_MATH_SINGULAR;
    00562
    00563         break;
    00564       }
    00565
    00566       /* Points to the pivot row of input and destination matrices */
    00567       pPivotRowIn = pIn + (l * numCols);
    00568       pPivotRowDst = pOut + (l * numCols);
    00569
    00570       /* Temporary pointers to the pivot row pointers */
    00571       pInT1 = pPivotRowIn;
    00572       pInT2 = pPivotRowDst;
    00573
    00574       /* Pivot element of the row */
    00575       in = *(pIn + (l * numCols));
    00576
    00577       /* Loop over number of columns
    00578        * to the right of the pilot element */
    00579       for (j = 0u; j < (numCols - l); j++)
    00580       {
    00581         /* Divide each element of the row of the input matrix
    00582          * by the pivot element */
    00583         *pInT1++ = *pInT1 / in;
    00584       }
    00585       for (j = 0u; j < numCols; j++)
    00586       {
    00587         /* Divide each element of the row of the destination matrix
    00588          * by the pivot element */
    00589         *pInT2++ = *pInT2 / in;
    00590       }
    00591
    00592       /* Replace the rows with the sum of that row and a multiple of row i
    00593        * so that each new element in column i above row i is zero.*/
    00594
    00595       /* Temporary pointers for input and destination matrices */
    00596       pInT1 = pIn;
    00597       pInT2 = pOut;
    00598
    00599       for (i = 0u; i < numRows; i++)
    00600       {
    00601         /* Check for the pivot element */
    00602         if(i == l)
    00603         {
    00604           /* If the processing element is the pivot element,
    00605              only the columns to the right are to be processed */
    00606           pInT1 += numCols - l;
    00607           pInT2 += numCols;
    00608         }
    00609         else
    00610         {
    00611           /* Element of the reference row */
    00612           in = *pInT1;
    00613
    00614           /* Working pointers for input and destination pivot rows */
    00615           pPRT_in = pPivotRowIn;
    00616           pPRT_pDst = pPivotRowDst;
    00617
    00618           /* Loop over the number of columns to the right of the pivot element,
    00619              to replace the elements in the input matrix */
    00620           for (j = 0u; j < (numCols - l); j++)
    00621           {
    00622             /* Replace the element by the sum of that row
    00623                and a multiple of the reference row  */
    00624             *pInT1++ = *pInT1 - (in * *pPRT_in++);
    00625           }
    00626           /* Loop over the number of columns to
    00627              replace the elements in the destination matrix */
    00628           for (j = 0u; j < numCols; j++)
    00629           {
    00630             /* Replace the element by the sum of that row
    00631                and a multiple of the reference row  */
    00632             *pInT2++ = *pInT2 - (in * *pPRT_pDst++);
    00633           }
    00634
    00635         }
    00636         /* Increment the temporary input pointer */
    00637         pInT1 = pInT1 + l;
    00638       }
    00639       /* Increment the input pointer */
    00640       pIn++;
    00641
    00642       /* Decrement the loop counter */
    00643       loopCnt--;
    00644       /* Increment the index modifier */
    00645       l++;
    00646     }
    00647
    00648
    00649 #endif /* #ifndef ARM_MATH_CM0 */
    00650
    00651     /* Set status as ARM_MATH_SUCCESS */
    00652     status = ARM_MATH_SUCCESS;
    00653
    00654     if((flag != 1u) && (in == 0.0f))
    00655     {
    00656       status = ARM_MATH_SINGULAR;
    00657     }
    00658   }
    00659   /* Return to application */
    00660   return (status);
    00661 }
    00662

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines