Revision 2:cb388602f5ed, committed 2016-11-25

Comitter:

ykuroda

Date:

Fri Nov 25 17:20:51 2016 +0000

Parent:

1:ad6e79769c8f

Commit message:

1st commit

Changed in this revision

--- 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!!
+    }
 }
+

--- a/mbed.bld	Thu Oct 13 08:15:23 2016 +0000
+++ b/mbed.bld	Fri Nov 25 17:20:51 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/d75b3fe1f5cb
\ No newline at end of file