........ Math Support Functions Source Code ..........

======= File-Name: 23_Math_Support_Functions.ino ==========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++ Begin Math Support Functions +++++++++++++++++++
//             Last Update: 30-08-2022  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//******* Matrix Multiplication Routine C=A*B ******************************
void MatrixMathMultiply(float* A, float* B, int m, int p, int n, float* C)
{
  // A = input matrix (m x p)
  // B = input matrix (p x n)
  // m = number of rows in A
  // p = number of columns in A = number of rows in B
  // n = number of columns in B
  // C = output matrix = A*B (m x n)

  int i, j, k;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
    {
      C[n * i + j] = 0;
      for (k = 0; k < p; k++)
        C[n * i + j] = C[n * i + j] + A[p * i + k] * B[n * k + j];
    }
}

//******* Matrix-Vector Multiplication Routine ******************
// V = A*U
void MatrixMathMatrixVectorMultiply(float* A, float* U, int rows, int cols, float* V)
 {
  // A = input matrix A(rows x cols)
  // U = input vector U(cols)
  // V = output vector V(rows)= A(rows x cols)*U(cols)

  int i, j, k;
  float Ah[rows][cols];   //hulp array
  float Uh[cols];         //hulp input vector
//  float Vh[rows];

  for (i=0;i