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