stm32f407-openocd/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_ldlt_f64.c

/* ----------------------------------------------------------------------
 * Project:      CMSIS DSP Library
 * Title:        arm_mat_ldl_f64.c
 * Description:  Floating-point LDL 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"
#include <math.h>



/// @private
#define SWAP_ROWS_F64(A,i,j) \
{                            \
  int w;                     \
  for(w=0;w < n; w++)        \
  {                          \
     float64_t tmp;          \
     tmp = A[i*n + w];       \
     A[i*n + w] = A[j*n + w];\
     A[j*n + w] = tmp;       \
  }                          \
}

/// @private
#define SWAP_COLS_F64(A,i,j) \
{                            \
  int w;                     \
  for(w=0;w < n; w++)        \
  {                          \
     float64_t tmp;          \
     tmp = A[w*n + i];       \
     A[w*n + i] = A[w*n + j];\
     A[w*n + j] = tmp;       \
  }                          \
}

/**
  @ingroup groupMatrix
 */

/**
  @addtogroup MatrixChol
  @{
 */

/**
   * @brief Floating-point LDL^t decomposition of positive semi-definite matrix.
   * @param[in]  pSrc   points to the instance of the input floating-point matrix structure.
   * @param[out] pl   points to the instance of the output floating-point triangular matrix structure.
   * @param[out] pd   points to the instance of the output floating-point diagonal matrix structure.
   * @param[out] pp   points to the instance of the output floating-point permutation vector.
   * @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
   *  Computes the LDL^t decomposition of a matrix A such that P A P^t = L D L^t.
   */

arm_status arm_mat_ldlt_f64(
  const arm_matrix_instance_f64 * pSrc,
  arm_matrix_instance_f64 * pl,
  arm_matrix_instance_f64 * pd,
  uint16_t * pp)
{

  arm_status status;                             /* status of matrix inverse */
 

#ifdef ARM_MATH_MATRIX_CHECK

  /* Check for matrix mismatch condition */
  if ((pSrc->numRows != pSrc->numCols) ||
      (pl->numRows != pl->numCols) ||
      (pd->numRows != pd->numCols) ||
      (pl->numRows != pd->numRows)   )
  {
    /* Set status as ARM_MATH_SIZE_MISMATCH */
    status = ARM_MATH_SIZE_MISMATCH;
  }
  else

#endif /* #ifdef ARM_MATH_MATRIX_CHECK */

  {

    const int n=pSrc->numRows;
    int fullRank = 1, diag,k;
    float64_t *pA;

    memset(pd->pData,0,sizeof(float64_t)*n*n);

    memcpy(pl->pData,pSrc->pData,n*n*sizeof(float64_t));
    pA = pl->pData;

    for(k=0;k < n; k++)
    {
      pp[k] = k;
    }


    for(k=0;k < n; k++)
    {
        /* Find pivot */
        float64_t m=F64_MIN,a;
        int w,r,j=k; 


        for(r=k;r<n;r++)
        {
           if (pA[r*n+r] > m)
           {
             m = pA[r*n+r];
             j = r;
           }
        }

        if(j != k)
        {
          SWAP_ROWS_F64(pA,k,j);
          SWAP_COLS_F64(pA,k,j);
        }


        pp[k] = j;

        a = pA[k*n+k];

        if (fabs(a) < 1.0e-18)
        {

            fullRank = 0;
            break;
        }

        for(w=k+1;w<n;w++)
        {
          int x;
          for(x=k+1;x<n;x++)
          {
             pA[w*n+x] = pA[w*n+x] - pA[w*n+k] * pA[x*n+k] / a;
          }
        }

        for(w=k+1;w<n;w++)
        {
               pA[w*n+k] = pA[w*n+k] / a;
        }

        

    }



    diag=k;
    if (!fullRank)
    {
      diag--;
      {
        int row;
        for(row=0; row < n;row++)
        {
          int col;
          for(col=k; col < n;col++)
          {
             pl->pData[row*n+col]=0.0;
          }
        }
      }
    }

    {
      int row;
      for(row=0; row < n;row++)
      {
         int col;
         for(col=row+1; col < n;col++)
         {
           pl->pData[row*n+col] = 0.0;
         }
      }
    }

    {
      int d;
      for(d=0; d < diag;d++)
      {
        pd->pData[d*n+d] = pl->pData[d*n+d];
        pl->pData[d*n+d] = 1.0;
      }
    }
  
    status = ARM_MATH_SUCCESS;

  }

  
  /* Return to application */
  return (status);
}

/**
  @} end of MatrixChol group
 */