Matrix Operation

1. Basic Setup

  • Open STM32CubeMX and generate basic code for your board.

  • Setup Makefile or CMakeLists.txt for ARM Math. See arm math tutorial.

  • Update Makefile or CMakeLists.txt to add matrix functions.

    C_SOURCES = \
... \
... \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c \
Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c

  • Add matrix check definition in Makefile or CMakeLists.txt.

    # C defines
C_DEFS = \
... \
-DARM_MATH_CM4 \
-DARM_MATH_MATRIX_CHECK

2. Test Matrix Functions

  • Navigate to Core > Src and open main.c.

  • Include headers.

    /* USER CODE BEGIN Includes */
#include <stdio.h>
#include "arm_math.h"
/* USER CODE END Includes */

Warning

Do not forget to add compiler flag -u _printf_float in Makefile or CMakeLists.txt to print floating point numbers otherwise it will not print the numbers.

Add to LDFLAGS.

LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
LDFLAGS += -u _printf_float

  • Overwrite definition of _write for printf as:

    /* USER CODE BEGIN 0 */
int _write(int file, char *data, int len)
{
  for (int i = 0; i < len; ++i)
  {
    ITM_SendChar(data[i]);
  }
  return len;
}
/* USER CODE END 0 */

  • Write code to print matrix.

    // ...

void print_matrix(arm_matrix_instance_f32 *matrix)
{
  for (int i = 0; i < matrix->numRows; i++)
  {
    for (int j = 0; j < matrix->numCols; j++)
    {
      printf("%f\t", matrix->pData[i * matrix->numCols + j]);
    }
    printf("\n");
  }
}
/* USER CODE END 0 */

  • Write code to test matrix functions.

    /* USER CODE BEGIN 2 */
  arm_matrix_instance_f32 m1;
  float m1data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  arm_mat_init_f32(&m1, 3, 3, m1data);

  arm_matrix_instance_f32 m2;
  float m2data[9] = {9, 8, 7, 6, 5, 4, 3, 2, 1};
  arm_mat_init_f32(&m2, 3, 3, m2data);

  arm_matrix_instance_f32 result;
  float resultdata[9];
  arm_mat_init_f32(&result, 3, 3, resultdata);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
    printf("Adding m1 and m2, result:\n");
    arm_mat_add_f32(&m1, &m2, &result);
    print_matrix(&result);
    HAL_Delay(1000);

    printf("Subtracting m1 and m2, result:\n");
    arm_mat_sub_f32(&m1, &m2, &result);
    print_matrix(&result);
    HAL_Delay(1000);

    printf("Multiplying m1 and m2, result:\n");
    arm_mat_mult_f32(&m1, &m2, &result);
    print_matrix(&result);
    HAL_Delay(1000);

    printf("Multiplying m1 by 2\n");
    arm_mat_scale_f32(&m1, 2, &result);
    print_matrix(&result);
    HAL_Delay(1000);

    printf("Transposing m1\n");
    arm_mat_trans_f32(&m1, &result);
    print_matrix(&result);
    HAL_Delay(1000);

    printf("Inverting m1\n");
    if (arm_mat_inverse_f32(&m1, &result) == ARM_MATH_SUCCESS)
    {
      print_matrix(&result);
    }
    else
    {
      printf("Matrix is not invertible\n");
    }
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */

3. Test Output:

Adding m1 and m2, result:
10.000000       10.000000       10.000000
10.000000       10.000000       10.000000
10.000000       10.000000       10.000000
Subtracting m1 and m2, result:
-8.000000       -6.000000       -4.000000
-2.000000       0.000000        2.000000
4.000000        6.000000        8.000000
Multiplying m1 and m2, result:
30.000000       24.000000       18.000000
84.000000       69.000000       54.000000
138.000000      114.000000      90.000000
Multiplying m1 by 2
2.000000        4.000000        6.000000
8.000000        10.000000       12.000000
14.000000       16.000000       18.000000
Transposing m1
1.000000        4.000000        7.000000
2.000000        5.000000        8.000000
3.000000        6.000000        9.000000
Inverting m1
Matrix is not invertible

4. Creating Matrix Class and Overloading Operators

  • Setup your Makefile or CMakeLists.txt``for ``C++. See cpp setup.

  • Create matrix32.hpp inside Core/Inc. Copy these contents.

    matrix32.hpp
      1/*
  2    Dynamic memory allocation is avoided.
  3    Template struct is used, so wisely use only few sizes of matrix otherwise it will consume lots of memory.
  4    No exeption is handled, instead used static assertion.
  5    Assign or copy data carefully especialy from where array of data through pointer is used to do.
  6    constructor = default is avoided, otherwise one class pointer will point other class data.
  7*/
  8
  9#ifndef MATRIX32_HPP
 10#define MATRIX32_HPP
 11
 12#include <stdio.h>
 13#include "arm_math.h"
 14
 15template <uint16_t numRows, uint16_t numCols>
 16class Matrix32
 17{
 18public:
 19    float32_t data[numRows * numCols];
 20    arm_matrix_instance_f32 arm_mat;
 21
 22    Matrix32() noexcept
 23    {
 24        arm_mat_init_f32(&arm_mat, numRows, numCols, data);
 25    }
 26
 27    Matrix32(const Matrix32 &other) noexcept
 28    {
 29        arm_mat_init_f32(&arm_mat, numRows, numCols, data);
 30        memcpy(arm_mat.pData, other.arm_mat.pData, numRows * numCols * 4);
 31    }
 32
 33    template <typename... Args>
 34    Matrix32(Args... values)
 35        : data{static_cast<float32_t>(values)...}
 36    {
 37        static_assert(sizeof...(values) == numRows * numCols,
 38                      "Incorrect number of values. If there is static_cast error just above this line, then it is probably be due to mismatched size of matrix passed to copy or assign.");
 39        arm_mat_init_f32(&arm_mat, numRows, numCols, data);
 40    }
 41
 42    Matrix32(const float32_t *pSrc) noexcept
 43    {
 44        arm_mat_init_f32(&arm_mat, numRows, numCols, data);
 45        memcpy(arm_mat.pData, pSrc, numRows * numCols * 4);
 46    }
 47
 48    Matrix32 &operator=(const Matrix32 &other) noexcept
 49    {
 50        memcpy(arm_mat.pData, other.arm_mat.pData, numRows * numCols * 4);
 51        return *this;
 52    }
 53
 54    Matrix32 &operator=(float32_t *pSrc) noexcept
 55    {
 56        memcpy(arm_mat.pData, pSrc, numRows * numCols * 4);
 57        return *this;
 58    }
 59
 60    Matrix32 operator+(const Matrix32 &rhs) const noexcept
 61    {
 62        Matrix32 dst;
 63        arm_mat_add_f32(&this->arm_mat, &rhs.arm_mat, &dst.arm_mat);
 64        return dst;
 65    }
 66
 67    Matrix32 operator-(const Matrix32 &rhs) const noexcept
 68    {
 69        Matrix32 dst;
 70        arm_mat_sub_f32(&this->arm_mat, &rhs.arm_mat, &dst.arm_mat);
 71        return dst;
 72    }
 73
 74    template <uint16_t numAny>
 75    Matrix32<numRows, numAny> operator*(const Matrix32<numCols, numAny> &rhs) const noexcept
 76    {
 77        Matrix32<numRows, numAny> dst;
 78        arm_mat_mult_f32(&this->arm_mat, &rhs.arm_mat, &dst.arm_mat);
 79        return dst;
 80    }
 81
 82    Matrix32<numCols, numRows> trans() const noexcept
 83    {
 84        Matrix32<numCols, numRows> dst;
 85        arm_mat_trans_f32(&this->arm_mat, &dst.arm_mat);
 86        return dst;
 87    }
 88
 89    Matrix32 inverse() const noexcept
 90    {
 91        static_assert(numRows == numCols,
 92                      "Inverse of rectangular matrix does not exists. Matrix must be square to have inverse.");
 93        Matrix32 dst;
 94        arm_status status = arm_mat_inverse_f32(&this->arm_mat, &dst.arm_mat);
 95        if (status == ARM_MATH_SINGULAR)
 96        {
 97            printf("EXCEPTION:: MATRIX INVERSION FAILED !!\n");
 98        }
 99
100        return dst;
101    }
102
103    Matrix32 operator*(const float32_t &rhs) const noexcept
104    {
105        Matrix32 dst;
106        arm_mat_scale_f32(&this->arm_mat, rhs, &dst.arm_mat);
107        return dst;
108    }
109
110    Matrix32 scale(const float32_t &scaler) const noexcept
111    {
112        Matrix32 dst;
113        arm_mat_scale_f32(&this->arm_mat, scaler, &dst.arm_mat);
114        return dst;
115    }
116
117    void fill(const float32_t &value) noexcept
118    {
119        arm_fill_f32(value, arm_mat.pData, numRows * numCols);
120    }
121
122    void setIdentity() noexcept
123    {
124        static_assert(numRows == numCols, "Only square matrix can be identity matrix");
125        for (uint16_t i = 0; i < numRows; ++i)
126        {
127            for (uint16_t j = 0; j < numCols; ++j)
128            {
129                arm_mat.pData[i * numCols + j] = (i == j) ? 1.0f : 0.0f;
130            }
131        }
132    }
133
134    void printData() const noexcept
135    {
136        for (uint16_t i = 0; i < numRows; ++i)
137        {
138            for (uint16_t j = 0; j < numCols; ++j)
139            {
140                printf("%f\t", arm_mat.pData[i * numCols + j]);
141            }
142            printf("\n");
143        }
144    }
145
146    ~Matrix32() = default;
147};
148
149#endif // MATRIXF32_HPP

  • Create app.h inside Core/Inc and app.cpp inside Core/Src. Copy these contents.

    app.h
     1#ifndef APP_H
 2#define APP_H
 3
 4#ifdef __cplusplus
 5extern "C" {
 6#endif
 7
 8void init();
 9
10void run();
11
12#ifdef __cplusplus
13}
14#endif
15
16#endif // APP_H
    app.cpp
     1#include "main.h"
 2#include "matrix32.hpp"
 3
 4#include "app.h"
 5
 6Matrix32<3, 3> m1 = {1, 2, 3, 4, 5, 6, 7, 8, 9};
 7Matrix32<3, 3> m2 = {9, 8, 7, 6, 5, 4, 3, 2, 1};
 8Matrix32<3, 3> result;
 9
10void init()
11{
12    // nothing to do here
13}
14
15void run()
16{
17    printf("Adding m1 and m2, result:\n");
18    result = m1 + m2;
19    result.printData();
20    HAL_Delay(1000);
21
22    printf("Subtracting m1 and m2, result:\n");
23    result = m1 - m2;
24    result.printData();
25    HAL_Delay(1000);
26
27    printf("Multiplying m1 and m2, result:\n");
28    result = m1 * m2;
29    result.printData();
30    HAL_Delay(1000);
31
32    printf("Multiplying m1 by 2\n");
33    result = m1.scale(2);
34    result.printData();
35    HAL_Delay(1000);
36
37    printf("Transposing m1\n");
38    result = m1.trans();
39    result.printData();
40    HAL_Delay(1000);
41
42    printf("Inverting m1\n");
43    result = m1.inverse();
44    result.printData();
45    HAL_Delay(1000);
46}

  • Include app.h in main.c.

    /* USER CODE BEGIN Includes */
// ..
#include "app.h"
/* USER CODE END Includes */

  • Call init() and run() functions. Also comment out previous codes from while loop.

    /* USER CODE BEGIN 2 */
// arm_matrix_instance_f32 m1;
// float m1data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
// arm_mat_init_f32(&m1, 3, 3, m1data);

// arm_matrix_instance_f32 m2;
// float m2data[9] = {9, 8, 7, 6, 5, 4, 3, 2, 1};
// arm_mat_init_f32(&m2, 3, 3, m2data);

// arm_matrix_instance_f32 result;
// float resultdata[9];
// arm_mat_init_f32(&result, 3, 3, resultdata);

init();
/* USER CODE END 2 */

/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
  /* USER CODE END WHILE */
  run();
  // printf("Adding m1 and m2, result:\n");
  // arm_mat_add_f32(&m1, &m2, &result);
  // print_matrix(&result);
  // HAL_Delay(1000);

  // printf("Subtracting m1 and m2, result:\n");
  // arm_mat_sub_f32(&m1, &m2, &result);
  // print_matrix(&result);
  // HAL_Delay(1000);

  // printf("Multiplying m1 and m2, result:\n");
  // arm_mat_mult_f32(&m1, &m2, &result);
  // print_matrix(&result);
  // HAL_Delay(1000);

  // printf("Multiplying m1 by 2\n");
  // arm_mat_scale_f32(&m1, 2, &result);
  // print_matrix(&result);
  // HAL_Delay(1000);

  // printf("Transposing m1\n");
  // arm_mat_trans_f32(&m1, &result);
  // print_matrix(&result);
  // HAL_Delay(1000);

  // printf("Inverting m1\n");
  // if (arm_mat_inverse_f32(&m1, &result) == ARM_MATH_SUCCESS)
  // {
  //   print_matrix(&result);
  // }
  // else
  // {
  //   printf("Matrix is not invertible\n");
  // }
  /* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */

  • Add sources to Makefile or CMakeLists.txt.

    # C sources
C_SOURCES = \
... \
Drivers/CMSIS/DSP/Source/SupportFunctions/arm_fill_f32.c

    # CXX sources
CXX_SOURCES =  \
Core/Src/app.cpp

5. Test Ouput of Matrix Class

Adding m1 and m2, result:
10.000000       10.000000       10.000000
10.000000       10.000000       10.000000
10.000000       10.000000       10.000000
Subtracting m1 and m2, result:
-8.000000       -6.000000       -4.000000
-2.000000       0.000000        2.000000
4.000000        6.000000        8.000000
Multiplying m1 and m2, result:
30.000000       24.000000       18.000000
84.000000       69.000000       54.000000
138.000000      114.000000      90.000000
Multiplying m1 by 2
2.000000        4.000000        6.000000
8.000000        10.000000       12.000000
14.000000       16.000000       18.000000
Transposing m1
1.000000        4.000000        7.000000
2.000000        5.000000        8.000000
3.000000        6.000000        9.000000
Inverting m1
EXCEPTION:: MATRIX INVERSION FAILED !!