arm_mat_inverse_f32.c

00001 /* ----------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
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
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated.  
00025 * -------------------------------------------------------------------- */ 
00026  
00027 #include "arm_math.h" 
00028  
00029 /**  
00030  * @ingroup groupMatrix  
00031  */ 
00032  
00033 /**  
00034  * @defgroup MatrixInv Matrix Inverse  
00035  *  
00036  * Computes the inverse of a matrix.  
00037  *  
00038  * The inverse is defined only if the input matrix is square and non-singular (the determinant  
00039  * is non-zero). The function checks that the input and output matrices are square and of the  
00040  * same size.  
00041  *  
00042  * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix  
00043  * inversion of floating-point matrices.  
00044  *  
00045  * \par Algorithm  
00046  * The Gauss-Jordan method is used to find the inverse.  
00047  * The algorithm performs a sequence of elementary row-operations till it  
00048  * reduces the input matrix to an identity matrix. Applying the same sequence  
00049  * of elementary row-operations to an identity matrix yields the inverse matrix.  
00050  * If the input matrix is singular, then the algorithm terminates and returns error status  
00051  * <code>ARM_MATH_SINGULAR</code>.  
00052  * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method"  
00053  */ 
00054  
00055 /**  
00056  * @addtogroup MatrixInv  
00057  * @{  
00058  */ 
00059  
00060 /**  
00061  * @brief Floating-point matrix inverse.  
00062  * @param[in]       *pSrc points to input matrix structure  
00063  * @param[out]      *pDst points to output matrix structure  
00064  * @return          The function returns  
00065  * <code>ARM_MATH_SIZE_MISMATCH</code> if the input matrix is not square or if the size  
00066  * of the output matrix does not match the size of the input matrix.  
00067  * If the input matrix is found to be singular (non-invertible), then the function returns  
00068  * <code>ARM_MATH_SINGULAR</code>.  Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>.  
00069  */ 
00070  
00071 arm_status arm_mat_inverse_f32( 
00072   const arm_matrix_instance_f32 * pSrc, 
00073   arm_matrix_instance_f32 * pDst) 
00074 { 
00075   float32_t *pIn = pSrc->pData;                  /* input data matrix pointer */ 
00076   float32_t *pOut = pDst->pData;                 /* output data matrix pointer */ 
00077   float32_t *pInT1, *pInT2;                      /* Temporary input data matrix pointer */ 
00078   float32_t *pInT3, *pInT4;                      /* Temporary output data matrix pointer */ 
00079   float32_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst;  /* Temporary input and output data matrix pointer */ 
00080   uint32_t numRows = pSrc->numRows;              /* Number of rows in the matrix  */ 
00081   uint32_t numCols = pSrc->numCols;              /* Number of Cols in the matrix  */ 
00082   float32_t Xchg, in = 0.0f, in1;                /* Temporary input values  */ 
00083   uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l;      /* loop counters */ 
00084   arm_status status;                             /* status of matrix inverse */ 
00085  
00086 #ifdef ARM_MATH_MATRIX_CHECK 
00087   /* Check for matrix mismatch condition */ 
00088   if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) 
00089      || (pSrc->numRows != pDst->numRows)) 
00090   { 
00091     /* Set status as ARM_MATH_SIZE_MISMATCH */ 
00092     status = ARM_MATH_SIZE_MISMATCH; 
00093   } 
00094   else 
00095 #endif 
00096   { 
00097  
00098     /*--------------------------------------------------------------------------------------------------------------  
00099      * Matrix Inverse can be solved using elementary row operations.  
00100      *  
00101      *  Gauss-Jordan Method:  
00102      *  
00103      *     1. First combine the identity matrix and the input matrix separated by a bar to form an  
00104      *        augmented matrix as follows:  
00105      *                      _                  _         _         _  
00106      *                     |  a11  a12 | 1   0  |       |  X11 X12  |  
00107      *                     |           |        |   =   |           |  
00108      *                     |_ a21  a22 | 0   1 _|       |_ X21 X21 _|  
00109      *  
00110      *      2. In our implementation, pDst Matrix is used as identity matrix.  
00111      *  
00112      *      3. Begin with the first row. Let i = 1.  
00113      *  
00114      *      4. Check to see if the pivot for row i is zero.  
00115      *         The pivot is the element of the main diagonal that is on the current row.  
00116      *         For instance, if working with row i, then the pivot element is aii.  
00117      *         If the pivot is zero, exchange that row with a row below it that does not  
00118      *         contain a zero in column i. If this is not possible, then an inverse  
00119      *         to that matrix does not exist.  
00120      *  
00121      *      5. Divide every element of row i by the pivot.  
00122      *  
00123      *      6. For every row below and  row i, replace that row with the sum of that row and  
00124      *         a multiple of row i so that each new element in column i below row i is zero.  
00125      *  
00126      *      7. Move to the next row and column and repeat steps 2 through 5 until you have zeros  
00127      *         for every element below and above the main diagonal.  
00128      *  
00129      *      8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).  
00130      *         Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).  
00131      *----------------------------------------------------------------------------------------------------------------*/ 
00132  
00133     /* Working pointer for destination matrix */ 
00134     pInT2 = pOut; 
00135  
00136     /* Loop over the number of rows */ 
00137     rowCnt = numRows; 
00138  
00139     /* Making the destination matrix as identity matrix */ 
00140     while(rowCnt > 0u) 
00141     { 
00142       /* Writing all zeroes in lower triangle of the destination matrix */ 
00143       j = numRows - rowCnt; 
00144       while(j > 0u) 
00145       { 
00146         *pInT2++ = 0.0f; 
00147         j--; 
00148       } 
00149  
00150       /* Writing all ones in the diagonal of the destination matrix */ 
00151       *pInT2++ = 1.0f; 
00152  
00153       /* Writing all zeroes in upper triangle of the destination matrix */ 
00154       j = rowCnt - 1u; 
00155       while(j > 0u) 
00156       { 
00157         *pInT2++ = 0.0f; 
00158         j--; 
00159       } 
00160  
00161       /* Decrement the loop counter */ 
00162       rowCnt--; 
00163     } 
00164  
00165     /* Loop over the number of columns of the input matrix.  
00166        All the elements in each column are processed by the row operations */ 
00167     loopCnt = numCols; 
00168  
00169     /* Index modifier to navigate through the columns */ 
00170     l = 0u; 
00171  
00172     while(loopCnt > 0u) 
00173     { 
00174       /* Check if the pivot element is zero..  
00175        * If it is zero then interchange the row with non zero row below.  
00176        * If there is no non zero element to replace in the rows below,  
00177        * then the matrix is Singular. */ 
00178  
00179       /* Working pointer for the input matrix that points  
00180        * to the pivot element of the particular row  */ 
00181       pInT1 = pIn + (l * numCols); 
00182  
00183       /* Working pointer for the destination matrix that points  
00184        * to the pivot element of the particular row  */ 
00185       pInT3 = pOut + (l * numCols); 
00186  
00187       /* Temporary variable to hold the pivot value */ 
00188       in = *pInT1; 
00189  
00190       /* Destination pointer modifier */ 
00191       k = 1u; 
00192  
00193       /* Check if the pivot element is zero */ 
00194       if(*pInT1 == 0.0f) 
00195       { 
00196         /* Loop over the number rows present below */ 
00197         i = numRows - (l + 1u); 
00198  
00199         while(i > 0u) 
00200         { 
00201           /* Update the input and destination pointers */ 
00202           pInT2 = pInT1 + (numCols * l); 
00203           pInT4 = pInT3 + (numCols * k); 
00204  
00205           /* Check if there is a non zero pivot element to  
00206            * replace in the rows below */ 
00207           if(*pInT2 != 0.0f) 
00208           { 
00209             /* Loop over number of columns  
00210              * to the right of the pilot element */ 
00211             j = numCols - l; 
00212  
00213             while(j > 0u) 
00214             { 
00215               /* Exchange the row elements of the input matrix */ 
00216               Xchg = *pInT2; 
00217               *pInT2++ = *pInT1; 
00218               *pInT1++ = Xchg; 
00219  
00220               /* Decrement the loop counter */ 
00221               j--; 
00222             } 
00223  
00224             /* Loop over number of columns of the destination matrix */ 
00225             j = numCols; 
00226  
00227             while(j > 0u) 
00228             { 
00229               /* Exchange the row elements of the destination matrix */ 
00230               Xchg = *pInT4; 
00231               *pInT4++ = *pInT3; 
00232               *pInT3++ = Xchg; 
00233  
00234               /* Decrement the loop counter */ 
00235               j--; 
00236             } 
00237  
00238             /* Flag to indicate whether exchange is done or not */ 
00239             flag = 1u; 
00240  
00241             /* Break after exchange is done */ 
00242             break; 
00243           } 
00244  
00245           /* Update the destination pointer modifier */ 
00246           k++; 
00247  
00248           /* Decrement the loop counter */ 
00249           i--; 
00250         } 
00251       } 
00252  
00253       /* Update the status if the matrix is singular */ 
00254       if((flag != 1u) && (in == 0.0f)) 
00255       { 
00256         status = ARM_MATH_SINGULAR; 
00257  
00258         break; 
00259       } 
00260  
00261       /* Points to the pivot row of input and destination matrices */ 
00262       pPivotRowIn = pIn + (l * numCols); 
00263       pPivotRowDst = pOut + (l * numCols); 
00264  
00265       /* Temporary pointers to the pivot row pointers */ 
00266       pInT1 = pPivotRowIn; 
00267       pInT2 = pPivotRowDst; 
00268  
00269       /* Pivot element of the row */ 
00270       in = *(pIn + (l * numCols)); 
00271  
00272       /* Loop over number of columns  
00273        * to the right of the pilot element */ 
00274       j = (numCols - l); 
00275  
00276       while(j > 0u) 
00277       { 
00278         /* Divide each element of the row of the input matrix  
00279          * by the pivot element */ 
00280         in1 = *pInT1; 
00281         *pInT1++ = in1 / in; 
00282  
00283         /* Decrement the loop counter */ 
00284         j--; 
00285       } 
00286  
00287       /* Loop over number of columns of the destination matrix */ 
00288       j = numCols; 
00289  
00290       while(j > 0u) 
00291       { 
00292         /* Divide each element of the row of the destination matrix  
00293          * by the pivot element */ 
00294         in1 = *pInT2; 
00295         *pInT2++ = in1 / in; 
00296  
00297         /* Decrement the loop counter */ 
00298         j--; 
00299       } 
00300  
00301       /* Replace the rows with the sum of that row and a multiple of row i  
00302        * so that each new element in column i above row i is zero.*/ 
00303  
00304       /* Temporary pointers for input and destination matrices */ 
00305       pInT1 = pIn; 
00306       pInT2 = pOut; 
00307  
00308       /* index used to check for pivot element */ 
00309       i = 0u; 
00310  
00311       /* Loop over number of rows */ 
00312       /*  to be replaced by the sum of that row and a multiple of row i */ 
00313       k = numRows; 
00314  
00315       while(k > 0u) 
00316       { 
00317         /* Check for the pivot element */ 
00318         if(i == l) 
00319         { 
00320           /* If the processing element is the pivot element,  
00321              only the columns to the right are to be processed */ 
00322           pInT1 += numCols - l; 
00323  
00324           pInT2 += numCols; 
00325         } 
00326         else 
00327         { 
00328           /* Element of the reference row */ 
00329           in = *pInT1; 
00330  
00331           /* Working pointers for input and destination pivot rows */ 
00332           pPRT_in = pPivotRowIn; 
00333           pPRT_pDst = pPivotRowDst; 
00334  
00335           /* Loop over the number of columns to the right of the pivot element,  
00336              to replace the elements in the input matrix */ 
00337           j = (numCols - l); 
00338  
00339           while(j > 0u) 
00340           { 
00341             /* Replace the element by the sum of that row  
00342                and a multiple of the reference row  */ 
00343             in1 = *pInT1; 
00344             *pInT1++ = in1 - (in * *pPRT_in++); 
00345  
00346             /* Decrement the loop counter */ 
00347             j--; 
00348           } 
00349  
00350           /* Loop over the number of columns to  
00351              replace the elements in the destination matrix */ 
00352           j = numCols; 
00353  
00354           while(j > 0u) 
00355           { 
00356             /* Replace the element by the sum of that row  
00357                and a multiple of the reference row  */ 
00358             in1 = *pInT2; 
00359             *pInT2++ = in1 - (in * *pPRT_pDst++); 
00360  
00361             /* Decrement the loop counter */ 
00362             j--; 
00363           } 
00364  
00365         } 
00366  
00367         /* Increment the temporary input pointer */ 
00368         pInT1 = pInT1 + l; 
00369  
00370         /* Decrement the loop counter */ 
00371         k--; 
00372  
00373         /* Increment the pivot index */ 
00374         i++; 
00375       } 
00376  
00377       /* Increment the input pointer */ 
00378       pIn++; 
00379  
00380       /* Decrement the loop counter */ 
00381       loopCnt--; 
00382  
00383       /* Increment the index modifier */ 
00384       l++; 
00385     } 
00386  
00387     /* Set status as ARM_MATH_SUCCESS */ 
00388     status = ARM_MATH_SUCCESS; 
00389  
00390     if((flag != 1u) && (in == 0.0f)) 
00391     { 
00392       status = ARM_MATH_SINGULAR; 
00393     } 
00394  
00395   } 
00396  
00397   /* Return to application */ 
00398   return (status); 
00399 } 
00400  
00401 /**  
00402  * @} end of MatrixInv group  
00403  */