Subversion Repositories dashGPS

Rev

Rev 2 | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. /* ----------------------------------------------------------------------
  2. * Copyright (C) 2010-2012 ARM Limited. All rights reserved.
  3. *
  4. * $Date:         17. January 2013
  5. * $Revision:     V1.4.0
  6. *
  7. * Project:       CMSIS DSP Library
  8. * Title:         arm_matrix_example_f32.c
  9. *
  10. * Description:   Example code demonstrating least square fit to data
  11. *                using matrix functions
  12. *
  13. * Target Processor: Cortex-M4/Cortex-M3
  14. *
  15. * Redistribution and use in source and binary forms, with or without
  16. * modification, are permitted provided that the following conditions
  17. * are met:
  18. *   - Redistributions of source code must retain the above copyright
  19. *     notice, this list of conditions and the following disclaimer.
  20. *   - Redistributions in binary form must reproduce the above copyright
  21. *     notice, this list of conditions and the following disclaimer in
  22. *     the documentation and/or other materials provided with the
  23. *     distribution.
  24. *   - Neither the name of ARM LIMITED nor the names of its contributors
  25. *     may be used to endorse or promote products derived from this
  26. *     software without specific prior written permission.
  27. *
  28. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  31. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  32. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  33. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  34. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  35. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  36. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  37. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39. * POSSIBILITY OF SUCH DAMAGE.
  40.  * -------------------------------------------------------------------- */
  41.  
  42. /**
  43.  * @ingroup groupExamples
  44.  */
  45.  
  46. /**
  47.  * @defgroup MatrixExample Matrix Example
  48.  *
  49.  * \par Description:
  50.  * \par
  51.  * Demonstrates the use of Matrix Transpose, Matrix Muliplication, and Matrix Inverse
  52.  * functions to apply least squares fitting to input data. Least squares fitting is
  53.  * the procedure for finding the best-fitting curve that minimizes the sum of the
  54.  * squares of the offsets (least square error) from a given set of data.
  55.  *
  56.  * \par Algorithm:
  57.  * \par
  58.  * The linear combination of parameters considered is as follows:
  59.  * \par
  60.  * <code>A * X = B</code>, where \c X is the unknown value and can be estimated
  61.  * from \c A & \c B.
  62.  * \par
  63.  * The least squares estimate \c X is given by the following equation:
  64.  * \par
  65.  * <code>X = Inverse(A<sup>T</sup> * A) *  A<sup>T</sup> * B</code>
  66.  *
  67.  * \par Block Diagram:
  68.  * \par
  69.  * \image html matrixExample.gif
  70.  *
  71.  * \par Variables Description:
  72.  * \par
  73.  * \li \c A_f32 input matrix in the linear combination equation
  74.  * \li \c B_f32 output matrix in the linear combination equation
  75.  * \li \c X_f32 unknown matrix estimated using \c A_f32 & \c B_f32 matrices
  76.  *
  77.  * \par CMSIS DSP Software Library Functions Used:
  78.  * \par
  79.  * - arm_mat_init_f32()
  80.  * - arm_mat_trans_f32()
  81.  * - arm_mat_mult_f32()
  82.  * - arm_mat_inverse_f32()
  83.  *
  84.  * <b> Refer  </b>
  85.  * \link arm_matrix_example_f32.c \endlink
  86.  *
  87.  */
  88.  
  89.  
  90. /** \example arm_matrix_example_f32.c
  91.   */
  92.  
  93. #include "arm_math.h"
  94. #include "math_helper.h"
  95.  
  96. #define SNR_THRESHOLD   90
  97.  
  98. /* --------------------------------------------------------------------------------
  99. * Test input data(Cycles) taken from FIR Q15 module for differant cases of blockSize
  100. * and tapSize
  101. * --------------------------------------------------------------------------------- */
  102.  
  103. const float32_t B_f32[4] =
  104. {
  105.   782.0, 7577.0, 470.0, 4505.0
  106. };
  107.  
  108. /* --------------------------------------------------------------------------------
  109. * Formula to fit is  C1 + C2 * numTaps + C3 * blockSize + C4 * numTaps * blockSize
  110. * -------------------------------------------------------------------------------- */
  111.  
  112. const float32_t A_f32[16] =
  113. {
  114.   /* Const,   numTaps,   blockSize,   numTaps*blockSize */
  115.   1.0,     32.0,      4.0,     128.0,
  116.   1.0,     32.0,     64.0,    2048.0,
  117.   1.0,     16.0,      4.0,      64.0,
  118.   1.0,     16.0,     64.0,    1024.0,
  119. };
  120.  
  121.  
  122. /* ----------------------------------------------------------------------
  123. * Temporary buffers  for storing intermediate values
  124. * ------------------------------------------------------------------- */
  125. /* Transpose of A Buffer */
  126. float32_t AT_f32[16];
  127. /* (Transpose of A * A) Buffer */
  128. float32_t ATMA_f32[16];
  129. /* Inverse(Transpose of A * A)  Buffer */
  130. float32_t ATMAI_f32[16];
  131. /* Test Output Buffer */
  132. float32_t X_f32[4];
  133.  
  134. /* ----------------------------------------------------------------------
  135. * Reference ouput buffer C1, C2, C3 and C4 taken from MATLAB
  136. * ------------------------------------------------------------------- */
  137. const float32_t xRef_f32[4] = {73.0, 8.0, 21.25, 2.875};
  138.  
  139. float32_t snr;
  140.  
  141.  
  142. /* ----------------------------------------------------------------------
  143. * Max magnitude FFT Bin test
  144. * ------------------------------------------------------------------- */
  145.  
  146. int32_t main(void)
  147. {
  148.  
  149.   arm_matrix_instance_f32 A;      /* Matrix A Instance */
  150.   arm_matrix_instance_f32 AT;     /* Matrix AT(A transpose) instance */
  151.   arm_matrix_instance_f32 ATMA;   /* Matrix ATMA( AT multiply with A) instance */
  152.   arm_matrix_instance_f32 ATMAI;  /* Matrix ATMAI(Inverse of ATMA) instance */
  153.   arm_matrix_instance_f32 B;      /* Matrix B instance */
  154.   arm_matrix_instance_f32 X;      /* Matrix X(Unknown Matrix) instance */
  155.  
  156.   uint32_t srcRows, srcColumns;  /* Temporary variables */
  157.   arm_status status;
  158.  
  159.   /* Initialise A Matrix Instance with numRows, numCols and data array(A_f32) */
  160.   srcRows = 4;
  161.   srcColumns = 4;
  162.   arm_mat_init_f32(&A, srcRows, srcColumns, (float32_t *)A_f32);
  163.  
  164.   /* Initialise Matrix Instance AT with numRows, numCols and data array(AT_f32) */
  165.   srcRows = 4;
  166.   srcColumns = 4;
  167.   arm_mat_init_f32(&AT, srcRows, srcColumns, AT_f32);
  168.  
  169.   /* calculation of A transpose */
  170.   status = arm_mat_trans_f32(&A, &AT);
  171.  
  172.  
  173.   /* Initialise ATMA Matrix Instance with numRows, numCols and data array(ATMA_f32) */
  174.   srcRows = 4;
  175.   srcColumns = 4;
  176.   arm_mat_init_f32(&ATMA, srcRows, srcColumns, ATMA_f32);
  177.  
  178.   /* calculation of AT Multiply with A */
  179.   status = arm_mat_mult_f32(&AT, &A, &ATMA);
  180.  
  181.   /* Initialise ATMAI Matrix Instance with numRows, numCols and data array(ATMAI_f32) */
  182.   srcRows = 4;
  183.   srcColumns = 4;
  184.   arm_mat_init_f32(&ATMAI, srcRows, srcColumns, ATMAI_f32);
  185.  
  186.   /* calculation of Inverse((Transpose(A) * A) */
  187.   status = arm_mat_inverse_f32(&ATMA, &ATMAI);
  188.  
  189.   /* calculation of (Inverse((Transpose(A) * A)) *  Transpose(A)) */
  190.   status = arm_mat_mult_f32(&ATMAI, &AT, &ATMA);
  191.  
  192.   /* Initialise B Matrix Instance with numRows, numCols and data array(B_f32) */
  193.   srcRows = 4;
  194.   srcColumns = 1;
  195.   arm_mat_init_f32(&B, srcRows, srcColumns, (float32_t *)B_f32);
  196.  
  197.   /* Initialise X Matrix Instance with numRows, numCols and data array(X_f32) */
  198.   srcRows = 4;
  199.   srcColumns = 1;
  200.   arm_mat_init_f32(&X, srcRows, srcColumns, X_f32);
  201.  
  202.   /* calculation ((Inverse((Transpose(A) * A)) *  Transpose(A)) * B) */
  203.   status = arm_mat_mult_f32(&ATMA, &B, &X);
  204.  
  205.   /* Comparison of reference with test output */
  206.   snr = arm_snr_f32((float32_t *)xRef_f32, X_f32, 4);
  207.  
  208.   /*------------------------------------------------------------------------------
  209.   *            Initialise status depending on SNR calculations
  210.   *------------------------------------------------------------------------------*/
  211.   if ( snr > SNR_THRESHOLD)
  212.   {
  213.     status = ARM_MATH_SUCCESS;
  214.   }
  215.   else
  216.   {
  217.     status = ARM_MATH_TEST_FAILURE;
  218.   }
  219.  
  220.  
  221.   /* ----------------------------------------------------------------------
  222.   ** Loop here if the signals fail the PASS check.
  223.   ** This denotes a test failure
  224.   ** ------------------------------------------------------------------- */
  225.   if ( status != ARM_MATH_SUCCESS)
  226.   {
  227.     while (1);
  228.   }
  229.  
  230.   while (1);                             /* main function does not return */
  231. }
  232.  
  233.  /** \endlink */
  234.