Diff: main.cpp

Revision:

4:e50e18eac72b

Parent:

3:121647a7cddf

Child:

6:f9569a07aff5

--- a/main.cpp	Fri Oct 18 21:49:59 2019 +0000
+++ b/main.cpp	Sun Oct 20 08:27:25 2019 +0000
@@ -14,19 +14,24 @@
 Timer timer; // timer for time measurement
 float dt = 0.0f;
 
-uint32_t counter;
+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();
 
-    counter = 0;
+    i = 0;
     
     /*
     A << 1, 2, 3, 5, 1, 8,10, 1, 3, 
@@ -67,20 +72,36 @@
          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;
         
-        ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
+        // 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();
         
-        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);
+        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));
+        }
 
-        counter++;
+        i++;
         
-        // wait_us(1000000);
+        wait_us(1000000);
     }
 }