File content as of revision 10:dd5ea359fb97:

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

using namespace Eigen;

Serial pc(SERIAL_TX, SERIAL_RX);

EKF_RP ekf_rp(0.02f);
EKF_RPY ekf_rpy(0.02f, 0.1554364f, -0.0879864f, 0.3507829f); 

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

uint32_t i;

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

Matrix<float, 4, 1> gyro_x;
Matrix<float, 4, 1> gyro_y;
Matrix<float, 4, 1> gyro_z;
Matrix<float, 4, 1> accel_x;
Matrix<float, 4, 1> accel_y;
Matrix<float, 4, 1> magnet_x;
Matrix<float, 4, 1> magnet_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;
    gyro_z   <<  0.05,  0.04, -0.03,  0.02;
    accel_x  <<  0.03, -0.01,  0.07, -0.03;
    accel_y  <<  0.01,  0.05, -0.04, -0.02;
    magnet_x <<  0.01, -0.02, -0.08,  0.02;
    magnet_y <<  0.06, -0.04,  0.04,  0.07;
        
    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 */
            ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
            pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; \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);
            /* ekf_rpy */
            // ekf_rpy.update(gyro_x(i), gyro_y(i), gyro_z(i), accel_x(i), accel_y(i), magnet_x(i), magnet_y(i));
            // pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f %.7f; %.7f; \r\n", i, ekf_rpy.get_est_state(0), ekf_rpy.get_est_state(1), ekf_rpy.get_est_state(2), ekf_rpy.get_est_state(3), ekf_rpy.get_est_state(4), ekf_rpy.get_est_state(5), ekf_rpy.get_est_state(6), ekf_rpy.get_est_state(7), dt);
            
        }

        i++;
        
        wait_us(1000000);
    }
}