Important development regarding Eigen. The Arduino/Eigen setup on Windows 7 seems to produce internal compiler errors when using matrices larger than 4x4. After struggling late at night with this, I have a solution.
As a Matlab user that appreciates minimalism, the Eigen library is written as plain header files. So no makefiles, no binary files, nothing to compile upfront, no 20th century nonsense. To add to the Arduino-Due IDE (1.5.1r2), follow these instructions: (applies to Windows 7)
Step-1:
-> As of 1/25/2013, latest stable release is Eigen 3.1.2
-> Download ZIP from:
http://eigen.tuxfamily.org
Step-2:
-> Unzip in temporary location
-> Unzip will create folder containing several files and subfolders.
-> The subfolder "Eigen" is all that is needed.
Step-3:
-> Copy "Eigen" subfolder to this precise location in the Arduino IDE directory tree:
...\arduino-1.5.1r2\hardware\arduino\sam\libraries
Step-4:
-> Eigen is written in C++, so header files do not include the *.h extension.
-> The Arduino IDE expects a *.h extension for it to appear in Sketch/Import Library pull-down menu.
-> So download "Eigen312.h" from this thread and copy to this precise location:
...\arduino-1.5.1r2\hardware\arduino\sam\libraries\Eigen
-> Notice "Eigen312.h" goes in the Eigen subdirectory
-> Normally, all that is needed in "Eigen312.h" is a call to the main Eigen Core header as follows: #include <Core>
However, the Arduino and AVR libraries interfere with Eigen for matrices larger than 4x4; this results in internal compiler errors. So @rpavlik came up with a bunch of #define statements that prevents this, and are conveniently included in "Eigen312.h".
Step-5:
-> Again, make sure you download "Eigen312.h" and copy to the Eigen subdirectory!
Step-6:
-> Run example code below demonstrating the Kalman gain equation using 6x6 matrices. Not as clean as Matlab or Octave, but not too shabby.
-> I wrote a function called "print_mtxf" that serially prints matrices; its included in the example.
Good luck.
Code: Select all
// Example By: RandomVibe
// Eigen Doc: http://eigen.tuxfamily.org/dox/
// Quick Reference: http://eigen.tuxfamily.org/dox/QuickRefPage.html
#include <Eigen312.h> // Calls main Eigen3.1.2 matrix class library
#include <LU> // Calls inverse, determinant, LU decomp., etc.
using namespace Eigen; // Eigen related statement; simplifies syntax for declaration of matrices
void print_mtxf(const Eigen::MatrixXf& K);
void setup() {
Serial.begin(9600);
// DECLARE MATRICES
//--------------------
MatrixXf Pp(6,6); // Produces 6x6 float matrix class
MatrixXf H(6,6); // Note: without "using namespace Eigen", declaration would be: Eigen::MatrixXf H(6,6);
MatrixXf R(6,6);
MatrixXf X(6,6);
MatrixXf K(6,6);
MatrixXf Z(6,6);
// INPUT MATRICES (so-called comma-initialize syntax)
//---------------------------------------------------------
Pp << 0.3252, 0.3192, 1.0933, -0.0068, -1.0891, -1.4916,
-0.7549, 0.3129, 1.1093, 1.5326, 0.0326, -0.7423,
1.3703, -0.8649, -0.8637, -0.7697, 0.5525, -1.0616,
-1.7115, -0.0301, 0.0774, 0.3714, 1.1006, 2.3505,
-0.1022, -0.1649, -1.2141, -0.2256, 1.5442, -0.6156,
-0.2414, 0.6277, -1.1135, 1.1174, 0.0859, 0.7481 ;
H << 0.8147, 0.2785, 0.9572, 0.7922, 0.6787, 0.7060,
0.9058, 0.5469, 0.4854, 0.9595, 0.7577, 0.0318,
0.1270, 0.9575, 0.8003, 0.6557, 0.7431, 0.2769,
0.9134, 0.9649, 0.1419, 0.0357, 0.3922, 0.0462,
0.6324, 0.1576, 0.4218, 0.8491, 0.6555, 0.0971,
0.0975, 0.9706, 0.9157, 0.9340, 0.1712, 0.8235;
R << 0.3252, 0.3192, 1.0933, -0.0068, -1.0891, -1.4916,
-0.7549, 0.3129, 1.1093, 1.5326, 0.0326, -0.7423,
1.3703, -0.8649, -0.8637, -0.7697, 0.5525, -1.0616,
-1.7115, -0.0301, 0.0774, 0.3714, 1.1006, 2.3505,
-0.1022, -0.1649, -1.2141, -0.2256, 1.5442, -0.6156,
-0.2414, 0.6277, -1.1135, 1.1174, 0.0859, 0.7481;
// Kalman Gain Example; Matlab form: K = Pp * H' * inv(H * Pp * H' + R)
//-----------------------------------
X = H * Pp * H.transpose() + R;
K = Pp * H.transpose() * X.inverse();
// Print Result
//----------------------------
print_mtxf(K); // Print Matrix Result (passed by reference)
}
void loop() {
// put your main code here, to run repeatedly:
}
// PRINT MATRIX (float type)
//-----------------------------
void print_mtxf(const Eigen::MatrixXf& X)
{
int i, j, nrow, ncol;
nrow = X.rows();
ncol = X.cols();
Serial.print("nrow: "); Serial.println(nrow);
Serial.print("ncol: "); Serial.println(ncol);
Serial.println();
for (i=0; i<nrow; i++)
{
for (j=0; j<ncol; j++)
{
Serial.print(X(i,j), 6); // print 6 decimal places
Serial.print(", ");
}
Serial.println();
}
Serial.println();
}