Diff: main.cpp

Revision:

2:cb388602f5ed

Parent:

1:ad6e79769c8f

--- a/main.cpp	Thu Oct 13 08:15:23 2016 +0000
+++ b/main.cpp	Fri Nov 25 17:20:51 2016 +0000
@@ -1,19 +1,33 @@
-#include "mbed.h"
-#include <iostream>
-#include <Eigen/Dense.h>
+//
+//  odometry.cpp ... Odometry calculation example
+//  (deterministic version)
+//
+//  2016.11.25 ... Y.Kuroda
+//
+//#include <iostream> I/Oストリームは使わないこと
+#include <Eigen/Core.h>
 using namespace std;
 using namespace Eigen;
 
-int main() {
+int main()
+{
+    float dt = 0.1;
+    Vector3f v(1, 0.1, 1);  // Vehicle velocity (v, omega, 1)
+    Vector3f p(0,0,0);      // Position in world coordinates (x, y, yaw)
+    Matrix3f M;             // Odometry transition matrix
+
+    for(int i=0; i<1000; i++){
 
-    Matrix3f A;
-    Vector3f b;
-    A << 1,2,3, 4,5,6, 7,8,10;
-    b << 3, 3, 4;
-    cout << "Here is the matrix A:\n" << A << endl;
-    cout << "Here is the vector b:\n" << b << endl;
-    Vector3f x = A.colPivHouseholderQr().solve(b);
-    //Vector3f x = A.inverse() * b;
-    cout << "The solution is:\n" << x << endl;
-    //printf("b[%g,%g,%g]\n", x(0),x(1),x(2));
+        if(i==500)
+            v << 1, -0.2, 1;    // just an example of controlling
+
+        printf("%f, %f, %f, %f\n", i*dt, p[0], p[1], p[2]); 
+
+        M << dt*cos(p[2]),  0, p[0],    // create transition matrix
+             dt*sin(p[2]),  0, p[1],
+                        0, dt, p[2];
+    
+        p = M * v;          // move!!
+    }
 }
+