Diff: main.cpp

Revision:

17:1d98928f7681

Parent:

16:c39e084f05ed

Child:

21:aab1ac72095b

--- a/main.cpp	Thu Oct 24 06:43:17 2019 +0000
+++ b/main.cpp	Fri Oct 25 08:44:32 2019 +0000
@@ -22,13 +22,13 @@
 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> 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;
+Matrix<float, 7, 1> gyro_x;
+Matrix<float, 7, 1> gyro_y;
+Matrix<float, 7, 1> gyro_z;
+Matrix<float, 7, 1> accel_x;
+Matrix<float, 7, 1> accel_y;
+Matrix<float, 7, 1> magnet_x;
+Matrix<float, 7, 1> magnet_y;
 
 int main()
 {
@@ -79,13 +79,13 @@
     I.setIdentity();
     // I = I.transpose();
     
-    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;
+    gyro_x   <<  0.1,  0.5, -0.2,  0.2,  0.3,  0.2, -0.7;
+    gyro_y   << -0.1, -0.3,  0.1,  0.1,  0.1, -0.1, -0.9;
+    gyro_z   <<  0.5,  0.4, -0.3,  0.2,  0.8, -0.6,  0.2;
+    accel_x  <<  0.3, -0.1,  0.7, -0.3, -0.9,  0.1,  0.2;
+    accel_y  <<  0.1,  0.5, -0.4, -0.2, -0.4,  0.5,  0.8;
+    magnet_x <<  0.1, -0.2, -0.8,  0.2,  0.6,  0.4, -0.4;
+    magnet_y <<  0.6, -0.4,  0.4,  0.7,  0.7,  0.5,  0.3;
         
     pc.printf("\r\n");
         
@@ -103,17 +103,17 @@
         dt = timer.read();
         timer.reset();
         
-        if( i < 4 ) {
+        if( i < 7 ) {
             /* 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_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);
+            // pc.printf("%.7f; \r\n", 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);
+            // 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);
+            // pc.printf("%.7f; \r\n", dt);
         }
-
-        i++;
-        
+        i++;        
         wait_us(1000000);
     }
 }