File content as of revision 3:121647a7cddf:

#include "mbed.h"
#include "iostream"
//#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, 8, 8> A;
Matrix<float, 8, 1> b;
Matrix<float, 8, 8> I;

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

    timer.start();

    counter = 0;
    
    /*
    A << 1, 2, 3, 5, 1, 8,10, 1, 3, 
         4, 5, 6, 8, 4, 2, 1, 9, 4,
         7, 8,10, 5, 6, 8, 4, 5, 1,
         4, 2,10, 8,10, 5, 6, 7, 8, 
         1, 8, 7, 3, 4, 6, 5, 1, 7,
         4, 2, 7, 5, 7, 6, 9, 2, 1,
         5, 5, 1, 7, 4, 2, 1, 1, 9,
         8, 9, 7, 4, 5, 6, 1, 2, 2,
         1, 5, 9, 4, 8, 7, 2, 6, 3;
    b << 3,
         3,
         4,
         2,
         7,
         5,
         1,
         8,
         1;
    */
    
    A << 1, 2, 3, 5, 1, 8,10, 1, 
         4, 5, 6, 8, 4, 2, 1, 9,
         7, 8,10, 5, 6, 8, 4, 5,
         4, 2,10, 8,10, 5, 6, 7, 
         1, 8, 7, 3, 4, 6, 5, 1,
         4, 2, 7, 5, 7, 6, 9, 2,
         5, 5, 1, 7, 4, 2, 1, 1,
         8, 9, 7, 4, 5, 6, 1, 2;
    b << 3,
         3,
         4,
         2,
         7,
         5,
         1,
         8;
         
    // I.setIdentity();
        
    while(1) {
        
        // Matrix<float, 8, 1> x = 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), dt);
        // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);

        counter++;
        
        // wait_us(1000000);
    }
}