diff --git a/src/odom_new.cpp b/src/odom_new.cpp
index 2ca4689eda03aed30741bfdd3acd85f99814f740..932622a2e97f9d032f7076a17431cdaf9817c1f4 100644
--- a/src/odom_new.cpp
+++ b/src/odom_new.cpp
@@ -69,6 +69,11 @@ class OdomNew
       //this->x  = 
       //this->y  = 
       //this->th = 
+      vx        = this->radius*( left_vel  + right_vel) / 2.0;
+      vth       = this->radius*( right_vel - left_vel ) / this->wheel_distance;
+      this->x  += vx * cos(this->th) * dt;
+      this->y  += vx * sin(this->th) * dt;
+      this->th += vth * dt;
       //////// TODO //////
       
       this->odom_msg.header.stamp = time;