This is an automated email from the ASF dual-hosted git repository.
acassis pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx.git
The following commit(s) were added to refs/heads/master by this push:
new d8d77c249c8 libs/libdsp: Add Matrix operations
d8d77c249c8 is described below
commit d8d77c249c81916ad3f08e9698b9e82bd38ed575
Author: Alan Carvalho de Assis <[email protected]>
AuthorDate: Sat Jul 11 11:10:41 2026 -0300
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 <[email protected]>
---
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(+)
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 <dsp.h>
+#include <math.h>
+#include <string.h>
+
+/****************************************************************************
+ * 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;
+}
+
