From d8d77c249c81916ad3f08e9698b9e82bd38ed575 Mon Sep 17 00:00:00 2001 From: Alan Carvalho de Assis Date: Sat, 11 Jul 2026 11:10:41 -0300 Subject: [PATCH] libs/libdsp: Add Matrix operations This commit adds support for matrix operation on libdsp. The code came from: https://github.com/DjVul/Extended-Kalman-Filter---STM32/blob/main/Core/Src/matrix_utils.c Signed-off-by: Alan C. Assis --- LICENSE | 7 + include/dsp.h | 149 ++++++++++++++++++++ libs/libdsp/CMakeLists.txt | 1 + libs/libdsp/Makefile | 1 + libs/libdsp/lib_matrix.c | 270 +++++++++++++++++++++++++++++++++++++ 5 files changed, 428 insertions(+) create mode 100644 libs/libdsp/lib_matrix.c diff --git a/LICENSE b/LICENSE index a22fd18f559..95bd2a00c68 100644 --- a/LICENSE +++ b/LICENSE @@ -8696,3 +8696,10 @@ http://www.opensource.org/licenses. Any Red Hat trademarks that are incorporated in the source code or documentation are not subject to the BSD License and may only be used or replicated with the express permission of Red Hat, Inc. + +libs/libdsp/lib_matrix.c +======================== + + Copyright (C) 2026 Djordje Vulovic + + SPDX-License-Identifier: Apache-2.0 diff --git a/include/dsp.h b/include/dsp.h index 4dce26ea6c1..0d67ef858e2 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -619,6 +619,155 @@ void avg_filter_data_init(FAR struct avg_filter_data_s *data, float prev_avg, float k); float avg_filter(FAR struct avg_filter_data_s *data, float x); +/**************************************************************************** + * Name: matrix_add + * + * Description: + * Matrix addition + * + * C (m x n) = A (m x n) + B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_add(const float *A, const float *B, float *C, + uint8_t m, uint8_t n); + +/**************************************************************************** + * Name: matrix_sub + * + * Description: + * Matrix subtraction + * C (m x n) = A (m x n) - B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_sub(const float *A, const float *B, float *C, + uint8_t m, uint8_t n); + +/**************************************************************************** + * Name: matrix_mul + * + * Description: + * Matrix multiplication + * + * C (m x n) = A (m x n) * B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_mul(const float *A, const float *B, float *C, + uint8_t m, uint8_t n, uint8_t p); + +/**************************************************************************** + * Name: matrix_mul_transpose + * + * Description: + * Matrix multiplication with a transposed matrix + * + * C (m x n) = A (m x n) * B (p x n)^T + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (p x n) - will be transposed + * C Pointer to the result matrix (m x p) + * m Number of rows in matrix A + * n Number of columns in matrices A and B + * p Number of rows in matrix B (becomes the number of columns + * in the result) + * + ****************************************************************************/ + +void matrix_mul_transpose(const float *A, const float *B, float *C, + uint8_t m, uint8_t n, uint8_t p); + +/**************************************************************************** + * Name: matrix_transpose + * + * Description: + * Matrix transposed + * + * B (m x n) = A^T (m x n)^T + * + * A Pointer to the input matrix (m x n) + * B Pointer to the output matrix (n x m) + * m Number of rows in the input matrix + * n Number of columns in the input matrix + * + ****************************************************************************/ + +void matrix_transpose(const float *A, float *B, uint8_t m, uint8_t n); + +/**************************************************************************** + * Name: matrix_scalar_mul + * + * Description: + * Matrix-scalar multiplication + * + * B (m x n) = A (m x n) * s + * + * A Pointer to the input matrix (m x n) + * s Scalar value (float) + * B Pointer to the output matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_scalar_mul(const float *A, float s, float *B, + uint8_t m, uint8_t n); + +/**************************************************************************** + * Name: matrix_copy + * + * Description: + * Matrix copy + * + * B (m x n) = A (m x n) + * + * A Pointer to the source matrix (m x n) + * B Pointer to the destination matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_copy(const float *A, float *B, uint8_t m, uint8_t n); + +/**************************************************************************** + * Name: matrix_copy + * + * Description: + * 3x3 matrix inversion + * + * inv (3x3) = A^-1 (3x3) + * + * A Input matrix (3x3) + * inv Output matrix (3x3) - inverse matrix + * + * Returns: + * 1 if the inversion was successful, + * 0 if the matrix is singular. + * + ****************************************************************************/ + +uint8_t matrix_inv_3x3(const float A[3][3], float inv[3][3]); + #undef EXTERN #if defined(__cplusplus) } diff --git a/libs/libdsp/CMakeLists.txt b/libs/libdsp/CMakeLists.txt index 82c40895115..2625d1b8b1f 100644 --- a/libs/libdsp/CMakeLists.txt +++ b/libs/libdsp/CMakeLists.txt @@ -24,6 +24,7 @@ if(CONFIG_LIBDSP) dsp lib_pid.c lib_svm.c + lib_matrix.c lib_transform.c lib_observer.c lib_observer_b16.c diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile index 1dc4524d21f..551f42ae4a4 100644 --- a/libs/libdsp/Makefile +++ b/libs/libdsp/Makefile @@ -26,6 +26,7 @@ ifeq ($(CONFIG_LIBDSP),y) CSRCS += lib_avg.c CSRCS += lib_pid.c CSRCS += lib_svm.c +CSRCS += lib_matrix.c CSRCS += lib_transform.c CSRCS += lib_observer.c CSRCS += lib_foc.c diff --git a/libs/libdsp/lib_matrix.c b/libs/libdsp/lib_matrix.c new file mode 100644 index 00000000000..f20881fc315 --- /dev/null +++ b/libs/libdsp/lib_matrix.c @@ -0,0 +1,270 @@ +/**************************************************************************** + * libs/libdsp/lib_matrix.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * From: Djordje Vulovic's Extended-Kalman-Filter for STM32 + * Licensed as Apache 2.0 to be included on NuttX: + * https://github.com/DjVul/Extended-Kalman-Filter---STM32/issues/1 + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: matrix_add + * + * Description: + * Matrix addition + * + * C (m x n) = A (m x n) + B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_add(const float *A, const float *B, float *C, + uint8_t m, uint8_t n) +{ + for (uint8_t i = 0; i < m; i++) + { + for (uint8_t j = 0; j < n; j++) + { + C[i * n + j] = A[i * n + j] + B[i * n + j]; + } + } +} + +/**************************************************************************** + * Name: matrix_sub + * + * Description: + * Matrix subtraction + * C (m x n) = A (m x n) - B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_sub(const float *A, const float *B, float *C, + uint8_t m, uint8_t n) +{ + for (uint8_t i = 0; i < m; i++) + { + for (uint8_t j = 0; j < n; j++) + { + C[i * n + j] = A[i * n + j] - B[i * n + j]; + } + } +} + +/**************************************************************************** + * Name: matrix_mul + * + * Description: + * Matrix multiplication + * + * C (m x n) = A (m x n) * B (m x n) + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (m x n) + * C Pointer to the result matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_mul(const float *A, const float *B, float *C, + uint8_t m, uint8_t n, uint8_t p) +{ + for (uint8_t i = 0; i < m; i++) + { + for (uint8_t j = 0; j < p; j++) + { + C[i * p + j] = 0.0f; + + for (uint8_t k = 0; k < n; k++) + { + C[i * p + j] += A[i * n + k] * B[k * p + j]; + } + } + } +} + +/**************************************************************************** + * Name: matrix_mul_transpose + * + * Description: + * Matrix multiplication with a transposed matrix + * + * C (m x n) = A (m x n) * B (p x n)^T + * + * A Pointer to the first matrix (m x n) + * B Pointer to the second matrix (p x n) - will be transposed + * C Pointer to the result matrix (m x p) + * m Number of rows in matrix A + * n Number of columns in matrices A and B + * p Number of rows in matrix B (becomes the number of columns + * in the result) + * + ****************************************************************************/ + +void matrix_mul_transpose(const float *A, const float *B, float *C, + uint8_t m, uint8_t n, uint8_t p) +{ + for (uint8_t i = 0; i < m; i++) + { + for (uint8_t j = 0; j < p; j++) + { + C[i * p + j] = 0.0f; + + for (uint8_t k = 0; k < n; k++) + { + /* B^T is B[j][k] */ + + C[i * p + j] += A[i * n + k] * B[j * n + k]; + } + } + } +} + +/**************************************************************************** + * Name: matrix_transpose + * + * Description: + * Matrix transposed + * + * B (m x n) = A^T (m x n)^T + * + * A Pointer to the input matrix (m x n) + * B Pointer to the output matrix (n x m) + * m Number of rows in the input matrix + * n Number of columns in the input matrix + * + ****************************************************************************/ + +void matrix_transpose(const float *A, float *B, uint8_t m, uint8_t n) +{ + for (uint8_t i = 0; i < m; i++) + { + for (uint8_t j = 0; j < n; j++) + { + B[j * m + i] = A[i * n + j]; + } + } +} + +/**************************************************************************** + * Name: matrix_scalar_mul + * + * Description: + * Matrix-scalar multiplication + * + * B (m x n) = A (m x n) * s + * + * A Pointer to the input matrix (m x n) + * s Scalar value (float) + * B Pointer to the output matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_scalar_mul(const float *A, float s, float *B, + uint8_t m, uint8_t n) +{ + uint16_t total = (uint16_t) m * n; + uint16_t i; + + for (i = 0; i < total; i++) + { + B[i] = A[i] * s; + } +} + +/**************************************************************************** + * Name: matrix_copy + * + * Description: + * Matrix copy + * + * B (m x n) = A (m x n) + * + * A Pointer to the source matrix (m x n) + * B Pointer to the destination matrix (m x n) + * m Number of rows + * n Number of columns + * + ****************************************************************************/ + +void matrix_copy(const float *A, float *B, uint8_t m, uint8_t n) +{ + uint16_t total = (uint16_t) m * n; + + memcpy(B, A, total * sizeof(float)); +} + +/**************************************************************************** + * Name: matrix_inv_3x3 + * + * Description: + * 3x3 matrix inversion + * + * inv (3x3) = A^-1 (3x3) + * + * A Input matrix (3x3) + * inv Output matrix (3x3) - inverse matrix + * + * Returns: + * 1 if the inversion was successful, + * 0 if the matrix is singular. + * + ****************************************************************************/ + +uint8_t matrix_inv_3x3(const float A[3][3], float inv[3][3]) +{ + float det = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + - A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + float inv_det = 1.0f / det; + + /* Check whether the matrix is singular */ + + if (fabsf(det) < 1e-10f) + { + return 0; + } + + inv[0][0] = (A[1][1] * A[2][2] - A[1][2] * A[2][1]) * inv_det; + inv[0][1] = (A[0][2] * A[2][1] - A[0][1] * A[2][2]) * inv_det; + inv[0][2] = (A[0][1] * A[1][2] - A[0][2] * A[1][1]) * inv_det; + + inv[1][0] = (A[1][2] * A[2][0] - A[1][0] * A[2][2]) * inv_det; + inv[1][1] = (A[0][0] * A[2][2] - A[0][2] * A[2][0]) * inv_det; + inv[1][2] = (A[0][2] * A[1][0] - A[0][0] * A[1][2]) * inv_det; + + inv[2][0] = (A[1][0] * A[2][1] - A[1][1] * A[2][0]) * inv_det; + inv[2][1] = (A[0][1] * A[2][0] - A[0][0] * A[2][1]) * inv_det; + inv[2][2] = (A[0][0] * A[1][1] - A[0][1] * A[1][0]) * inv_det; + + return 1; +} +