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