Revision 1:c92781bb4d5e, committed 2016-01-30

Comitter:

ErmGas

Date:

Sat Jan 30 18:56:10 2016 +0000

Parent:

0:4e756c4c88a7

Commit message:

Programm to use a minimu9 V3 sensormodule and save its values on an USB-Stick. Additionally an EKF (extended Kalman-Filter) is implemented, but commented because the m3pi works not fast enough to do this on runtime.

Changed in this revision

--- a/FATFileSystem.lib	Mon Sep 13 14:20:21 2010 +0000
+++ b/FATFileSystem.lib	Sat Jan 30 18:56:10 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_unsupported/code/fatfilesystem/
\ No newline at end of file
+http://mbed.org/users/mbed_unsupported/code/fatfilesystem/#333d6e93e58f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi_TUB.lib	Sat Jan 30 18:56:10 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/ErmGas/code/m3pi_TUB/#a57be5cc5a94

--- a/main.cpp	Mon Sep 13 14:20:21 2010 +0000
+++ b/main.cpp	Sat Jan 30 18:56:10 2016 +0000
@@ -1,12 +1,743 @@
 #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>
 
-MSCFileSystem fs ("fs");
+
+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");
 
-int main () {
+    // 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);
 
-    FILE *fp = fopen("/fs/hello.txt","w");
-    fprintf(fp,"Hello world!\n");
-    fclose (fp);
+    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);  
+        }
 
 }
\ No newline at end of file