Go Down

Topic: Lightweight Linear Algebra Library (how to contribute?) (Read 105 times) previous topic - next topic

naru95

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  :smiley-confuse: (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. :smiley-cool:

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
Code: [Select]

#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

Code: [Select]

#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.

Code: [Select]

// 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;

robtillaart

Thank you for sharing.

You could create a github repository to be able to track issues and versions.
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy