Diff: main.cpp

Revision:

7:bcbcc23983de

Parent:

6:f9569a07aff5

Child:

8:a72cd6356bab

--- a/main.cpp	Sun Oct 20 10:20:05 2019 +0000
+++ b/main.cpp	Sun Oct 20 13:16:13 2019 +0000
@@ -4,12 +4,14 @@
 #include "Eigen/Core.h"
 #include "Eigen/Geometry.h"
 #include "EKF_RP.h"
+#include "EKF_RPY.h"
 
 using namespace Eigen;
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 EKF_RP ekf_rp(0.02f);
+EKF_RPY ekf_rpy(0.02f, 0.1554364f, -0.0879864f, 0.3507829f); 
 
 Timer timer; // timer for time measurement
 float dt = 0.0f;
@@ -22,8 +24,11 @@
 
 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;
 
 int main()
 {
@@ -73,10 +78,13 @@
          
     // 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;
+    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;
         
     pc.printf("\r\n");
         
@@ -95,9 +103,11 @@
         timer.reset();
         
         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));
+            // 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.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++;