I wanted to develop a more thorough Extended Kalman Filter routine (which requires matrix operations including the most dreaded one... inversion) for an Arduino project. So, I was looking around for good linear algebra libraries for AVR/ARM platforms (specifically Arduino) and found that Eigen has an arduino port. However, I had a lot of difficulties installing it and also read posts about how it takes up a lot of space. I found the MatrixMath library but saw it was it was clearly lacking functionality (no operator overloadings, inefficient access control, and missing functions) so I wrote a new library.
I'm not sure how to contribute it to the community though - should I make a git repository for it and upload it there? How would I add it to the contributed libraries repository so it can be downloaded with the IDE library manager?
I tested 6x6 Matrix inversion routine on the Uno, Mega 2560 and Due. Sadly, the inversion crashes on the Uno 
(and takes up 23% flash), but it works fine on the Mega 2560 and the Due. The 6x6 inversion routine takes 29.3 ms on the Mega 2560 and only 2.1 ms on the Due. 
I've attached the library to this post for now. Here are the header files for the list of functions supported at this moment. Let me know if you guys are interested in any more operations and I'll work on it. 
Matrix.h
#ifndef MATRIX_H
#define MATRIX_H
#include <string>
#define MAX_LEN 6
class mat {
public:
// Constructor
mat();
mat(int m, int n);
// mat operations
mat operator+(mat m); // Addition
void operator+=(mat m);
mat operator-(mat m); // Subtraction
void operator-=(mat m);
mat operator^(mat m); // Element-by-element multiplication
void operator^=(mat m);
mat operator/(mat m); // Element-by-element division
void operator/=(mat m);
mat operator*(mat m); // mat multiplication
void operator*=(mat m);
mat operator*(float k); // scalar multiplication
void operator*=(float k);
mat operator/(float k); // scalar division
void operator/=(float k);
// mat functions
float det(); // Determinant
mat t(); // Transpose
mat inv(); // Inverse
void resize(int m, int n); // Resize
void copy(mat m); // Copy
mat cols_cat(mat m); // Horizontal concatenation
mat rows_cat(mat m); // Vertical concatenation
// Access functions
mat get_row(int m); // Get row
mat get_col(int n); // Get column
void set_row(int m, mat row); // Set row
void set_col(int n, mat col); // Set col
mat get_subm(int m1, int m2, int n1, int n2); // Get matrix subset
void set_subm(int m, int n, mat subm); // Set matrix subset
mat cofactor(int m, int n); // Get cofactor matrix
float& operator()(int m, int n);
// Initializers
static mat zeros(int m, int n);
static mat ones(int m, int n);
static mat identity(int n);
// Debug stuff
std::string sprint(); // Dump matrix to string
// TO-DO: Add relational operators
// mat variables
int rows;
int cols;
float data[MAX_LEN][MAX_LEN];
protected:
mat get_mat(); // Return self
};
#endif
Vector.h - Derived class for row vectors
#ifndef VECTOR_H
#define VECTOR_H
#include "Matrix.h"
class vec : public mat {
public:
vec(int n); // Constructor
// vec specific Operations
float dot(vec v); // Dot product
vec cross(vec v); // Cross product
// Vector access
float& operator()(int m);
// Vector Initializers
static vec zeros(int n);
static vec ones(int n);
};
#endif
This library allows for some fairly complex matrix operations - for instance, here is the sequence I'm using in the Extended Kalman Filter function.
// State and Error Prediction
vec xp = A * x + B * u;
mat Pp = A * P * A.t() + Q;
// Compute Kalman Gain
mat S = H * Pp * H.t() + R;
mat K = Pp * H.t() * S.inv();
// Measurement Update
x = xp + K * (z - H * xp);
P = Pp - K * H * Pp;
LinearAlgebra.zip (3.77 KB)