Diff: main.cpp

Revision:

16:c39e084f05ed

Parent:

15:53485bd1ff28

Child:

17:1d98928f7681

--- a/main.cpp	Thu Oct 24 06:32:00 2019 +0000
+++ b/main.cpp	Thu Oct 24 06:43:17 2019 +0000
@@ -77,6 +77,7 @@
          8;
          
     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;
@@ -104,12 +105,11 @@
         
         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_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);
-            
+            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++;