Yoji KURODA / Mbed 2 deprecated Odometry_test

This is an example of an odometry calculation

Dependencies:   Eigen mbed

Fork of Eigen_test by Yoji KURODA

main.cpp@2:cb388602f5ed, 2016-11-25 (annotated)

Committer:
ykuroda
Date:
Fri Nov 25 17:20:51 2016 +0000
Revision:
2:cb388602f5ed
Parent:
1:ad6e79769c8f 
1st commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 2:cb388602f5ed 1 //
ykuroda 2:cb388602f5ed 2 // odometry.cpp ... Odometry calculation example
ykuroda 2:cb388602f5ed 3 // (deterministic version)
ykuroda 2:cb388602f5ed 4 //
ykuroda 2:cb388602f5ed 5 // 2016.11.25 ... Y.Kuroda
ykuroda 2:cb388602f5ed 6 //
ykuroda 2:cb388602f5ed 7 //#include <iostream> I/Oストリームは使わないこと
ykuroda 2:cb388602f5ed 8 #include <Eigen/Core.h>
ykuroda 0:5971b11ad8ff 9 using namespace std;
ykuroda 0:5971b11ad8ff 10 using namespace Eigen;
ykuroda 0:5971b11ad8ff 11
ykuroda 2:cb388602f5ed 12 int main()
ykuroda 2:cb388602f5ed 13 {
ykuroda 2:cb388602f5ed 14 float dt = 0.1;
ykuroda 2:cb388602f5ed 15 Vector3f v(1, 0.1, 1); // Vehicle velocity (v, omega, 1)
ykuroda 2:cb388602f5ed 16 Vector3f p(0,0,0); // Position in world coordinates (x, y, yaw)
ykuroda 2:cb388602f5ed 17 Matrix3f M; // Odometry transition matrix
ykuroda 2:cb388602f5ed 18
ykuroda 2:cb388602f5ed 19 for(int i=0; i<1000; i++){
ykuroda 0:5971b11ad8ff 20
ykuroda 2:cb388602f5ed 21 if(i==500)
ykuroda 2:cb388602f5ed 22 v << 1, -0.2, 1; // just an example of controlling
ykuroda 2:cb388602f5ed 23
ykuroda 2:cb388602f5ed 24 printf("%f, %f, %f, %f\n", i*dt, p[0], p[1], p[2]);
ykuroda 2:cb388602f5ed 25
ykuroda 2:cb388602f5ed 26 M << dt*cos(p[2]), 0, p[0], // create transition matrix
ykuroda 2:cb388602f5ed 27 dt*sin(p[2]), 0, p[1],
ykuroda 2:cb388602f5ed 28 0, dt, p[2];
ykuroda 2:cb388602f5ed 29
ykuroda 2:cb388602f5ed 30 p = M * v; // move!!
ykuroda 2:cb388602f5ed 31 }
ykuroda 0:5971b11ad8ff 32 }
ykuroda 2:cb388602f5ed 33