matrix_math.cpp File Reference

#include <Arduino.h>
#include "maths/matrix_math.h"

Functions
void	VectorfFill (float *vec, float fill, size_t n)
	USES SINGLE-PRECISION FLOATS! More...
void	VectorfAdd (float *c, float *a, float *b, size_t n)
	Add two vectors together and output result to a new vector. More...
void	VectorfAccumulate (float *a, float *b, size_t n)
	a <- a + b. More...
void	VectorfSubtract (float *c, float *a, float *b, size_t n)
	Subtract two vectors and output result to a new vector. More...
void	MatrixFill (float fill, float *A, size_t rows, size_t cols)
	Fill a matrix with a specified value. More...
void	MatrixTranspose (float *A, float *At, size_t arows, size_t acols)
	Compute the transpose of a matrix and output it to another one. More...
void	MatrixTransposeSquare (float *A, size_t n)
	Compute the transpose of a SQUARE (n, n) matrix in-place. More...
void	MatrixAdd (float *C, float *A, float *B, size_t rows, size_t cols)
	C <- A + B. More...
void	MatrixAddIdentity (float *A, size_t rows, size_t cols)
	A <- I + A. More...
void	MatrixSubtractIdentity (float *A, size_t rows, size_t cols)
	A <- I - A. More...
void	MatrixAccumulate (float *A, float *B, size_t rows, size_t cols)
	A <- A + B. More...
void	MatrixSubtract (float *C, float *A, float *B, size_t rows, size_t cols)
	C <- A - B. More...
void	MatrixSubAccumulate (float *A, float *B, size_t rows, size_t cols)
	A <- A - B. More...
void	MatrixNegate (float *A, size_t rows, size_t cols)
	Multiply all elements in a matrix by -1. More...
void	MatrixVectorfMult (float *outVec, float *A, float *b, size_t rows, size_t cols)
	c <- A * b. More...
void	MatrixMultiply (float *C, float *A, float *B, size_t aRows, size_t aCols, size_t bRows, size_t bCols)
	C <- A * B. More...
void	MatrixMultiply_ABt (float *C, float *A, float *B, size_t arows, size_t acols, size_t brows)
	C <- A * B^T. More...
bool	MatrixInverseCholesky (float *A, size_t n)
	Compute the inverse of matrix A via Cholesky decomposition. More...
bool	_MatrixCholeskyDecomp (float *A, size_t n)
	Perform Cholesky decomposition on a square, symmetric, positive definite matrix. More...
bool	_MatrixLowerTriangularInverse (float *A, size_t n)
	Compute the inverse of a lower triangular matrix. More...

Function Documentation

_MatrixCholeskyDecomp()

bool _MatrixCholeskyDecomp	(	float *	A,
		size_t	n
	)

Perform Cholesky decomposition on a square, symmetric, positive definite matrix.

Used in the MatrixInverseCholesky() routine. Original code can be found at: http://www.mymathlib.com/matrices/

_MatrixLowerTriangularInverse()

bool _MatrixLowerTriangularInverse	(	float *	A,
		size_t	n
	)

Compute the inverse of a lower triangular matrix.

Used in the MatrixInverseCholesky() function. Original code can be found at: http://www.mymathlib.com/matrices/

MatrixAccumulate()

void MatrixAccumulate	(	float *	A,
		float *	B,
		size_t	rows,
		size_t	cols
	)

A <- A + B.

Add matrix B to matrix A in-place. A and B must have identical dimensions!

Parameters

A	Matrix A
B	Matrix B
rows	Matrix rows
cols	Matrix columns

MatrixAdd()

void MatrixAdd	(	float *	C,
		float *	A,
		float *	B,
		size_t	rows,
		size_t	cols
	)

C <- A + B.

Add matrix A to matrix B and output result to a new array C. MAKE SURE THE DIMENSIONS OF THE ARRAYS MATCH!

Parameters

C	Output matrix
A	Matrix A
B	Matrix B
rows	Matrix rows
cols	Matrix columns

MatrixAddIdentity()

void MatrixAddIdentity	(	float *	A,
		size_t	rows,
		size_t	cols
	)

A <- I + A.

Add identity matrix to matrix A in-place.

Parameters

A	Matrix
rows	Matrix rows
cols	Matrix columns

MatrixFill()

void MatrixFill	(	float	fill,
		float *	A,
		size_t	rows,
		size_t	cols
	)

Fill a matrix with a specified value.

Use this to initialize arrays before they're used.

Parameters

fill	Value to fill array with
A	Matrix of interest
r	Rows
c	Columns

MatrixInverseCholesky()

bool MatrixInverseCholesky	(	float *	A,
		size_t	n
	)

Compute the inverse of matrix A via Cholesky decomposition.

This algorithm outputs replaces the input matrix with it's inverse. Note that Cholesky decomposition only works for square, symmetric, positive definite matrices. The original algorithm cab be found at: http://www.mymathlib.com/c_source/matrices/linearsystems/choleski.c

Parameters

A	Input matrix, SPD
n	Rows /columns of the square matrix

Returns

True if successful, false if not.

MatrixMultiply()

void MatrixMultiply	(	float *	C,
		float *	A,
		float *	B,
		size_t	aRows,
		size_t	aCols,
		size_t	bRows,
		size_t	bCols
	)

C <- A * B.

Perform matrix multiplication for two matrices. Outputs result to a new array C. MAKE SURE A COLUMNS EQUAL B ROWS. OUTPUT DIMENSION SHOULD BE (A_ROWS, B_COLS).

Parameters

A	First matrix
B	Second matrix
C	Output array
aRows	Rows of A
aCols	Columns of A
bRows	Rows of B
bCols	Columns of B

MatrixMultiply_ABt()

void MatrixMultiply_ABt	(	float *	C,
		float *	A,
		float *	B,
		size_t	arows,
		size_t	acols,
		size_t	brows
	)

C <- A * B^T.

Multiply matrix A by the transpose of matrix B, i.e. C = A*B'. A columns must match B rows! Outputs result to matrix C.

Parameters

C	Output matrix (Arows, Brows)
A	Matrix A (Arows, Acols)
B	Matrix B (Brows, Bcols)
arows	Matrix A rows
acols	Matrix A columns
brows	Matrix B rows

MatrixNegate()

void MatrixNegate	(	float *	A,
		size_t	rows,
		size_t	cols
	)

Multiply all elements in a matrix by -1.

Parameters

A	Matrix
rows	Matrix rows
cols	Matrix columns

MatrixSubAccumulate()

void MatrixSubAccumulate	(	float *	A,
		float *	B,
		size_t	rows,
		size_t	cols
	)

A <- A - B.

Subtract matrix B from matrix A in-place. A and B must have identical dimensions!

Parameters

A	Matrix A
B	Matrix B
rows	Matrix rows
cols	Matrix columns

MatrixSubtract()

void MatrixSubtract	(	float *	C,
		float *	A,
		float *	B,
		size_t	rows,
		size_t	cols
	)

C <- A - B.

Subtract matrix B from matrix A and output result to a new array C. MAKE SURE THE DIMENSIONS OF THE ARRAYS MATCH!

Parameters

C	Output matrix
A	Matrix A
B	Matrix B
rows	Matrix rows
cols	Matrix columns

MatrixSubtractIdentity()

void MatrixSubtractIdentity	(	float *	A,
		size_t	rows,
		size_t	cols
	)

A <- I - A.

Subtract matrix A from an identity matrix in-place.

Parameters

A	Matrix
rows	Matrix rows
cols	Matrix columns

MatrixTranspose()

void MatrixTranspose	(	float *	A,
		float *	At,
		size_t	arows,
		size_t	acols
	)

Compute the transpose of a matrix and output it to another one.

Matrix "A" has dimensions (r, c) and the transpose "At" has dimensions (c, r). See the following link for original source code: http://www.mymathlib.com/c_source/matrices/datamovement/transpose_matrix.c

Parameters

A	Original matrix
At	Transpose of matrix
arows	Matrix rows in the orig.
acols	Matrix columns in the orig.

MatrixTransposeSquare()

void MatrixTransposeSquare	(	float *	A,
		size_t	n
	)

Compute the transpose of a SQUARE (n, n) matrix in-place.

See the following link for original source code: http://www.mymathlib.com/c_source/matrices/datamovement/transpose_square_matrix.c

Parameters

A	Original matrix
n	Rows/columns of the square matrix

MatrixVectorfMult()

void MatrixVectorfMult	(	float *	outVec,
		float *	A,
		float *	b,
		size_t	rows,
		size_t	cols
	)

c <- A * b.

Perform matrix-vector multiplication and output result to a new array. Make sure that the matrix columns match the vector rows!

Parameters

outVec	Vectorf to output result to
A	Matrix
b	Vectorf
rows	Matrix rows
cols	Matrix columns

VectorfAccumulate()

void VectorfAccumulate	(	float *	a,
		float *	b,
		size_t	n
	)

a <- a + b.

Add two vectors in-place and output result to vector a. Assumes vectors have similar dimensions.

Parameters

a	Pointer to first vector
b	Pointer to second vector
n	Length of vector

VectorfAdd()

void VectorfAdd	(	float *	c,
		float *	a,
		float *	b,
		size_t	n
	)

Add two vectors together and output result to a new vector.

Assumes vectors have similar dimensions. c <- a + b

Parameters

a	Pointer to first vector
b	Pointer to second vector
c	Pointer to output vector
n	Length of vector

VectorfFill()

void VectorfFill	(	float *	vec,
		float	fill,
		size_t	n
	)

USES SINGLE-PRECISION FLOATS!

The library assumes the 2D array is one long dynamic array in memory (row major). The array indexing may be a bit strange, but since we allocated one long array in memory, it sould help a bit with cache performance. See below for an example of this.

I give the following site credit for the matrix math algorithms. It was a significant inspiration and resource for algorithms! http://www.mymathlib.com/matrices/

Vectorf operations include:

• Fill
• Addition
• Accumulate (in-place)
• Subtract

Matrix operations include:

• Fill
• Transpose
• Square matrix transpose (in-place)
• Addition
• Add identity (I + A)
• Subtract identity (I - A)
• Accumulate (in-place)
• Subtraction
• Subtract accumulate (in-place)
• Negate (-1 * A)
• Matrix-vector multiply
• Matrix-matrix multiply
• Inversion via Cholesky decomposition
• Special multiplication (A * B^T)

---

How to Allocate and Access a 1D Array (vector):

float *b = new float[rows]; // Allocate 1D aray/vector

for (size_t i = 0; i < rows; i++)

b[i] = 0.0f; // Or, use: *(b + i)

How to Allocate and Access a 2D Array:

float *A = new float[rows * cols]; // Allocate 2D array

for (size_t i = 0; i < rows; i++) // Set value for (size_t j = 0; j < cols; j++)

*(A + i*cols + j) = 0.0f; // Row-major

Resources

https://www.techiedelight.com/dynamic-memory-allocation-in-c-for-2d-3d-array/ https://eli.thegreenplace.net/2015/memory-layout-of-multi-dimensional-arrays Fill a vector with a specified value. Use this to initialize arrays before they're used.

Parameters

fill	Value to fill array with
a	Vectorf of interest
n	Size/length of vector

VectorfSubtract()

void VectorfSubtract	(	float *	c,
		float *	a,
		float *	b,
		size_t	n
	)

Subtract two vectors and output result to a new vector.

Assumes vectors have similar dimensions. c <- a - b

Parameters

a	Pointer to first vector
b	Pointer to second vector
c	Pointer to output vector
n	Length of vector