File content as of revision 6:f9569a07aff5:

#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);

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

uint32_t i;

Matrix<float, 8, 8> A;
Matrix<float, 8, 1> b;
Matrix<float, 8, 8> I;

Matrix<float, 4, 1> gyro_x;
Matrix<float, 4, 1> gyro_y;
Matrix<float, 4, 1> accel_x;
Matrix<float, 4, 1> accel_y;

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

    timer.start();

    i = 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();
    
    gyro_x  <<  0.01,  0.05, -0.02,  0.02;
    gyro_y  << -0.01, -0.03,  0.01,  0.01;
    accel_x <<  0.03, -0.01,  0.07, -0.03;
    accel_y <<  0.01,  0.05, -0.04,  0.02;
        
    pc.printf("\r\n");
        
    while(1) {
        
        // Matrix<float, 8, 1> x = A.inverse() * b;
        
        // float temp0 = (float)(rand()%10000-4999)*0.00001f;
        // float temp1 = (float)(rand()%10000-4999)*0.00001f;
        // float temp2 = (float)(rand()%10000-4999)*0.00001f;
        // float temp3 = (float)(rand()%10000-4999)*0.00001f;
        // float temp4 = (float)(rand()%10000-4999)*0.00001f;
        // float temp5 = (float)(rand()%10000-4999)*0.00001f;
        // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
        dt = timer.read();
        timer.reset();
        
        if( i < 4 ) {
            ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
            pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; \r\n", i, ekf_rp.get_est_state(0), ekf_rp.get_est_state(1), ekf_rp.get_est_state(2), ekf_rp.get_est_state(3), ekf_rp.get_est_state(4), ekf_rp.get_est_state(5), dt);
            // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f;  \r\n", i, ekf_rp.read_Q(0,0), ekf_rp.read_Q(1,1), ekf_rp.read_Q(2,2), ekf_rp.read_Q(3,3), ekf_rp.read_Q(4,4), ekf_rp.read_Q(5,5), ekf_rp.read_Q(0,1), ekf_rp.read_Q(1,0));
        }

        i++;
        
        wait_us(1000000);
    }
}