1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_mat_ldl_f64.c
4 * Description: Floating-point LDL decomposition
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 #include "dsp/matrix_utils.h"
31
32 #include <math.h>
33
34
35 /**
36 @ingroup groupMatrix
37 */
38
39 /**
40 @addtogroup MatrixChol
41 @{
42 */
43
44 /**
45 * @brief Floating-point LDL^t decomposition of positive semi-definite matrix.
46 * @param[in] pSrc points to the instance of the input floating-point matrix structure.
47 * @param[out] pl points to the instance of the output floating-point triangular matrix structure.
48 * @param[out] pd points to the instance of the output floating-point diagonal matrix structure.
49 * @param[out] pp points to the instance of the output floating-point permutation vector.
50 * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
51 * @return execution status
52 - \ref ARM_MATH_SUCCESS : Operation successful
53 - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed
54 - \ref ARM_MATH_DECOMPOSITION_FAILURE : Input matrix cannot be decomposed
55 * @par
56 * Computes the LDL^t decomposition of a matrix A such that P A P^t = L D L^t.
57 */
58
arm_mat_ldlt_f64(const arm_matrix_instance_f64 * pSrc,arm_matrix_instance_f64 * pl,arm_matrix_instance_f64 * pd,uint16_t * pp)59 arm_status arm_mat_ldlt_f64(
60 const arm_matrix_instance_f64 * pSrc,
61 arm_matrix_instance_f64 * pl,
62 arm_matrix_instance_f64 * pd,
63 uint16_t * pp)
64 {
65
66 arm_status status; /* status of matrix inverse */
67
68
69 #ifdef ARM_MATH_MATRIX_CHECK
70
71 /* Check for matrix mismatch condition */
72 if ((pSrc->numRows != pSrc->numCols) ||
73 (pl->numRows != pl->numCols) ||
74 (pd->numRows != pd->numCols) ||
75 (pl->numRows != pd->numRows) )
76 {
77 /* Set status as ARM_MATH_SIZE_MISMATCH */
78 status = ARM_MATH_SIZE_MISMATCH;
79 }
80 else
81
82 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
83
84 {
85
86 const int n=pSrc->numRows;
87 int fullRank = 1, diag,k;
88 float64_t *pA;
89
90 memset(pd->pData,0,sizeof(float64_t)*n*n);
91
92 memcpy(pl->pData,pSrc->pData,n*n*sizeof(float64_t));
93 pA = pl->pData;
94
95 for(k=0;k < n; k++)
96 {
97 pp[k] = k;
98 }
99
100
101 for(k=0;k < n; k++)
102 {
103 /* Find pivot */
104 float64_t m=F64_MIN,a;
105 int r,j=k;
106
107
108 for(r=k;r<n;r++)
109 {
110 if (pA[r*n+r] > m)
111 {
112 m = pA[r*n+r];
113 j = r;
114 }
115 }
116
117 if(j != k)
118 {
119 SWAP_ROWS_F64(pl,0,k,j);
120 SWAP_COLS_F64(pl,0,k,j);
121 }
122
123
124 pp[k] = j;
125
126 a = pA[k*n+k];
127
128 if (fabs(a) < 1.0e-18)
129 {
130
131 fullRank = 0;
132 break;
133 }
134
135 for(int w=k+1;w<n;w++)
136 {
137 int x;
138 for(x=k+1;x<n;x++)
139 {
140 pA[w*n+x] = pA[w*n+x] - pA[w*n+k] * pA[x*n+k] / a;
141 }
142 }
143
144 for(int w=k+1;w<n;w++)
145 {
146 pA[w*n+k] = pA[w*n+k] / a;
147 }
148
149
150
151 }
152
153
154
155 diag=k;
156 if (!fullRank)
157 {
158 diag--;
159 {
160 int row;
161 for(row=0; row < n;row++)
162 {
163 int col;
164 for(col=k; col < n;col++)
165 {
166 pl->pData[row*n+col]=0.0;
167 }
168 }
169 }
170 }
171
172 {
173 int row;
174 for(row=0; row < n;row++)
175 {
176 int col;
177 for(col=row+1; col < n;col++)
178 {
179 pl->pData[row*n+col] = 0.0;
180 }
181 }
182 }
183
184 {
185 int d;
186 for(d=0; d < diag;d++)
187 {
188 pd->pData[d*n+d] = pl->pData[d*n+d];
189 pl->pData[d*n+d] = 1.0;
190 }
191 }
192
193 status = ARM_MATH_SUCCESS;
194
195 }
196
197
198 /* Return to application */
199 return (status);
200 }
201
202 /**
203 @} end of MatrixChol group
204 */
205
