1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_mat_inverse_f64.c
4 * Description: Floating-point matrix inverse
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 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
29 #include "dsp/matrix_functions.h"
30
31 /**
32 @ingroup groupMatrix
33 */
34
35
36 /**
37 @addtogroup MatrixInv
38 @{
39 */
40
41 /**
42 @brief Floating-point (64 bit) matrix inverse.
43 @param[in] pSrc points to input matrix structure. The source matrix is modified by the function.
44 @param[out] pDst points to output matrix structure
45 @return execution status
46 - \ref ARM_MATH_SUCCESS : Operation successful
47 - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed
48 - \ref ARM_MATH_SINGULAR : Input matrix is found to be singular (non-invertible)
49 */
50
arm_mat_inverse_f64(const arm_matrix_instance_f64 * pSrc,arm_matrix_instance_f64 * pDst)51 arm_status arm_mat_inverse_f64(
52 const arm_matrix_instance_f64 * pSrc,
53 arm_matrix_instance_f64 * pDst)
54 {
55 float64_t *pIn = pSrc->pData; /* input data matrix pointer */
56 float64_t *pOut = pDst->pData; /* output data matrix pointer */
57 float64_t *pInT1, *pInT2; /* Temporary input data matrix pointer */
58 float64_t *pOutT1, *pOutT2; /* Temporary output data matrix pointer */
59 float64_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst; /* Temporary input and output data matrix pointer */
60 uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */
61 uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */
62
63 #if defined (ARM_MATH_DSP)
64
65 float64_t Xchg, in = 0.0, in1; /* Temporary input values */
66 uint32_t i, rowCnt, flag = 0U, j, loopCnt, k,l; /* loop counters */
67 arm_status status; /* status of matrix inverse */
68
69 #ifdef ARM_MATH_MATRIX_CHECK
70
71 /* Check for matrix mismatch condition */
72 if ((pSrc->numRows != pSrc->numCols) ||
73 (pDst->numRows != pDst->numCols) ||
74 (pSrc->numRows != pDst->numRows) )
75 {
76 /* Set status as ARM_MATH_SIZE_MISMATCH */
77 status = ARM_MATH_SIZE_MISMATCH;
78 }
79 else
80
81 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
82
83 {
84
85 /*--------------------------------------------------------------------------------------------------------------
86 * Matrix Inverse can be solved using elementary row operations.
87 *
88 * Gauss-Jordan Method:
89 *
90 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
91 * augmented matrix as follows:
92 * _ _ _ _
93 * | a11 a12 | 1 0 | | X11 X12 |
94 * | | | = | |
95 * |_ a21 a22 | 0 1 _| |_ X21 X21 _|
96 *
97 * 2. In our implementation, pDst Matrix is used as identity matrix.
98 *
99 * 3. Begin with the first row. Let i = 1.
100 *
101 * 4. Check to see if the pivot for row i is zero.
102 * The pivot is the element of the main diagonal that is on the current row.
103 * For instance, if working with row i, then the pivot element is aii.
104 * If the pivot is zero, exchange that row with a row below it that does not
105 * contain a zero in column i. If this is not possible, then an inverse
106 * to that matrix does not exist.
107 *
108 * 5. Divide every element of row i by the pivot.
109 *
110 * 6. For every row below and row i, replace that row with the sum of that row and
111 * a multiple of row i so that each new element in column i below row i is zero.
112 *
113 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
114 * for every element below and above the main diagonal.
115 *
116 * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
117 * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
118 *----------------------------------------------------------------------------------------------------------------*/
119
120 /* Working pointer for destination matrix */
121 pOutT1 = pOut;
122
123 /* Loop over the number of rows */
124 rowCnt = numRows;
125
126 /* Making the destination matrix as identity matrix */
127 while (rowCnt > 0U)
128 {
129 /* Writing all zeroes in lower triangle of the destination matrix */
130 j = numRows - rowCnt;
131 while (j > 0U)
132 {
133 *pOutT1++ = 0.0;
134 j--;
135 }
136
137 /* Writing all ones in the diagonal of the destination matrix */
138 *pOutT1++ = 1.0;
139
140 /* Writing all zeroes in upper triangle of the destination matrix */
141 j = rowCnt - 1U;
142 while (j > 0U)
143 {
144 *pOutT1++ = 0.0;
145 j--;
146 }
147
148 /* Decrement loop counter */
149 rowCnt--;
150 }
151
152 /* Loop over the number of columns of the input matrix.
153 All the elements in each column are processed by the row operations */
154 loopCnt = numCols;
155
156 /* Index modifier to navigate through the columns */
157 l = 0U;
158
159 while (loopCnt > 0U)
160 {
161 /* Check if the pivot element is zero..
162 * If it is zero then interchange the row with non zero row below.
163 * If there is no non zero element to replace in the rows below,
164 * then the matrix is Singular. */
165
166 /* Working pointer for the input matrix that points
167 * to the pivot element of the particular row */
168 pInT1 = pIn + (l * numCols);
169
170 /* Working pointer for the destination matrix that points
171 * to the pivot element of the particular row */
172 pOutT1 = pOut + (l * numCols);
173
174 /* Temporary variable to hold the pivot value */
175 in = *pInT1;
176
177
178
179 /* Check if the pivot element is zero */
180 if (*pInT1 == 0.0)
181 {
182 /* Loop over the number rows present below */
183
184 for (i = 1U; i < numRows - l; i++)
185 {
186 /* Update the input and destination pointers */
187 pInT2 = pInT1 + (numCols * i);
188 pOutT2 = pOutT1 + (numCols * i);
189
190 /* Check if there is a non zero pivot element to
191 * replace in the rows below */
192 if (*pInT2 != 0.0)
193 {
194 /* Loop over number of columns
195 * to the right of the pilot element */
196 j = numCols - l;
197
198 while (j > 0U)
199 {
200 /* Exchange the row elements of the input matrix */
201 Xchg = *pInT2;
202 *pInT2++ = *pInT1;
203 *pInT1++ = Xchg;
204
205 /* Decrement the loop counter */
206 j--;
207 }
208
209 /* Loop over number of columns of the destination matrix */
210 j = numCols;
211
212 while (j > 0U)
213 {
214 /* Exchange the row elements of the destination matrix */
215 Xchg = *pOutT2;
216 *pOutT2++ = *pOutT1;
217 *pOutT1++ = Xchg;
218
219 /* Decrement loop counter */
220 j--;
221 }
222
223 /* Flag to indicate whether exchange is done or not */
224 flag = 1U;
225
226 /* Break after exchange is done */
227 break;
228 }
229
230
231 /* Decrement loop counter */
232 }
233 }
234
235 /* Update the status if the matrix is singular */
236 if ((flag != 1U) && (in == 0.0))
237 {
238 return ARM_MATH_SINGULAR;
239 }
240
241 /* Points to the pivot row of input and destination matrices */
242 pPivotRowIn = pIn + (l * numCols);
243 pPivotRowDst = pOut + (l * numCols);
244
245 /* Temporary pointers to the pivot row pointers */
246 pInT1 = pPivotRowIn;
247 pInT2 = pPivotRowDst;
248
249 /* Pivot element of the row */
250 in = *pPivotRowIn;
251
252 /* Loop over number of columns
253 * to the right of the pilot element */
254 j = (numCols - l);
255
256 while (j > 0U)
257 {
258 /* Divide each element of the row of the input matrix
259 * by the pivot element */
260 in1 = *pInT1;
261 *pInT1++ = in1 / in;
262
263 /* Decrement the loop counter */
264 j--;
265 }
266
267 /* Loop over number of columns of the destination matrix */
268 j = numCols;
269
270 while (j > 0U)
271 {
272 /* Divide each element of the row of the destination matrix
273 * by the pivot element */
274 in1 = *pInT2;
275 *pInT2++ = in1 / in;
276
277 /* Decrement the loop counter */
278 j--;
279 }
280
281 /* Replace the rows with the sum of that row and a multiple of row i
282 * so that each new element in column i above row i is zero.*/
283
284 /* Temporary pointers for input and destination matrices */
285 pInT1 = pIn;
286 pInT2 = pOut;
287
288 /* index used to check for pivot element */
289 i = 0U;
290
291 /* Loop over number of rows */
292 /* to be replaced by the sum of that row and a multiple of row i */
293 k = numRows;
294
295 while (k > 0U)
296 {
297 /* Check for the pivot element */
298 if (i == l)
299 {
300 /* If the processing element is the pivot element,
301 only the columns to the right are to be processed */
302 pInT1 += numCols - l;
303
304 pInT2 += numCols;
305 }
306 else
307 {
308 /* Element of the reference row */
309 in = *pInT1;
310
311 /* Working pointers for input and destination pivot rows */
312 pPRT_in = pPivotRowIn;
313 pPRT_pDst = pPivotRowDst;
314
315 /* Loop over the number of columns to the right of the pivot element,
316 to replace the elements in the input matrix */
317 j = (numCols - l);
318
319 while (j > 0U)
320 {
321 /* Replace the element by the sum of that row
322 and a multiple of the reference row */
323 in1 = *pInT1;
324 *pInT1++ = in1 - (in * *pPRT_in++);
325
326 /* Decrement the loop counter */
327 j--;
328 }
329
330 /* Loop over the number of columns to
331 replace the elements in the destination matrix */
332 j = numCols;
333
334 while (j > 0U)
335 {
336 /* Replace the element by the sum of that row
337 and a multiple of the reference row */
338 in1 = *pInT2;
339 *pInT2++ = in1 - (in * *pPRT_pDst++);
340
341 /* Decrement loop counter */
342 j--;
343 }
344
345 }
346
347 /* Increment temporary input pointer */
348 pInT1 = pInT1 + l;
349
350 /* Decrement loop counter */
351 k--;
352
353 /* Increment pivot index */
354 i++;
355 }
356
357 /* Increment the input pointer */
358 pIn++;
359
360 /* Decrement the loop counter */
361 loopCnt--;
362
363 /* Increment the index modifier */
364 l++;
365 }
366
367
368 #else
369
370 float64_t Xchg, in = 0.0; /* Temporary input values */
371 uint32_t i, rowCnt, flag = 0U, j, loopCnt, l; /* loop counters */
372 arm_status status; /* status of matrix inverse */
373
374 #ifdef ARM_MATH_MATRIX_CHECK
375
376 /* Check for matrix mismatch condition */
377 if ((pSrc->numRows != pSrc->numCols) ||
378 (pDst->numRows != pDst->numCols) ||
379 (pSrc->numRows != pDst->numRows) )
380 {
381 /* Set status as ARM_MATH_SIZE_MISMATCH */
382 status = ARM_MATH_SIZE_MISMATCH;
383 }
384 else
385
386 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
387
388 {
389
390 /*--------------------------------------------------------------------------------------------------------------
391 * Matrix Inverse can be solved using elementary row operations.
392 *
393 * Gauss-Jordan Method:
394 *
395 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
396 * augmented matrix as follows:
397 * _ _ _ _ _ _ _ _
398 * | | a11 a12 | | | 1 0 | | | X11 X12 |
399 * | | | | | | | = | |
400 * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _|
401 *
402 * 2. In our implementation, pDst Matrix is used as identity matrix.
403 *
404 * 3. Begin with the first row. Let i = 1.
405 *
406 * 4. Check to see if the pivot for row i is zero.
407 * The pivot is the element of the main diagonal that is on the current row.
408 * For instance, if working with row i, then the pivot element is aii.
409 * If the pivot is zero, exchange that row with a row below it that does not
410 * contain a zero in column i. If this is not possible, then an inverse
411 * to that matrix does not exist.
412 *
413 * 5. Divide every element of row i by the pivot.
414 *
415 * 6. For every row below and row i, replace that row with the sum of that row and
416 * a multiple of row i so that each new element in column i below row i is zero.
417 *
418 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
419 * for every element below and above the main diagonal.
420 *
421 * 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
422 * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
423 *----------------------------------------------------------------------------------------------------------------*/
424
425 /* Working pointer for destination matrix */
426 pOutT1 = pOut;
427
428 /* Loop over the number of rows */
429 rowCnt = numRows;
430
431 /* Making the destination matrix as identity matrix */
432 while (rowCnt > 0U)
433 {
434 /* Writing all zeroes in lower triangle of the destination matrix */
435 j = numRows - rowCnt;
436 while (j > 0U)
437 {
438 *pOutT1++ = 0.0;
439 j--;
440 }
441
442 /* Writing all ones in the diagonal of the destination matrix */
443 *pOutT1++ = 1.0;
444
445 /* Writing all zeroes in upper triangle of the destination matrix */
446 j = rowCnt - 1U;
447 while (j > 0U)
448 {
449 *pOutT1++ = 0.0;
450 j--;
451 }
452
453 /* Decrement loop counter */
454 rowCnt--;
455 }
456
457 /* Loop over the number of columns of the input matrix.
458 All the elements in each column are processed by the row operations */
459 loopCnt = numCols;
460
461 /* Index modifier to navigate through the columns */
462 l = 0U;
463
464 while (loopCnt > 0U)
465 {
466 /* Check if the pivot element is zero..
467 * If it is zero then interchange the row with non zero row below.
468 * If there is no non zero element to replace in the rows below,
469 * then the matrix is Singular. */
470
471 /* Working pointer for the input matrix that points
472 * to the pivot element of the particular row */
473 pInT1 = pIn + (l * numCols);
474
475 /* Working pointer for the destination matrix that points
476 * to the pivot element of the particular row */
477 pOutT1 = pOut + (l * numCols);
478
479 /* Temporary variable to hold the pivot value */
480 in = *pInT1;
481
482 /* Check if the pivot element is zero */
483 if (*pInT1 == 0.0)
484 {
485 /* Loop over the number rows present below */
486 for (i = 1U; i < numRows-l; i++)
487 {
488 /* Update the input and destination pointers */
489 pInT2 = pInT1 + (numCols * i);
490 pOutT2 = pOutT1 + (numCols * i);
491
492 /* Check if there is a non zero pivot element to
493 * replace in the rows below */
494 if (*pInT2 != 0.0)
495 {
496 /* Loop over number of columns
497 * to the right of the pilot element */
498 for (j = 0U; j < (numCols - l); j++)
499 {
500 /* Exchange the row elements of the input matrix */
501 Xchg = *pInT2;
502 *pInT2++ = *pInT1;
503 *pInT1++ = Xchg;
504 }
505
506 for (j = 0U; j < numCols; j++)
507 {
508 Xchg = *pOutT2;
509 *pOutT2++ = *pOutT1;
510 *pOutT1++ = Xchg;
511 }
512
513 /* Flag to indicate whether exchange is done or not */
514 flag = 1U;
515
516 /* Break after exchange is done */
517 break;
518 }
519 }
520 }
521
522
523 /* Update the status if the matrix is singular */
524 if ((flag != 1U) && (in == 0.0))
525 {
526 return ARM_MATH_SINGULAR;
527 }
528
529 /* Points to the pivot row of input and destination matrices */
530 pPivotRowIn = pIn + (l * numCols);
531 pPivotRowDst = pOut + (l * numCols);
532
533 /* Temporary pointers to the pivot row pointers */
534 pInT1 = pPivotRowIn;
535 pOutT1 = pPivotRowDst;
536
537 /* Pivot element of the row */
538 in = *(pIn + (l * numCols));
539
540 /* Loop over number of columns
541 * to the right of the pilot element */
542 for (j = 0U; j < (numCols - l); j++)
543 {
544 /* Divide each element of the row of the input matrix
545 * by the pivot element */
546 *pInT1 = *pInT1 / in;
547 pInT1++;
548 }
549 for (j = 0U; j < numCols; j++)
550 {
551 /* Divide each element of the row of the destination matrix
552 * by the pivot element */
553 *pOutT1 = *pOutT1 / in;
554 pOutT1++;
555 }
556
557 /* Replace the rows with the sum of that row and a multiple of row i
558 * so that each new element in column i above row i is zero.*/
559
560 /* Temporary pointers for input and destination matrices */
561 pInT1 = pIn;
562 pOutT1 = pOut;
563
564 for (i = 0U; i < numRows; i++)
565 {
566 /* Check for the pivot element */
567 if (i == l)
568 {
569 /* If the processing element is the pivot element,
570 only the columns to the right are to be processed */
571 pInT1 += numCols - l;
572 pOutT1 += numCols;
573 }
574 else
575 {
576 /* Element of the reference row */
577 in = *pInT1;
578
579 /* Working pointers for input and destination pivot rows */
580 pPRT_in = pPivotRowIn;
581 pPRT_pDst = pPivotRowDst;
582
583 /* Loop over the number of columns to the right of the pivot element,
584 to replace the elements in the input matrix */
585 for (j = 0U; j < (numCols - l); j++)
586 {
587 /* Replace the element by the sum of that row
588 and a multiple of the reference row */
589 *pInT1 = *pInT1 - (in * *pPRT_in++);
590 pInT1++;
591 }
592
593 /* Loop over the number of columns to
594 replace the elements in the destination matrix */
595 for (j = 0U; j < numCols; j++)
596 {
597 /* Replace the element by the sum of that row
598 and a multiple of the reference row */
599 *pOutT1 = *pOutT1 - (in * *pPRT_pDst++);
600 pOutT1++;
601 }
602
603 }
604
605 /* Increment temporary input pointer */
606 pInT1 = pInT1 + l;
607 }
608
609 /* Increment the input pointer */
610 pIn++;
611
612 /* Decrement the loop counter */
613 loopCnt--;
614
615 /* Increment the index modifier */
616 l++;
617 }
618
619 #endif /* #if defined (ARM_MATH_DSP) */
620
621 /* Set status as ARM_MATH_SUCCESS */
622 status = ARM_MATH_SUCCESS;
623
624 if ((flag != 1U) && (in == 0.0))
625 {
626 pIn = pSrc->pData;
627 for (i = 0; i < numRows * numCols; i++)
628 {
629 if (pIn[i] != 0.0)
630 break;
631 }
632
633 if (i == numRows * numCols)
634 status = ARM_MATH_SINGULAR;
635 }
636 }
637
638 /* Return to application */
639 return (status);
640 }
641
642 /**
643 @} end of MatrixInv group
644 */
645