I needed some code to do matrix math for a Kalman Filter. I spent a long time looking for something, eventually I found some C code that I turned into an Arduino library. It has multiplication, addition, subtraction, transpose, and inverse. Here it is:
////////////////////////////
/*
MathUtil.h - Library for MathUtil code.
*/
#ifndef MathUtil_h
#define MathUtil_h
#include "WProgram.h"
class MathUtil
{
public:
MathUtil();
void MatrixPrint(float* A, int m, int n, char label);
void MatrixMultiply(float* A, float* B, int m, int p, int n, float* C);
void MatrixAddition(float* A, float* B, int m, int n, float* C);
void MatrixSubtraction(float* A, float* B, int m, int n, float* C);
void MatrixTranspose(float* A, int m, int n, float* C);
int MatrixInversion(float* A, int n, float* AInverse);
};
#endif
///////////////
/*
MathUtil.cpp - Library for MathUtil
*/
#include "WProgram.h"
#include "MathUtil.h"
#define NR_END 1
MathUtil::MathUtil()
{
}
// Matrix Printing Routine
void MathUtil::MatrixPrint(float* A, int m, int n, char label){
// A = input matrix (m x n)
int i,j;
Serial.print(label);
Serial.println(" Matrix values:");
for (i=0; i<m; i++){
for (j=0;j<n;j++){
Serial.print(A[n*i+j]);
Serial.print("; i=");
Serial.print(i);
Serial.print(" j=");
Serial.println(j);
}
}
}
//Matrix Multiplication Routine
void MathUtil::MatrixMultiply(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 = AB (m x n)
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
C[ni+j]=0;
for (k=0;k<p;k++)
C[ni+j]= C[ni+j]+A[p*i+k]B[nk+j];
}
}
//Matrix Addition Routine
void MathUtil::MatrixAddition(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A+B (m x n)
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[ni+j]=A[ni+j]+B[n*i+j];
}
//Matrix Subtraction Routine
void MathUtil::MatrixSubtraction(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A-B (m x n)
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[ni+j]=A[ni+j]-B[n*i+j];
}
//Matrix Transpose Routine
void MathUtil::MatrixTranspose(float* A, int m, int n, float* C)
{
// A = input matrix (m x n)
// m = number of rows in A
// n = number of columns in A
// C = output matrix = the transpose of A (n x m)
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[mj+i]=A[ni+j];
}
//Matrix Inversion Routine
int MathUtil::MatrixInversion(float* A, int n, float* AInverse)
{
// A = input matrix (n x n)
// n = dimension of A
// AInverse = inverted matrix (n x n)
// This function inverts a matrix based on the Gauss Jordan method.
// The function returns 1 on success, 0 on failure.
int i, j, iPass, imx, icol, irow;
float det, temp, pivot, factor;
float* ac = (float*)calloc(nn, sizeof(float));
det = 1;
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
AInverse[ni+j] = 0;
ac[ni+j] = A[ni+j];
}
AInverse[ni+i] = 1;
}
// The current pivot row is iPass.
// For each pass, first find the maximum element in the pivot column.
for (iPass = 0; iPass < n; iPass++)
{
imx = iPass;
for (irow = iPass; irow < n; irow++)
{
if (fabs(A[nirow+iPass]) > fabs(A[nimx+iPass])) imx = irow;
}
// Interchange the elements of row iPass and row imx in both A and AInverse.
if (imx != iPass)
{
for (icol = 0; icol < n; icol++)
{
temp = AInverse[niPass+icol];
AInverse[niPass+icol] = AInverse[nimx+icol];
AInverse[nimx+icol] = temp;
if (icol >= iPass)
{
temp = A[niPass+icol];
A[niPass+icol] = A[nimx+icol];
A[nimx+icol] = temp;
}
}
}
// The current pivot is now A[iPass][iPass].
// The determinant is the product of the pivot elements.
pivot = A[niPass+iPass];
det = det * pivot;
if (det == 0)
{
free(ac);
return 0;
}
for (icol = 0; icol < n; icol++)
{
// Normalize the pivot row by dividing by the pivot element.
AInverse[niPass+icol] = AInverse[niPass+icol] / pivot;
if (icol >= iPass) A[niPass+icol] = A[niPass+icol] / pivot;
}
for (irow = 0; irow < n; irow++)
// Add a multiple of the pivot row to each row. The multiple factor
// is chosen so that the element of A on the pivot column is 0.
{
if (irow != iPass) factor = A[nirow+iPass];
for (icol = 0; icol < n; icol++)
{
if (irow != iPass)
{
AInverse[nirow+icol] -= factor * AInverse[niPass+icol];
A[nirow+icol] -= factor * A[n*iPass+icol];
}
}
}
}
free(ac);
return 1;
}
///////////////////////
// example .pde file for Math Utilities
#include <MathUtil.h>
MathUtil math;
float A[2][2];
float B[2][2];
float C[2][2];
void setup() {
Serial.begin(9600);
A[0][0] = 1.0;
A[0][1] = 1.0;
A[1][0] = 1.0;
A[1][1] = 2.0;
B[0][0] = 1.0;
B[0][1] = 3.0;
B[1][0] = 2.0;
B[1][1] = 7.0;
}
void loop(){
math.MatrixMultiply((float*)A,(float*)B,2,2,2,(float*)C);
math.MatrixPrint((float*)A,2,2,'A');
math.MatrixPrint((float*)B,2,2,'B');
math.MatrixPrint((float*)C,2,2,'C');
while(1);
}