1 /* ---------------------------------------------------------------------- 2 * Project: CMSIS DSP Library 3 * Title: arm_mat_solve_upper_triangular_f64.c 4 * Description: Solve linear system UT X = A with UT upper triangular matrix 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 /** 33 @ingroup groupMatrix 34 */ 35 36 37 /** 38 @addtogroup MatrixInv 39 @{ 40 */ 41 42 /** 43 * @brief Solve UT . X = A where UT is an upper triangular matrix 44 * @param[in] ut The upper triangular matrix 45 * @param[in] a The matrix a 46 * @param[out] dst The solution X of UT . X = A 47 * @return The function returns ARM_MATH_SINGULAR, if the system can't be solved. 48 */ arm_mat_solve_upper_triangular_f64(const arm_matrix_instance_f64 * ut,const arm_matrix_instance_f64 * a,arm_matrix_instance_f64 * dst)49 arm_status arm_mat_solve_upper_triangular_f64( 50 const arm_matrix_instance_f64 * ut, 51 const arm_matrix_instance_f64 * a, 52 arm_matrix_instance_f64 * dst) 53 { 54 arm_status status; /* status of matrix inverse */ 55 56 57 #ifdef ARM_MATH_MATRIX_CHECK 58 59 /* Check for matrix mismatch condition */ 60 if ((ut->numRows != ut->numCols) || 61 (a->numRows != a->numCols) || 62 (ut->numRows != a->numRows) ) 63 { 64 /* Set status as ARM_MATH_SIZE_MISMATCH */ 65 status = ARM_MATH_SIZE_MISMATCH; 66 } 67 else 68 69 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */ 70 71 { 72 73 int i,j,k,n; 74 75 n = dst->numRows; 76 77 float64_t *pX = dst->pData; 78 float64_t *pUT = ut->pData; 79 float64_t *pA = a->pData; 80 81 float64_t *ut_row; 82 float64_t *a_col; 83 84 for(j=0; j < n; j ++) 85 { 86 a_col = &pA[j]; 87 88 for(i=n-1; i >= 0 ; i--) 89 { 90 ut_row = &pUT[n*i]; 91 92 float64_t tmp=a_col[i * n]; 93 94 for(k=n-1; k > i; k--) 95 { 96 tmp -= ut_row[k] * pX[n*k+j]; 97 } 98 99 if (ut_row[i]==0.0f) 100 { 101 return(ARM_MATH_SINGULAR); 102 } 103 tmp = tmp / ut_row[i]; 104 pX[i*n+j] = tmp; 105 } 106 107 } 108 status = ARM_MATH_SUCCESS; 109 110 } 111 112 113 /* Return to application */ 114 return (status); 115 } 116 117 118 /** 119 @} end of MatrixInv group 120 */ 121