Diff: main.cpp

Revision:

0:a0e9705be9c4

Child:

1:6b803652d032

diff -r 000000000000 -r a0e9705be9c4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 18 14:55:53 2019 +0000
@@ -0,0 +1,61 @@
+#include "mbed.h"
+//#include "Eigen/Dense.h"
+#include "Eigen/Core.h"
+#include "Eigen/Geometry.h"
+#include "EKF_RP.h"
+
+using namespace Eigen;
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+// EKF_RP ekf_rp(0.02f);    // initialize counter on PB_6 and PB_7
+
+Timer timer; // timer for time measurement
+float dt = 0.0f;
+
+uint32_t counter;
+
+Matrix<float, 3, 3> A;
+Matrix<float, 3, 1> b;
+DiagonalMatrix<float, 3, 3> I;
+
+// Matrix<float,4,1> mat = state.block(6,0,4,1);
+
+int main()
+{
+    pc.baud(2000000);
+
+    timer.start();
+
+    counter = 0;
+    
+    A << 1,  2,  3,
+         4,  5,  6,
+         7,  8, 10;
+    b << 3,
+         3,
+         4;
+         
+    // I.setIdentity();
+    
+    I.diagonal()[0] = 1;
+    I.diagonal()[1] = 1;
+    I.diagonal()[2] = 1;
+    
+    while(1) {
+        
+        Vector3f x = I * A.inverse() * b;
+        
+        // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
+        dt = timer.read();
+        timer.reset();
+        
+        pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), 1.0f/dt);
+        pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));
+
+        counter++;
+        
+        wait_us(1000000);
+    }
+}
+