AnsweredAssumed Answered

Matlab-like math library on top of CMSIS -> Thoughts?

Question asked by Valentin on Dec 11, 2016
Latest reply on Jan 12, 2017 by bouzereau.renaud

Hi,

I recently started a hobby project which eventually ends up needing an unscented kalman filter. So I'm now facing the challenge to port my matlab code to the STM32. I know there is embedded target code generator etc but I want to build it myself to also learn something in the process and build my own C math toolbox.

 

After some research I found that the CMSIS-DSP library is quite powerful, handles matrizes and seems optimized for CortexM cpus. It's even able to make use of the M4 DSP instructions and FPU.

 

Therefore, I decided to use it as a foundation for my project.Alternatives like Eigen, Armadillo or gsl seemed 1. overkill and 2. difficult to implement on an embedde platform (let alone the potential lack of FPU/DSP instructions use and the bigger code size).

But some needed functionality in CMSIS is still missing, like diag(), ones(), chol() etc. So I set off to build my own matlab.c/.h library to act on top of CMSIS. The idea is to implement functions with the same name as the ones in matlab and to use CMSIS data types + functions and standard C style in the best (speed-wise and code-readability-wise) way possible.

 

I'd like to get an opinion about my approach:

- Am I re-inventing the wheel in vein? Should I rather stick to an established library?

- Functions like ones() pose the question about malloc. It would be way more straightofrward if I malloced a matrix instance in ones() and returned it instead of relying on working with pre-allocated memory. But I somewhat fear the additional memory management involved. What do you guys think?

- Any other thoughts? Should I maybe put this on github? Would someone else be interested in collaborating and adding own matlab functions to this?

 

Here is a snippet of the first code I've written today (actually the complete file so far) so you can get an idea of how it all works:

/*
* matlab.c
*
*  Created on: 11.12.2016
*      Author: nose
*
*      Tool library to add matlab-like functionality on top of CMSIS math library.
*      The goal is to add all matlab functions over time with C-Style parameters.
*
*      Requirements:
*      ARM Cortex-M CPU (M4/7 for SIMD instructions) and CMSIS-DSP library
*
*      As this library uses 32bit floats, a FPU is highly recommended (M4/M7).
*      Maybe I'll do another one with Q31/Q15 fixed point math, too.
*      No 64bit math in here (no doubles) for speed reasons.
*
*      Note: All arm_matrix_instance_f32 variables used as parameters MUST be initialized first.
*      Each function assumes allocated memory for the data array. NO dynamic memory allocation.
*/


/*
* undefine MAT_EXTRA_CHECKS for speedup
*/

#define MAT_EXTRA_CHECKS

#include "matlab.h"

/**
* Fills the given Matrix with ones.
* @param pMatrix     The matrix to fill. Needs numRows and numCols set to final values first!
* @return               ARM_MATH_SUCCESS or ARM_MATH_SIZE_MISMATCH
*/

arm_status mat_ones(arm_matrix_instance_f32 *pMatrix) {
#ifdef MAT_EXTRA_CHECKS
     if (pMatrix->numCols < 1 || pMatrix->numRows < 1)
          return ARM_MATH_SIZE_MISMATCH;

     // pDataVector not initialized?
     if (&(pMatrix->pData) == NULL)
          return ARM_MATH_ARGUMENT_ERROR;
#endif

     for (int i = 0; i < pMatrix->numCols * pMatrix->numRows; i++) {
          pMatrix->pData[i] = 1.0f;
     }
     return ARM_MATH_SUCCESS;
}

/**
* Takes an input vector (matrix with numCols OR numRows == 1) and creates a diagonal matrix
* with pVecIn along the diagonal.
* @param pVecIn          Input Vector (length N)
* @param pMatOut          Output Matrix (has to be size NxN)
* @return                    ARM_MATH_SUCCESS if OK, ARM_MATH_SIZE_MISMATCH if error
*/

arm_status mat_diag(arm_matrix_instance_f32 *pVecIn, arm_matrix_instance_f32 *pMatResult) {

#ifdef MAT_EXTRA_CHECKS
     // pDataVector not initialized?
     if (&(pVecIn->pData) == NULL)
          return ARM_MATH_ARGUMENT_ERROR;
     if (&(pMatResult->pData) == NULL)
          return ARM_MATH_ARGUMENT_ERROR;

     // only accept vectors!
     if (pVecIn->numCols != 1 && pVecIn->numRows != 1) {
          return ARM_MATH_SIZE_MISMATCH;
     }
     // result matrix size checks
     if (pMatResult->numCols != pMatResult->numRows)
          return ARM_MATH_SIZE_MISMATCH;
#endif

     uint16_t size = pVecIn->numCols > pVecIn->numRows ? pVecIn->numCols : pVecIn->numRows;

#ifdef MAT_EXTRA_CHECKS
     // result matrix size checks
     if (pMatResult->numCols != size)
          return ARM_MATH_SIZE_MISMATCH;
#endif

     memset(pMatResult->pData, 0, size * size);

     for (int i = 0; i < size; i++) {
          pMatResult->pData[i + i * size] = pVecIn->pData[i];
     }

     return ARM_MATH_SUCCESS;
}

/*
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

double *cholesky(double *A, int n) {
double *L = (double*)calloc(n * n, sizeof(double));
if (L == NULL)
exit(EXIT_FAILURE);

for (int i = 0; i < n; i++)
for (int j = 0; j < (i+1); j++) {
double s = 0;
for (int k = 0; k < j; k++)
s += L[i * n + k] * L[j * n + k];
L[i * n + j] = (i == j) ?
sqrt(A[i * n + i] - s) :
(1.0 / L[j * n + j] * (A[i * n + j] - s));
}

return L;
}
*/

/**
* Cholesky separation of pMatIn.
* MUST initialize pMatResult before passing to this.
* @param pMatIn          NxN input matrix
* @param pMatResult       NxN output matrix
* @return                    ARM_MATH_ARGUMENT_ERROR if matrizes uninitialized
*                               ARM_MATH_SIZE_MISMATCH on matrix size mismatches
*                               ARM_MATH_SUCCESS on success
*/

arm_status mat_chol(arm_matrix_instance_f32 *pMatIn, arm_matrix_instance_f32 *pMatResult) {

#ifdef MAT_EXTRA_CHECKS
     // pDataVector not initialized?
     if (&(pMatIn->pData) == NULL)
          return ARM_MATH_ARGUMENT_ERROR;
     if (&(pMatResult->pData) == NULL)
          return ARM_MATH_ARGUMENT_ERROR;

     if (pMatIn->numCols != pMatIn->numRows)
          return ARM_MATH_SIZE_MISMATCH;
     if (pMatResult->numCols != pMatIn->numCols)
          return ARM_MATH_SIZE_MISMATCH;
     if (pMatResult->numRows != pMatIn->numRows)
          return ARM_MATH_SIZE_MISMATCH;
#endif

     for (int i = 0; i < pMatIn->numCols; i++)
          for (int j = 0; j < (i + 1); j++) {
               float32_t s = 0;
               for (int k = 0; k < j; k++) {
                    s += pMatResult->pData[i * pMatIn->numCols + k]
                              * pMatResult->pData[j * pMatIn->numCols + k];
               }
               if (i == j) {
                    arm_sqrt_f32(pMatIn->pData[i * pMatIn->numCols + i] - s,
                              pMatResult->pData[i * pMatIn->numCols + j]);
//                    pMatResult->pData[i * pMatIn->numCols + j] = sqrt(pMatIn->pData[i * pMatIn->numCols + i] - s);
               } else {
                    pMatResult->pData[i * pMatIn->numCols + j] = (1.0
                              / pMatResult->pData[j * pMatIn->numCols + j]
                              * (pMatIn->pData[i * pMatIn->numCols + j] - s));
               }
          }

     return ARM_MATH_SUCCESS;
}

 

/*
* matlab.h
*
*  Created on: 11.12.2016
*      Author: nose
*
*      Tool library to add matlab-like functionality on top of CMSIS math library.
*      The goal is to add all matlab functions over time with C-Style parameters.
*
*      Requirements:
*      ARM Cortex-M cpu and CMSIS-DSP library
*/


#ifndef MATLAB_H_
#define MATLAB_H_

#define __FPU_PRESENT 1U
#include "arm_math.h"
#include "stdint.h"


arm_status mat_ones(arm_matrix_instance_f32 *pMatrix);
arm_status mat_diag(arm_matrix_instance_f32 *pVecIn, arm_matrix_instance_f32 *pMatResult);
arm_status mat_chol(arm_matrix_instance_f32 *pMatIn, arm_matrix_instance_f32 *pMatResult);

#endif /* MATLAB_H_ */

 

(or would it be better to make the whole thing a C++ project? But then I could also use Eigen, I suppose ... did anybody get Eigen running on STM32?)

Outcomes