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

Committer:
ykuroda
Date:
2016-11-25
Revision:
2:cb388602f5ed
Parent:
1:ad6e79769c8f

File content as of revision 2:cb388602f5ed:

//
//  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()
{
    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++){

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