altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

Files at this revision

API Documentation at this revision

Revision 16:c39e084f05ed, committed 2019-10-24

Comitter:
pmic
Date:
Thu Oct 24 06:43:17 2019 +0000
Parent:
15:53485bd1ff28
Child:
17:1d98928f7681
Commit message:
Get recursion running. Everything is fine now : - )

Changed in this revision

EKF_RP.cpp Show annotated file Show diff for this revision Revisions of this file
EKF_RPY.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/EKF_RP.cpp	Thu Oct 24 06:32:00 2019 +0000
+++ b/EKF_RP.cpp	Thu Oct 24 06:43:17 2019 +0000
@@ -59,16 +59,16 @@
     P = F * P * F.transpose() + Q;
     e = y - gy();
     
-    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
-    x = x + K * e;
-    P = (I - K * H) * P;
+    // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    // x = x + K * e;
+    // P = (I - K * H) * P;
     
     /* only valid if R is diagonal (uncorrelated noise) */
-    // for(uint8_t i = 0; i < 4; i++)  {
-    //     K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
-    //     x = x + K.col(i) * e(i);
-    //     P = (I - K.col(i)*H.row(i)) * P;
-    // }    
+    for(uint8_t i = 0; i < 2; i++)  {
+        K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
+        x = x + K.col(i) * e(i);
+        P = (I - K.col(i)*H.row(i)) * P;
+    }    
 }
 
 void EKF_RP::update_angles()

--- a/EKF_RPY.cpp	Thu Oct 24 06:32:00 2019 +0000
+++ b/EKF_RPY.cpp	Thu Oct 24 06:43:17 2019 +0000
@@ -79,16 +79,16 @@
     P = F * P * F.transpose() + Q;
     e = y - gy();
     
-    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
-    x = x + K * e;
-    P = (I - K * H) * P;
+    // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
+    // x = x + K * e;
+    // P = (I - K * H) * P;
     
     /* only valid if R is diagonal (uncorrelated noise) */
-    // for(uint8_t i = 0; i < 4; i++)  {
-    //     K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
-    //     x = x + K.col(i) * e(i);
-    //     P = (I - K.col(i)*H.row(i)) * P;
-    // }
+    for(uint8_t i = 0; i < 4; i++)  {
+        K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
+        x = x + K.col(i) * e(i);
+        P = (I - K.col(i)*H.row(i)) * P;
+    }
 }
 
 void EKF_RPY::update_angles()

--- 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++;