init

Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cholesky_f64.c

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_mat_cholesky_f64.c
+ * Description:  Floating-point Cholesky decomposition
+ *
+ * $Date:        23 April 2021
+ * $Revision:    V1.9.0
+ *
+ * Target Processor: Cortex-M and Cortex-A cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "dsp/matrix_functions.h"
+
+/**
+  @ingroup groupMatrix
+ */
+
+/**
+  @addtogroup MatrixChol
+  @{
+ */
+
+/**
+   * @brief Floating-point Cholesky decomposition of positive-definite matrix.
+   * @param[in]  pSrc   points to the instance of the input floating-point matrix structure.
+   * @param[out] pDst   points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * @return        execution status
+                   - \ref ARM_MATH_SUCCESS       : Operation successful
+                   - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed
+                   - \ref ARM_MATH_DECOMPOSITION_FAILURE      : Input matrix cannot be decomposed
+   * @par
+   * If the matrix is ill conditioned or only semi-definite, then it is better using the LDL^t decomposition.
+   * The decomposition of A is returning a lower triangular matrix U such that A = U U^t
+   */
+
+
+arm_status arm_mat_cholesky_f64(
+  const arm_matrix_instance_f64 * pSrc,
+        arm_matrix_instance_f64 * pDst)
+{
+
+  arm_status status;                             /* status of matrix inverse */
+
+
+#ifdef ARM_MATH_MATRIX_CHECK
+
+  /* Check for matrix mismatch condition */
+  if ((pSrc->numRows != pSrc->numCols) ||
+      (pDst->numRows != pDst->numCols) ||
+      (pSrc->numRows != pDst->numRows)   )
+  {
+    /* Set status as ARM_MATH_SIZE_MISMATCH */
+    status = ARM_MATH_SIZE_MISMATCH;
+  }
+  else
+
+#endif /* #ifdef ARM_MATH_MATRIX_CHECK */
+
+  {
+    int i,j,k;
+    int n = pSrc->numRows;
+    float64_t invSqrtVj;
+    float64_t *pA,*pG;
+
+    pA = pSrc->pData;
+    pG = pDst->pData;
+    
+
+    for(i=0 ; i < n ; i++)
+    {
+       for(j=i ; j < n ; j++)
+       {
+          pG[j * n + i] = pA[j * n + i];
+
+          for(k=0; k < i ; k++)
+          {
+             pG[j * n + i] = pG[j * n + i] - pG[i * n + k] * pG[j * n + k];
+          }
+       }
+
+       if (pG[i * n + i] <= 0.0)
+       {
+         return(ARM_MATH_DECOMPOSITION_FAILURE);
+       }
+
+       invSqrtVj = 1.0/sqrt(pG[i * n + i]);
+       for(j=i ; j < n ; j++)
+       {
+         pG[j * n + i] = pG[j * n + i] * invSqrtVj ;
+       }
+    }
+
+    status = ARM_MATH_SUCCESS;
+
+  }
+
+  
+  /* Return to application */
+  return (status);
+}
+
+/**
+  @} end of MatrixChol group
+ */