File content as of revision 1:c92781bb4d5e:

#include "mbed.h"
#include "m3pi_ng.h"
#include "MSCFileSystem.h"
#include <string.h>
#include <fstream>
//#include <vector>
#include <sstream>
//#include <algorithm>
//#include <functional>
#include <math.h>


m3pi m3pi;
MSCFileSystem msc("usb");
I2C minimu9V3(p28,p27);

#define pi 3.14159265358979323846
/*
----------------------------------------------------------------------------------------------------
Completely commented out, needed for EKF, but is not working, because of the m3pi's small power
----------------------------------------------------------------------------------------------------
// Lookuptable with map from txt-file, will be facking slow at runtime
float Lookuptable(float x_pos, float y_pos, float alpha, int size)
{   ifstream map("/usb/wheelmap.txt");
    m3pi.locate(0,1);
    m3pi.printf("TestLoTa");
    float dalpha = pi;
    float POL = -1;
    int counter = 0;
    float alpha_corr = atan2(sin(alpha), cos(alpha));
    float x_abs, y_abs, dist, dy, dx, alpha_map;
    string ssx_abs, ssy_abs;
    map>>ssx_abs;
    map>>ssy_abs;
    x_abs = std::atof(ssx_abs.c_str());
    y_abs = std::atof(ssy_abs.c_str());
    float dist_o = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos));
    ssx_abs.clear();
    ssy_abs.clear();
    while (abs(dalpha)>pi/5 && counter< size){
        string ssx_abs, ssy_abs;
        map>>ssx_abs;
        map>>ssy_abs;
        x_abs = std::atof(ssx_abs.c_str());
        y_abs = std::atof(ssy_abs.c_str());
        dist = sqrt((x_abs-x_pos)*(x_abs-x_pos) + (y_abs-y_pos)*(y_abs-y_pos));
        // if measurementradius is crossed, compute angle
        if ( (dist >4.05 && dist_o <4.05) || (dist <4.05 && dist_o >4.05)){
            dx = x_abs-x_pos;
            dy = y_abs-y_pos;
            alpha_map = atan2(dy, dx);
            dalpha = alpha_corr - alpha_map;
            // correct it, if the angle is obtuse
            if (dalpha<-pi){
                dalpha=2*pi + dalpha;
                }
            else if (dalpha>pi){
                dalpha = 2*pi - dalpha;
                } 
            }
        // if the end of the txt is reached, we need to break out
        counter++;
        m3pi.locate(0,0);
        m3pi.printf("%i",(float)dist);
        ssx_abs.clear();
        ssy_abs.clear();
        }
    if (abs(dalpha)<pi/5){POL = std::pow(abs(dalpha*5/pi),2.5) * dalpha/abs(dalpha);}
    map.close();
    return POL;
    
    
    }
// 4x4 Matrix inversion
bool gluInvertMatrix(float M[4][4], float INVOut[4][4])
{
    double inv[16], m[16], invOut[16], det;
    // conversion, because too lazy to change the whole code
    for (int i = 0; i<4; i++){
        for (int j = 0; j<4; j++){
            m[4*i + j] = M[i][j];
            }
        }
    int i;

    inv[0] = m[5]  * m[10] * m[15] - 
             m[5]  * m[11] * m[14] - 
             m[9]  * m[6]  * m[15] + 
             m[9]  * m[7]  * m[14] +
             m[13] * m[6]  * m[11] - 
             m[13] * m[7]  * m[10];

    inv[4] = -m[4]  * m[10] * m[15] + 
              m[4]  * m[11] * m[14] + 
              m[8]  * m[6]  * m[15] - 
              m[8]  * m[7]  * m[14] - 
              m[12] * m[6]  * m[11] + 
              m[12] * m[7]  * m[10];

    inv[8] = m[4]  * m[9] * m[15] - 
             m[4]  * m[11] * m[13] - 
             m[8]  * m[5] * m[15] + 
             m[8]  * m[7] * m[13] + 
             m[12] * m[5] * m[11] - 
             m[12] * m[7] * m[9];

    inv[12] = -m[4]  * m[9] * m[14] + 
               m[4]  * m[10] * m[13] +
               m[8]  * m[5] * m[14] - 
               m[8]  * m[6] * m[13] - 
               m[12] * m[5] * m[10] + 
               m[12] * m[6] * m[9];

    inv[1] = -m[1]  * m[10] * m[15] + 
              m[1]  * m[11] * m[14] + 
              m[9]  * m[2] * m[15] - 
              m[9]  * m[3] * m[14] - 
              m[13] * m[2] * m[11] + 
              m[13] * m[3] * m[10];

    inv[5] = m[0]  * m[10] * m[15] - 
             m[0]  * m[11] * m[14] - 
             m[8]  * m[2] * m[15] + 
             m[8]  * m[3] * m[14] + 
             m[12] * m[2] * m[11] - 
             m[12] * m[3] * m[10];

    inv[9] = -m[0]  * m[9] * m[15] + 
              m[0]  * m[11] * m[13] + 
              m[8]  * m[1] * m[15] - 
              m[8]  * m[3] * m[13] - 
              m[12] * m[1] * m[11] + 
              m[12] * m[3] * m[9];

    inv[13] = m[0]  * m[9] * m[14] - 
              m[0]  * m[10] * m[13] - 
              m[8]  * m[1] * m[14] + 
              m[8]  * m[2] * m[13] + 
              m[12] * m[1] * m[10] - 
              m[12] * m[2] * m[9];

    inv[2] = m[1]  * m[6] * m[15] - 
             m[1]  * m[7] * m[14] - 
             m[5]  * m[2] * m[15] + 
             m[5]  * m[3] * m[14] + 
             m[13] * m[2] * m[7] - 
             m[13] * m[3] * m[6];

    inv[6] = -m[0]  * m[6] * m[15] + 
              m[0]  * m[7] * m[14] + 
              m[4]  * m[2] * m[15] - 
              m[4]  * m[3] * m[14] - 
              m[12] * m[2] * m[7] + 
              m[12] * m[3] * m[6];

    inv[10] = m[0]  * m[5] * m[15] - 
              m[0]  * m[7] * m[13] - 
              m[4]  * m[1] * m[15] + 
              m[4]  * m[3] * m[13] + 
              m[12] * m[1] * m[7] - 
              m[12] * m[3] * m[5];

    inv[14] = -m[0]  * m[5] * m[14] + 
               m[0]  * m[6] * m[13] + 
               m[4]  * m[1] * m[14] - 
               m[4]  * m[2] * m[13] - 
               m[12] * m[1] * m[6] + 
               m[12] * m[2] * m[5];

    inv[3] = -m[1] * m[6] * m[11] + 
              m[1] * m[7] * m[10] + 
              m[5] * m[2] * m[11] - 
              m[5] * m[3] * m[10] - 
              m[9] * m[2] * m[7] + 
              m[9] * m[3] * m[6];

    inv[7] = m[0] * m[6] * m[11] - 
             m[0] * m[7] * m[10] - 
             m[4] * m[2] * m[11] + 
             m[4] * m[3] * m[10] + 
             m[8] * m[2] * m[7] - 
             m[8] * m[3] * m[6];

    inv[11] = -m[0] * m[5] * m[11] + 
               m[0] * m[7] * m[9] + 
               m[4] * m[1] * m[11] - 
               m[4] * m[3] * m[9] - 
               m[8] * m[1] * m[7] + 
               m[8] * m[3] * m[5];

    inv[15] = m[0] * m[5] * m[10] - 
              m[0] * m[6] * m[9] - 
              m[4] * m[1] * m[10] + 
              m[4] * m[2] * m[9] + 
              m[8] * m[1] * m[6] - 
              m[8] * m[2] * m[5];

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    if (det == 0)
        return false;

    det = 1.0 / det;

    for (i = 0; i < 16; i++)
        invOut[i] = inv[i] * det;
    
    
    for (int i = 0; i<4; i++){
        for (int j = 0; j<4; j++){
            INVOut[i][j]=invOut[4*i + j];
            }
        }
    return true;
}

----------------------------------------------------------------------------------------------------
End of out commented subfunctions
----------------------------------------------------------------------------------------------------
*/
int main() {

    /* PHASE 1 */
         
    // beepsound
    char dixie[]={'V','1','5','O','5','G','1','6','C','1','6'};  
    //the number of characters in the array
    int numb=10;
    float mu = 0.4; // mu between m3pi and ground
    float g = 9.81; // you know it!
    // system time
    float time;    
    // sensorarray
    int sensors [5] = {0, 0, 0, 0, 0}; 
    // number of rounds driven with PID-Controller to map the track
    int testrounds = 1;
    // max velocity in phase 1
    float speed1 = 0.1;
    // how strong will the outer motor be slowed to get back on track / the inner motor be fastend       
    float correction = speed1; 
    // how far away from the black line is tolerable  
    float threshold = 0.2;  
    
    m3pi.locate(0,1);         // x,y-Position on LCD
    m3pi.printf("Line Flw");  // display output
    
    // wait. REMIND: While waiting, the drive-order will be fullfilled
    wait(2.0);
    
    // robot turns left and right, looking for a line
    m3pi.sensor_auto_calibrate();
    
    // this first part should go on as long as the startline isn't crossed
    int rounds = 0;
    while (rounds == 0) {

        // -1.0 is far left, 1.0 is far right, 0.0 in the middle
        float position_of_line = m3pi.line_position();

        // Line is more than the threshold to the right, slow the left motor
        if (position_of_line > threshold) {
            m3pi.left_motor(speed1+correction);
            m3pi.right_motor(speed1-correction);
        }

        // Line is more than 50% to the left, slow the right motor
        else if (position_of_line < -threshold) {
            m3pi.right_motor(speed1+correction);
            m3pi.left_motor(speed1-correction);
        }

        // Line is in the middle
        else {
            m3pi.forward(speed1);
        }
        



        // read the sensors, values in [0,1000] with 1000=completely dark
        m3pi.readsensor(sensors);
        

        // startline is found if all sensor show black  
        if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500){
            rounds=1; 
            m3pi.playtune(dixie,numb);
            time = clock()/CLOCKS_PER_SEC;
            
               
        }
    }
    
    // startline is crossed now for the first time
    // now its time to follow the track with PID-Controll while mapping it
    ofstream testmap;
    testmap.open ("/usb/map.txt");

    /* LSM303D initialization*/
    int sensor_addr_LSM =  0x3A; // 0x3C;
    // request for x_l_m, results in sending: xlm,xhm,ylm,yhm,zlm,zhm
    char START_MAG = 0x08;
    // request for x_l_a, same as line 13
    char START_ACC = 0x28;
    // request for x_l_g, same as line 13
    char START_GYRO = 0x28;
    // empty array for readingvalues
    char content[6] = {0x00,0x00,0x00,0x00,0x00,0x00};
    // array for setting properties
    char ctrl[2];
    // set properties
    ctrl[0] = 0x21; // propertyadress 
    ctrl[1] = 0x00; // AFS = 0
    minimu9V3.write(sensor_addr_LSM, ctrl, 2);
    ctrl[0] = 0x20; // propertyadress 
    ctrl[1] = 0x57; // = 0b01010111, AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
    minimu9V3.write(sensor_addr_LSM, ctrl, 2);
    ctrl[0] = 0x24; // propertyadress 
    ctrl[1] = 0x64; // = 0b01100100, M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
    minimu9V3.write(sensor_addr_LSM, ctrl, 2);
    ctrl[0] = 0x25; // propertyadress 
    ctrl[1] = 0x20; // 0b00100000, MFS = 01 (+/- 4 gauss full scale)
    minimu9V3.write(sensor_addr_LSM, ctrl, 2);
    ctrl[0] = 0x26; // propertyadress 
    ctrl[1] = 0x00; // = 0b00100000, MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
    minimu9V3.write(sensor_addr_LSM, ctrl, 2);
    /* L3GD20H initialization*/
    int sensor_addr_L3G = 0xD6;
    // set properties
    ctrl[0] = 0x39; // propertyadress 
    ctrl[1] = 0x00; // Low_ODR = 0 (low speed ODR disabled)
    minimu9V3.write(sensor_addr_L3G,ctrl,2);
    ctrl[0] = 0x23; // propertyadress 
    ctrl[1] = 0x00; // FS = 00 (+/- 250 dps full scale)
    minimu9V3.write(sensor_addr_L3G,ctrl,2);
    ctrl[0] = 0x20; // propertyadress 
    ctrl[1] = 0x6F; // = 0b01101111, DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
    minimu9V3.write(sensor_addr_L3G,ctrl,2);

    // MAY be unneeded
    START_MAG |= 0x80;
    START_ACC |= 0x80;
    START_GYRO |= 0x80;
    
    uint8_t axl, axh, ayl, ayh, azl, azh;
    uint8_t mxl, mxh, myl, myh, mzl, mzh;
    uint8_t gxl, gxh, gyl, gyh, gzl, gzh;  
    int16_t ax, ay, az;
    int16_t mx, my, mz;
    int16_t gx, gy, gz;
         
    m3pi.locate(0,1); 
    m3pi.printf("Line PID");

    // params for the PID-Controller
    float P_TERM=1;
    float I_TERM=0;
    float D_TERM=20;
    
    float speed2 = 5*speed1;       // max velocity in phase 2
    float right;
    float left;
    float current_pos_of_line = 0.0;
    float previous_pos_of_line = 0.0;
    float derivative,proportional,integral = 0;
    float power;
    float MAX = 2*speed2;
    if(MAX>1) MAX = 1;
    float MIN = 0;
    // mapping variables 
    float x_abs = 0;
    float y_abs = 0;
    float alpha_abs = 0;
    //float x_rel, dx_abs;
    //float y_rel, dy_abs;
    float min_mx=0;
    float min_my=0;
    float max_mx=0;
    float max_my=0;
    float faktor = 893562.2181291698; 

    float dt = 0.0046; 

    int size = 0;
    while (rounds < testrounds+1){
      
        // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        
        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
        
        // Compute the integral
        integral += proportional;
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        
        // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
        
        // Compute new speeds   
        right = speed2-power;
        left  = speed2+power;
        
        // limit checks
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;
            
        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;
            
        // set speed 
        m3pi.left_motor(left);
        m3pi.right_motor(right);

       // sensors
       minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true);
       minimu9V3.read(sensor_addr_L3G, content, 6);
      
       gxl = content[0];
       gxh = content[1];
       gyl = content[2];
       gyh = content[3];
       gzl = content[4];
       gzh = content[5];
      
      
       gx=(int16_t)(gxh<<8|gxl);
       gy=(int16_t)(gyh<<8|gyl);
       gz=(int16_t)(gzh<<8|gzl);
       
       minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true
       minimu9V3.read(sensor_addr_LSM, content,6);
        
       mxl = content[0];
       mxh = content[1];
       myl = content[2];
       myh = content[3];
       mzl = content[4];
       mzh = content[5];
      
      
       mx=(int16_t)(mxh<<8|mxl);
       my=(int16_t)(myh<<8|myl);
       mz=(int16_t)(mzh<<8|mzl);

       minimu9V3.write(sensor_addr_LSM, &START_ACC, 1, true);//, true
       minimu9V3.read(sensor_addr_LSM, content, 6);
       axl = content[0];
       axh = content[1];
       ayl = content[2];
       ayh = content[3];
       azl = content[4];
       azh = content[5];
      
      
       ax=(int16_t)(axh<<8|axl);
       ay=(int16_t)(ayh<<8|ayl);
       az=(int16_t)(azh<<8|azl);   




       testmap<<mx<<" "<<my<<" "<<current_pos_of_line<<" "<<gz<<" "<<left<<" "<<right<<" \n";
       
       // read the sensors, values in [0,1000] with 1000=completely dark
       m3pi.readsensor(sensors);

       // startline is found if all sensor show black
           
       if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){
           rounds++;
           m3pi.playtune(dixie,numb);  
           time=clock()/CLOCKS_PER_SEC;
        }  
        size++;

    }
    testmap.close();
/*
----------------------------------------------------------------------------------------------------
Completely commented out, EKF-Loop, but is not working, because of the m3pi's small power
----------------------------------------------------------------------------------------------------


    m3pi.forward(0);
    m3pi.locate(0,1);

    float x_kp1_k[3], x_k_k[3], y_kp1_k[4], y_k[4];
    x_kp1_k[0]=0;
    x_kp1_k[1]=0;
    x_kp1_k[2]=0;
    // F is Jakobi of x
    float F[3][3];
    F[0][0] = 1;//F11
    F[0][1] = 0;//F12
    F[1][0] = 0;//F21
    F[1][1] = 1;//F22
    F[2][0] = 0;//F31
    F[2][1] = 0;//F32
    F[2][2] = 1;//F33
    // H is Jakobi of y
    float H[4][3];
    H[0][0] = 0;
    H[0][1] = 0;
    H[1][0] = 0;
    H[1][1] = 0;
    H[3][0] = 0;
    H[3][1] = 0;
    H[3][2] = -1/dt;
    // P(k+1|k) = P11_kp1_k etc / P(k|k) = P11_k_k etc
    float P_kp1_k[3][3], P_k_k[3][3];
    P_k_k[0][0] = 1; // P0 is eye(3)
    P_k_k[0][1] = 0;
    P_k_k[0][2] = 0;
    P_k_k[1][0] = 0;
    P_k_k[1][1] = 1;
    P_k_k[1][2] = 0;
    P_k_k[2][0] = 0;
    P_k_k[2][1] = 0;
    P_k_k[2][2] = 1;
    // Matrix Q
    float Q[3][3];
    Q[0][0] = 2.1289; // is result from optimization on simulation
    Q[0][1] = 0.9727;
    Q[0][2] = 0.2097;
    Q[1][0] = 0.9727;
    Q[1][1] = 1.6290;
    Q[1][2] = 0.1533;
    Q[2][0] = 0.2097;
    Q[2][1] = 0.1533;
    Q[2][2] = 0.0443;
    // Helpmatrix M = HP(k+1|k)H + R
    float M[4][4], invM[4][4];
    // Matrix R
    float R[4][4];
    R[0][0] = 0.2039; // also result from optimization on simulation
    R[0][1] = 0;
    R[0][2] = 0;
    R[0][3] = 0;
    R[1][0] = 0;
    R[1][1] = 0.2039;
    R[1][2] = 0;
    R[1][3] = 0;
    R[2][0] = 0;
    R[2][1] = 0;
    R[2][2] = 0.5281;
    R[2][3] = 0;
    R[3][0] = 0;
    R[3][1] = 0;
    R[3][2] = 0;
    R[3][3] = 0.0216;
    // Matrix K
    float K[3][4];
    // speed for phase 3
    float speed3 = speed2;
    m3pi.forward(speed3);
    while(rounds<testrounds+2){
        // PID Stuff
        // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        
        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
        
        // Compute the integral
        integral += proportional;
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        
        // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
        
        // Compute new speeds   
        right = speed3-power;
        left  = speed3+power;
        // limit checks
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;
            
        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;
            
       // set speed 
        m3pi.left_motor(left);
        m3pi.right_motor(right);
        // Kalman Filter
        // step 1 x_hat(k+1|k)=f(u,x_hat(k|k))
        if (right>left){
            x_kp1_k[0] = x_k_k[0] + ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*sin(x_k_k[2]);
            x_kp1_k[1] = x_k_k[1] + ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*cos(x_k_k[2]);
            x_kp1_k[2] = x_k_k[2] + (100*(right-left)/8.1*dt);
            }
        else if (left>right){
            x_kp1_k[0] = x_k_k[0] + ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*sin(x_k_k[2]);
            x_kp1_k[1] = x_k_k[1] + ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*cos(x_k_k[2]);
            x_kp1_k[2] = x_k_k[2] - (100*(left-right)/8.1*dt);
            }
        else { // left == right
            x_kp1_k[0] = x_k_k[0] + (100*right*dt)*cos(x_k_k[2]);
            x_kp1_k[1] = x_k_k[1] + (100*right*dt)*sin(x_k_k[2]);
            x_kp1_k[2] = x_k_k[2];
            }    
        // step 2  y_hat(k+1|k)=h(u(k+1),x_hat(k+1|k))
        y_kp1_k[0] = -cos(x_kp1_k[2]);
        y_kp1_k[1] = -sin(x_kp1_k[2]);
        y_kp1_k[2] = Lookuptable(x_kp1_k[0],x_kp1_k[1],x_kp1_k[2], size); // problems, no LUT implemented!
        y_kp1_k[3] = -(x_kp1_k[2] - x_k_k[2])/dt;
        // step 3 F is Jakobi of x
        if (right>left){
            F[0][2] = ((8.1*right/(right-left)-4.05)*(-1)*sin((100*(right-left)/8.1 *dt)))*sin(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*cos(x_k_k[2]);
            F[1][2] = ((8.1*right/(right-left)-4.05)*sin((100*(right-left)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*right/(right-left)-4.05)*(1-cos((100*(right-left)/8.1 *dt))))*(-1)*sin(x_k_k[2]);
            }
        else if (left>right){
            F[0][2] = ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*(-1)*sin(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*cos(x_k_k[2]);
            F[1][2] = ((8.1*left/(left-right)-4.05)*sin((100*(left-right)/8.1 *dt)))*cos(x_k_k[2]) + ((8.1*left/(left-right)-4.05)*(1-cos((100*(left-right)/8.1 *dt))))*(-1)*sin(x_k_k[2]);            
            }
        else { // left == right
            F[0][2] = (-1)*right*100*dt*sin(x_k_k[2]);
            F[1][2] = 100*left*dt*cos(x_k_k[2]);            
            }
        // step 4 H is Jakobi of y
        float LoTa = Lookuptable(x_k_k[0],x_k_k[1],x_kp1_k[2], size);
        H[0][2] = sin(x_k_k[2]);
        H[1][2] = -cos(x_k_k[2]);
        H[2][0] = (Lookuptable(x_k_k[0],x_k_k[1],x_k_k[2], size)-Lookuptable(x_kp1_k[0],x_k_k[1],x_k_k[2], size))/(x_k_k[0]-x_kp1_k[0]);
        H[2][1] = (Lookuptable(x_k_k[0],x_k_k[1],x_k_k[2], size)-Lookuptable(x_k_k[0],x_kp1_k[1],x_k_k[2], size))/(x_k_k[1]-x_kp1_k[1]);
        H[2][2] = std::pow((float)abs(LoTa),(float)(1.5/2.5))*2.5/pi*abs(LoTa)/LoTa;
        // step 5 P(k+1|k) = F*P(k|k)*F' + Q;
        // trust me, thats correct (says matlab)
        for (int i = 0; i<3; i++){
            for (int j = 0; j<3; j++){
                P_kp1_k[i][j] = F[j][2]*(F[i][2]*P_k_k[2][2] + F[i][1]*P_k_k[1][2] + F[i][0]*P_k_k[0][2]) + F[j][1]*(F[i][2]*P_k_k[2][1] + F[i][1]*P_k_k[1][1] + F[i][0]*P_k_k[0][1]) + F[j][0]*(F[i][2]*P_k_k[2][0] + F[i][1]*P_k_k[1][0] + F[i][0]*P_k_k[0][0]) + Q[i][j];
                }
            }

        // step 6 K = P(k+1|k)H' * (HP(k+1|k)H'+R)^(-1)
        // same as above, blame matlab for errors!
        // M = H*P(k+1|k)*H' + R
        for (int i= 0; i<4; i++ ){
            for (int j = 0; j<4; j++){
                M[i][j] = H[j][2]*(H[i][2]*P_kp1_k[2][2] + H[i][1]*P_kp1_k[1][2] + H[i][0]*P_kp1_k[0][2]) + H[j][1]*(H[i][2]*P_kp1_k[2][1] + H[i][1]*P_kp1_k[1][1] + H[i][0]*P_kp1_k[0][1]) + H[j][0]*(H[i][2]*P_kp1_k[2][0] + H[i][1]*P_kp1_k[1][0] + H[i][0]*P_kp1_k[0][0]) + R[i][j];
                }
            }
        // same goes here, tested with MatLab!
        gluInvertMatrix(M, invM);
        // K = P(k+1|k) * H' * invM
        for (int i= 0; i<3; i++ ){
            for (int j = 0; j<4; j++){
                K[i][j] = invM[0][j]*(P_kp1_k[i][0]*H[0][0] + P_kp1_k[i][1]*H[0][1] + P_kp1_k[i][2]*H[0][2]) + invM[1][j]*(P_kp1_k[i][0]*H[1][0] + P_kp1_k[i][1]*H[1][1] + P_kp1_k[i][2]*H[1][2]) + invM[2][j]*(P_kp1_k[i][0]*H[2][0] + P_kp1_k[i][1]*H[2][1] + P_kp1_k[i][2]*H[2][2]) + invM[3][j]*(P_kp1_k[i][0]*H[3][0] + P_kp1_k[i][1]*H[3][1] + P_kp1_k[i][2]*H[3][2]);
                }
            }

        // Measurementupdate needs measurmentdata!
        // sensors
        minimu9V3.write(sensor_addr_L3G, &START_GYRO,1,true);
        minimu9V3.read(sensor_addr_L3G, content, 6);
      
        gxl = content[0];
        gxh = content[1];
        gyl = content[2];
        gyh = content[3];
        gzl = content[4];
        gzh = content[5];
      
      
        gx=(int16_t)(gxh<<8|gxl);
        gy=(int16_t)(gyh<<8|gyl);
        gz=(int16_t)(gzh<<8|gzl);
      
        minimu9V3.write(sensor_addr_LSM, &START_MAG, 1, true);//, true
        minimu9V3.read(sensor_addr_LSM, content,6);
        // 
        mxl = content[0];
        mxh = content[1];
        myl = content[2];
        myh = content[3];
        mzl = content[4];
        mzh = content[5];
      
      
        mx=(int16_t)(mxh<<8|mxl);
        my=(int16_t)(myh<<8|myl);
        mz=(int16_t)(mzh<<8|mzl);
        //
        y_k[0] = (mx - min_mx)/(max_mx-min_mx)*2-1;
        y_k[1] = (my - min_my)/(max_my-min_my)*2-1;
        y_k[2] = current_pos_of_line;
        y_k[3] = gz/faktor;
        // step 7 x_hat(k+1|k+1) = x_hat(k+1|k) + K*(y(k+1) - y_hat(k+1|k)
        for (int i = 0; i<3; i++){
            x_k_k[i] = x_kp1_k[i] + K[i][0]*(y_k[0] - y_kp1_k[0]) +  K[i][1]*(y_k[1] - y_kp1_k[1]) +  K[i][2]*(y_k[2] - y_kp1_k[2]) +  K[i][3]*(y_k[3] - y_kp1_k[3]);
            }

        // step 8 P(k+1|k+1) = P(k+1|k) - K*H*P(k+1|k)
        // except of numerical problems, matlab proves the correctness here also
        for (int i = 0; i<3; i++){
            for (int j = 0; j<3; j++){
                P_k_k[i][j] = P_kp1_k[i][j] -(P_kp1_k[2][j]*(H[3][2]*K[i][3] + H[2][2]*K[i][2] + H[1][2]*K[i][1] + H[0][2]*K[i][0]) + P_kp1_k[1][j]*(H[3][1]*K[i][3] + H[2][1]*K[i][2] + H[1][1]*K[i][1] + H[0][1]*K[i][0]) + P_kp1_k[1][j]*(H[3][0]*K[i][3] + H[2][0]*K[i][2] + H[1][0]*K[i][1] + H[0][0]*K[i][0])); 
                }
            }
        

        // read the sensors, values in [0,1000] with 1000=completely dark
        m3pi.readsensor(sensors);

        // startline is found if all sensor show black
           
        if (sensors[0]+sensors[1]+sensors[2]+sensors[3]+sensors[4]>4500 && clock()/CLOCKS_PER_SEC>=time+3){
            rounds++;
            m3pi.playtune(dixie,numb);  
            time=clock()/CLOCKS_PER_SEC;
               
        }
    }    
    
       
----------------------------------------------------------------------------------------------------
End of out commented EKF-Loop
----------------------------------------------------------------------------------------------------   
*/    
    /* mapping is over, stay stuck in empty loop*/
    m3pi.locate(0,1);
    m3pi.printf("EmptyEnd");
    while(1) {
      // empty loop 
        m3pi.left_motor(0.1);
        m3pi.right_motor(0);  
        }

}