File content as of revision 0:a0e9705be9c4:

#include "mbed.h"
//#include "Eigen/Dense.h"
#include "Eigen/Core.h"
#include "Eigen/Geometry.h"
#include "EKF_RP.h"

using namespace Eigen;

Serial pc(SERIAL_TX, SERIAL_RX);

// EKF_RP ekf_rp(0.02f);    // initialize counter on PB_6 and PB_7

Timer timer; // timer for time measurement
float dt = 0.0f;

uint32_t counter;

Matrix<float, 3, 3> A;
Matrix<float, 3, 1> b;
DiagonalMatrix<float, 3, 3> I;

// Matrix<float,4,1> mat = state.block(6,0,4,1);

int main()
{
    pc.baud(2000000);

    timer.start();

    counter = 0;
    
    A << 1,  2,  3,
         4,  5,  6,
         7,  8, 10;
    b << 3,
         3,
         4;
         
    // I.setIdentity();
    
    I.diagonal()[0] = 1;
    I.diagonal()[1] = 1;
    I.diagonal()[2] = 1;
    
    while(1) {
        
        Vector3f x = I * A.inverse() * b;
        
        // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
        dt = timer.read();
        timer.reset();
        
        pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), 1.0f/dt);
        pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));

        counter++;
        
        wait_us(1000000);
    }
}