diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 9234aa45dc2c57e5b4fcf6334d92e490962b3905..2de7c427a31ed1788e2d3427f4c8beaab3f21c51 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -283,6 +283,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
             return;
 
         // get current and origin states
+        auto origin_ts = processor_motion_ptr_->getOrigin()->getTimeStamp();
         auto origin_state = processor_motion_ptr_->getOrigin()->getFrame()->getState("PO");
         auto current_state = processor_motion_ptr_->getState("PO");
         auto current_ts = processor_motion_ptr_->getTimeStamp();
@@ -291,7 +292,8 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         // Fill message
         geometry_msgs::PoseStamped pose_msg;
-        pose_msg.header.stamp = ros::Time(current_ts.getSeconds(), current_ts.getNanoSeconds());
+        auto dt = current_ts - origin_ts;
+        pose_msg.header.stamp = ros::Time(dt);
 
         // 3D
         if(sensor_ptr_->getProblem()->getDim() == 3)