arm_mat_inverse_f64.c 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691
  1. /* ----------------------------------------------------------------------
  2. * Project: CMSIS DSP Library
  3. * Title: arm_mat_inverse_f64.c
  4. * Description: Floating-point matrix inverse
  5. *
  6. * $Date: 27. January 2017
  7. * $Revision: V.1.5.1
  8. *
  9. * Target Processor: Cortex-M cores
  10. * -------------------------------------------------------------------- */
  11. /*
  12. * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
  13. *
  14. * SPDX-License-Identifier: Apache-2.0
  15. *
  16. * Licensed under the Apache License, Version 2.0 (the License); you may
  17. * not use this file except in compliance with the License.
  18. * You may obtain a copy of the License at
  19. *
  20. * www.apache.org/licenses/LICENSE-2.0
  21. *
  22. * Unless required by applicable law or agreed to in writing, software
  23. * distributed under the License is distributed on an AS IS BASIS, WITHOUT
  24. * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  25. * See the License for the specific language governing permissions and
  26. * limitations under the License.
  27. */
  28. #include "arm_math.h"
  29. /**
  30. * @ingroup groupMatrix
  31. */
  32. /**
  33. * @defgroup MatrixInv Matrix Inverse
  34. *
  35. * Computes the inverse of a matrix.
  36. *
  37. * The inverse is defined only if the input matrix is square and non-singular (the determinant
  38. * is non-zero). The function checks that the input and output matrices are square and of the
  39. * same size.
  40. *
  41. * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix
  42. * inversion of floating-point matrices.
  43. *
  44. * \par Algorithm
  45. * The Gauss-Jordan method is used to find the inverse.
  46. * The algorithm performs a sequence of elementary row-operations until it
  47. * reduces the input matrix to an identity matrix. Applying the same sequence
  48. * of elementary row-operations to an identity matrix yields the inverse matrix.
  49. * If the input matrix is singular, then the algorithm terminates and returns error status
  50. * <code>ARM_MATH_SINGULAR</code>.
  51. * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method"
  52. */
  53. /**
  54. * @addtogroup MatrixInv
  55. * @{
  56. */
  57. /**
  58. * @brief Floating-point matrix inverse.
  59. * @param[in] *pSrc points to input matrix structure
  60. * @param[out] *pDst points to output matrix structure
  61. * @return The function returns
  62. * <code>ARM_MATH_SIZE_MISMATCH</code> if the input matrix is not square or if the size
  63. * of the output matrix does not match the size of the input matrix.
  64. * If the input matrix is found to be singular (non-invertible), then the function returns
  65. * <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>.
  66. */
  67. arm_status arm_mat_inverse_f64(
  68. const arm_matrix_instance_f64 * pSrc,
  69. arm_matrix_instance_f64 * pDst)
  70. {
  71. float64_t *pIn = pSrc->pData; /* input data matrix pointer */
  72. float64_t *pOut = pDst->pData; /* output data matrix pointer */
  73. float64_t *pInT1, *pInT2; /* Temporary input data matrix pointer */
  74. float64_t *pOutT1, *pOutT2; /* Temporary output data matrix pointer */
  75. float64_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst; /* Temporary input and output data matrix pointer */
  76. uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */
  77. uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */
  78. #if defined (ARM_MATH_DSP)
  79. float64_t maxC; /* maximum value in the column */
  80. /* Run the below code for Cortex-M4 and Cortex-M3 */
  81. float64_t Xchg, in = 0.0f, in1; /* Temporary input values */
  82. uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */
  83. arm_status status; /* status of matrix inverse */
  84. #ifdef ARM_MATH_MATRIX_CHECK
  85. /* Check for matrix mismatch condition */
  86. if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
  87. || (pSrc->numRows != pDst->numRows))
  88. {
  89. /* Set status as ARM_MATH_SIZE_MISMATCH */
  90. status = ARM_MATH_SIZE_MISMATCH;
  91. }
  92. else
  93. #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
  94. {
  95. /*--------------------------------------------------------------------------------------------------------------
  96. * Matrix Inverse can be solved using elementary row operations.
  97. *
  98. * Gauss-Jordan Method:
  99. *
  100. * 1. First combine the identity matrix and the input matrix separated by a bar to form an
  101. * augmented matrix as follows:
  102. * _ _ _ _
  103. * | a11 a12 | 1 0 | | X11 X12 |
  104. * | | | = | |
  105. * |_ a21 a22 | 0 1 _| |_ X21 X21 _|
  106. *
  107. * 2. In our implementation, pDst Matrix is used as identity matrix.
  108. *
  109. * 3. Begin with the first row. Let i = 1.
  110. *
  111. * 4. Check to see if the pivot for column i is the greatest of the column.
  112. * The pivot is the element of the main diagonal that is on the current row.
  113. * For instance, if working with row i, then the pivot element is aii.
  114. * If the pivot is not the most significant of the columns, exchange that row with a row
  115. * below it that does contain the most significant value in column i. If the most
  116. * significant value of the column is zero, then an inverse to that matrix does not exist.
  117. * The most significant value of the column is the absolute maximum.
  118. *
  119. * 5. Divide every element of row i by the pivot.
  120. *
  121. * 6. For every row below and row i, replace that row with the sum of that row and
  122. * a multiple of row i so that each new element in column i below row i is zero.
  123. *
  124. * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
  125. * for every element below and above the main diagonal.
  126. *
  127. * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
  128. * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
  129. *----------------------------------------------------------------------------------------------------------------*/
  130. /* Working pointer for destination matrix */
  131. pOutT1 = pOut;
  132. /* Loop over the number of rows */
  133. rowCnt = numRows;
  134. /* Making the destination matrix as identity matrix */
  135. while (rowCnt > 0U)
  136. {
  137. /* Writing all zeroes in lower triangle of the destination matrix */
  138. j = numRows - rowCnt;
  139. while (j > 0U)
  140. {
  141. *pOutT1++ = 0.0f;
  142. j--;
  143. }
  144. /* Writing all ones in the diagonal of the destination matrix */
  145. *pOutT1++ = 1.0f;
  146. /* Writing all zeroes in upper triangle of the destination matrix */
  147. j = rowCnt - 1U;
  148. while (j > 0U)
  149. {
  150. *pOutT1++ = 0.0f;
  151. j--;
  152. }
  153. /* Decrement the loop counter */
  154. rowCnt--;
  155. }
  156. /* Loop over the number of columns of the input matrix.
  157. All the elements in each column are processed by the row operations */
  158. loopCnt = numCols;
  159. /* Index modifier to navigate through the columns */
  160. l = 0U;
  161. while (loopCnt > 0U)
  162. {
  163. /* Check if the pivot element is zero..
  164. * If it is zero then interchange the row with non zero row below.
  165. * If there is no non zero element to replace in the rows below,
  166. * then the matrix is Singular. */
  167. /* Working pointer for the input matrix that points
  168. * to the pivot element of the particular row */
  169. pInT1 = pIn + (l * numCols);
  170. /* Working pointer for the destination matrix that points
  171. * to the pivot element of the particular row */
  172. pOutT1 = pOut + (l * numCols);
  173. /* Temporary variable to hold the pivot value */
  174. in = *pInT1;
  175. /* Grab the most significant value from column l */
  176. maxC = 0;
  177. for (i = l; i < numRows; i++)
  178. {
  179. maxC = *pInT1 > 0 ? (*pInT1 > maxC ? *pInT1 : maxC) : (-*pInT1 > maxC ? -*pInT1 : maxC);
  180. pInT1 += numCols;
  181. }
  182. /* Update the status if the matrix is singular */
  183. if (maxC == 0.0f)
  184. {
  185. return ARM_MATH_SINGULAR;
  186. }
  187. /* Restore pInT1 */
  188. pInT1 = pIn;
  189. /* Destination pointer modifier */
  190. k = 1U;
  191. /* Check if the pivot element is the most significant of the column */
  192. if ( (in > 0.0f ? in : -in) != maxC)
  193. {
  194. /* Loop over the number rows present below */
  195. i = numRows - (l + 1U);
  196. while (i > 0U)
  197. {
  198. /* Update the input and destination pointers */
  199. pInT2 = pInT1 + (numCols * l);
  200. pOutT2 = pOutT1 + (numCols * k);
  201. /* Look for the most significant element to
  202. * replace in the rows below */
  203. if ((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC)
  204. {
  205. /* Loop over number of columns
  206. * to the right of the pilot element */
  207. j = numCols - l;
  208. while (j > 0U)
  209. {
  210. /* Exchange the row elements of the input matrix */
  211. Xchg = *pInT2;
  212. *pInT2++ = *pInT1;
  213. *pInT1++ = Xchg;
  214. /* Decrement the loop counter */
  215. j--;
  216. }
  217. /* Loop over number of columns of the destination matrix */
  218. j = numCols;
  219. while (j > 0U)
  220. {
  221. /* Exchange the row elements of the destination matrix */
  222. Xchg = *pOutT2;
  223. *pOutT2++ = *pOutT1;
  224. *pOutT1++ = Xchg;
  225. /* Decrement the loop counter */
  226. j--;
  227. }
  228. /* Flag to indicate whether exchange is done or not */
  229. flag = 1U;
  230. /* Break after exchange is done */
  231. break;
  232. }
  233. /* Update the destination pointer modifier */
  234. k++;
  235. /* Decrement the loop counter */
  236. i--;
  237. }
  238. }
  239. /* Update the status if the matrix is singular */
  240. if ((flag != 1U) && (in == 0.0f))
  241. {
  242. return ARM_MATH_SINGULAR;
  243. }
  244. /* Points to the pivot row of input and destination matrices */
  245. pPivotRowIn = pIn + (l * numCols);
  246. pPivotRowDst = pOut + (l * numCols);
  247. /* Temporary pointers to the pivot row pointers */
  248. pInT1 = pPivotRowIn;
  249. pInT2 = pPivotRowDst;
  250. /* Pivot element of the row */
  251. in = *pPivotRowIn;
  252. /* Loop over number of columns
  253. * to the right of the pilot element */
  254. j = (numCols - l);
  255. while (j > 0U)
  256. {
  257. /* Divide each element of the row of the input matrix
  258. * by the pivot element */
  259. in1 = *pInT1;
  260. *pInT1++ = in1 / in;
  261. /* Decrement the loop counter */
  262. j--;
  263. }
  264. /* Loop over number of columns of the destination matrix */
  265. j = numCols;
  266. while (j > 0U)
  267. {
  268. /* Divide each element of the row of the destination matrix
  269. * by the pivot element */
  270. in1 = *pInT2;
  271. *pInT2++ = in1 / in;
  272. /* Decrement the loop counter */
  273. j--;
  274. }
  275. /* Replace the rows with the sum of that row and a multiple of row i
  276. * so that each new element in column i above row i is zero.*/
  277. /* Temporary pointers for input and destination matrices */
  278. pInT1 = pIn;
  279. pInT2 = pOut;
  280. /* index used to check for pivot element */
  281. i = 0U;
  282. /* Loop over number of rows */
  283. /* to be replaced by the sum of that row and a multiple of row i */
  284. k = numRows;
  285. while (k > 0U)
  286. {
  287. /* Check for the pivot element */
  288. if (i == l)
  289. {
  290. /* If the processing element is the pivot element,
  291. only the columns to the right are to be processed */
  292. pInT1 += numCols - l;
  293. pInT2 += numCols;
  294. }
  295. else
  296. {
  297. /* Element of the reference row */
  298. in = *pInT1;
  299. /* Working pointers for input and destination pivot rows */
  300. pPRT_in = pPivotRowIn;
  301. pPRT_pDst = pPivotRowDst;
  302. /* Loop over the number of columns to the right of the pivot element,
  303. to replace the elements in the input matrix */
  304. j = (numCols - l);
  305. while (j > 0U)
  306. {
  307. /* Replace the element by the sum of that row
  308. and a multiple of the reference row */
  309. in1 = *pInT1;
  310. *pInT1++ = in1 - (in * *pPRT_in++);
  311. /* Decrement the loop counter */
  312. j--;
  313. }
  314. /* Loop over the number of columns to
  315. replace the elements in the destination matrix */
  316. j = numCols;
  317. while (j > 0U)
  318. {
  319. /* Replace the element by the sum of that row
  320. and a multiple of the reference row */
  321. in1 = *pInT2;
  322. *pInT2++ = in1 - (in * *pPRT_pDst++);
  323. /* Decrement the loop counter */
  324. j--;
  325. }
  326. }
  327. /* Increment the temporary input pointer */
  328. pInT1 = pInT1 + l;
  329. /* Decrement the loop counter */
  330. k--;
  331. /* Increment the pivot index */
  332. i++;
  333. }
  334. /* Increment the input pointer */
  335. pIn++;
  336. /* Decrement the loop counter */
  337. loopCnt--;
  338. /* Increment the index modifier */
  339. l++;
  340. }
  341. #else
  342. /* Run the below code for Cortex-M0 */
  343. float64_t Xchg, in = 0.0f; /* Temporary input values */
  344. uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */
  345. arm_status status; /* status of matrix inverse */
  346. #ifdef ARM_MATH_MATRIX_CHECK
  347. /* Check for matrix mismatch condition */
  348. if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
  349. || (pSrc->numRows != pDst->numRows))
  350. {
  351. /* Set status as ARM_MATH_SIZE_MISMATCH */
  352. status = ARM_MATH_SIZE_MISMATCH;
  353. }
  354. else
  355. #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
  356. {
  357. /*--------------------------------------------------------------------------------------------------------------
  358. * Matrix Inverse can be solved using elementary row operations.
  359. *
  360. * Gauss-Jordan Method:
  361. *
  362. * 1. First combine the identity matrix and the input matrix separated by a bar to form an
  363. * augmented matrix as follows:
  364. * _ _ _ _ _ _ _ _
  365. * | | a11 a12 | | | 1 0 | | | X11 X12 |
  366. * | | | | | | | = | |
  367. * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _|
  368. *
  369. * 2. In our implementation, pDst Matrix is used as identity matrix.
  370. *
  371. * 3. Begin with the first row. Let i = 1.
  372. *
  373. * 4. Check to see if the pivot for row i is zero.
  374. * The pivot is the element of the main diagonal that is on the current row.
  375. * For instance, if working with row i, then the pivot element is aii.
  376. * If the pivot is zero, exchange that row with a row below it that does not
  377. * contain a zero in column i. If this is not possible, then an inverse
  378. * to that matrix does not exist.
  379. *
  380. * 5. Divide every element of row i by the pivot.
  381. *
  382. * 6. For every row below and row i, replace that row with the sum of that row and
  383. * a multiple of row i so that each new element in column i below row i is zero.
  384. *
  385. * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
  386. * for every element below and above the main diagonal.
  387. *
  388. * 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
  389. * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
  390. *----------------------------------------------------------------------------------------------------------------*/
  391. /* Working pointer for destination matrix */
  392. pOutT1 = pOut;
  393. /* Loop over the number of rows */
  394. rowCnt = numRows;
  395. /* Making the destination matrix as identity matrix */
  396. while (rowCnt > 0U)
  397. {
  398. /* Writing all zeroes in lower triangle of the destination matrix */
  399. j = numRows - rowCnt;
  400. while (j > 0U)
  401. {
  402. *pOutT1++ = 0.0f;
  403. j--;
  404. }
  405. /* Writing all ones in the diagonal of the destination matrix */
  406. *pOutT1++ = 1.0f;
  407. /* Writing all zeroes in upper triangle of the destination matrix */
  408. j = rowCnt - 1U;
  409. while (j > 0U)
  410. {
  411. *pOutT1++ = 0.0f;
  412. j--;
  413. }
  414. /* Decrement the loop counter */
  415. rowCnt--;
  416. }
  417. /* Loop over the number of columns of the input matrix.
  418. All the elements in each column are processed by the row operations */
  419. loopCnt = numCols;
  420. /* Index modifier to navigate through the columns */
  421. l = 0U;
  422. //for(loopCnt = 0U; loopCnt < numCols; loopCnt++)
  423. while (loopCnt > 0U)
  424. {
  425. /* Check if the pivot element is zero..
  426. * If it is zero then interchange the row with non zero row below.
  427. * If there is no non zero element to replace in the rows below,
  428. * then the matrix is Singular. */
  429. /* Working pointer for the input matrix that points
  430. * to the pivot element of the particular row */
  431. pInT1 = pIn + (l * numCols);
  432. /* Working pointer for the destination matrix that points
  433. * to the pivot element of the particular row */
  434. pOutT1 = pOut + (l * numCols);
  435. /* Temporary variable to hold the pivot value */
  436. in = *pInT1;
  437. /* Destination pointer modifier */
  438. k = 1U;
  439. /* Check if the pivot element is zero */
  440. if (*pInT1 == 0.0f)
  441. {
  442. /* Loop over the number rows present below */
  443. for (i = (l + 1U); i < numRows; i++)
  444. {
  445. /* Update the input and destination pointers */
  446. pInT2 = pInT1 + (numCols * l);
  447. pOutT2 = pOutT1 + (numCols * k);
  448. /* Check if there is a non zero pivot element to
  449. * replace in the rows below */
  450. if (*pInT2 != 0.0f)
  451. {
  452. /* Loop over number of columns
  453. * to the right of the pilot element */
  454. for (j = 0U; j < (numCols - l); j++)
  455. {
  456. /* Exchange the row elements of the input matrix */
  457. Xchg = *pInT2;
  458. *pInT2++ = *pInT1;
  459. *pInT1++ = Xchg;
  460. }
  461. for (j = 0U; j < numCols; j++)
  462. {
  463. Xchg = *pOutT2;
  464. *pOutT2++ = *pOutT1;
  465. *pOutT1++ = Xchg;
  466. }
  467. /* Flag to indicate whether exchange is done or not */
  468. flag = 1U;
  469. /* Break after exchange is done */
  470. break;
  471. }
  472. /* Update the destination pointer modifier */
  473. k++;
  474. }
  475. }
  476. /* Update the status if the matrix is singular */
  477. if ((flag != 1U) && (in == 0.0f))
  478. {
  479. return ARM_MATH_SINGULAR;
  480. }
  481. /* Points to the pivot row of input and destination matrices */
  482. pPivotRowIn = pIn + (l * numCols);
  483. pPivotRowDst = pOut + (l * numCols);
  484. /* Temporary pointers to the pivot row pointers */
  485. pInT1 = pPivotRowIn;
  486. pOutT1 = pPivotRowDst;
  487. /* Pivot element of the row */
  488. in = *(pIn + (l * numCols));
  489. /* Loop over number of columns
  490. * to the right of the pilot element */
  491. for (j = 0U; j < (numCols - l); j++)
  492. {
  493. /* Divide each element of the row of the input matrix
  494. * by the pivot element */
  495. *pInT1 = *pInT1 / in;
  496. pInT1++;
  497. }
  498. for (j = 0U; j < numCols; j++)
  499. {
  500. /* Divide each element of the row of the destination matrix
  501. * by the pivot element */
  502. *pOutT1 = *pOutT1 / in;
  503. pOutT1++;
  504. }
  505. /* Replace the rows with the sum of that row and a multiple of row i
  506. * so that each new element in column i above row i is zero.*/
  507. /* Temporary pointers for input and destination matrices */
  508. pInT1 = pIn;
  509. pOutT1 = pOut;
  510. for (i = 0U; i < numRows; i++)
  511. {
  512. /* Check for the pivot element */
  513. if (i == l)
  514. {
  515. /* If the processing element is the pivot element,
  516. only the columns to the right are to be processed */
  517. pInT1 += numCols - l;
  518. pOutT1 += numCols;
  519. }
  520. else
  521. {
  522. /* Element of the reference row */
  523. in = *pInT1;
  524. /* Working pointers for input and destination pivot rows */
  525. pPRT_in = pPivotRowIn;
  526. pPRT_pDst = pPivotRowDst;
  527. /* Loop over the number of columns to the right of the pivot element,
  528. to replace the elements in the input matrix */
  529. for (j = 0U; j < (numCols - l); j++)
  530. {
  531. /* Replace the element by the sum of that row
  532. and a multiple of the reference row */
  533. *pInT1 = *pInT1 - (in * *pPRT_in++);
  534. pInT1++;
  535. }
  536. /* Loop over the number of columns to
  537. replace the elements in the destination matrix */
  538. for (j = 0U; j < numCols; j++)
  539. {
  540. /* Replace the element by the sum of that row
  541. and a multiple of the reference row */
  542. *pOutT1 = *pOutT1 - (in * *pPRT_pDst++);
  543. pOutT1++;
  544. }
  545. }
  546. /* Increment the temporary input pointer */
  547. pInT1 = pInT1 + l;
  548. }
  549. /* Increment the input pointer */
  550. pIn++;
  551. /* Decrement the loop counter */
  552. loopCnt--;
  553. /* Increment the index modifier */
  554. l++;
  555. }
  556. #endif /* #if defined (ARM_MATH_DSP) */
  557. /* Set status as ARM_MATH_SUCCESS */
  558. status = ARM_MATH_SUCCESS;
  559. if ((flag != 1U) && (in == 0.0f))
  560. {
  561. pIn = pSrc->pData;
  562. for (i = 0; i < numRows * numCols; i++)
  563. {
  564. if (pIn[i] != 0.0f)
  565. break;
  566. }
  567. if (i == numRows * numCols)
  568. status = ARM_MATH_SINGULAR;
  569. }
  570. }
  571. /* Return to application */
  572. return (status);
  573. }
  574. /**
  575. * @} end of MatrixInv group
  576. */