diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index 2f3da744717fa0c9a8d99e6d163a09f0a16ea77c..ef833a48a77679f658627edc3be1a7a68f972769 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -354,12 +354,16 @@ TEST(Odom2D, KF_callback)
     // Capture to use as container for all incoming data
     CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
 
+    std::cout << "t: " << t << std::endl;
     for (int n=1; n<=N; n++)
     {
         t += dt;
 
         // re-use capture with updated timestamp
         capture->setTimeStamp(t);
+        std::cout << "capture ts: " << capture->getTimeStamp() << " - " << capture->getTimeStamp().get();
+        std::cout << "nsec:        " << capture->getTimeStamp().getNanoSeconds() << std::endl;
+        std::cout << "filled nsec: " << std::setfill('0') << std::setw(9) << std::right << capture->getTimeStamp().getNanoSeconds() << std::endl;
 
         // Processor
         sensor_odom2d->process(capture);
@@ -388,9 +392,9 @@ TEST(Odom2D, KF_callback)
     ////////////////////////////////////////////////////////////////
     // Split after the last keyframe, exact timestamp
     int n_split = 8;
-    TimeStamp t_split (t0 + n_split*dt);
+    TimeStamp t_split = t0 + n_split*dt;
 
-//    std::cout << "-----------------------------\nSplit after last KF; time: " << t_split - t0 << std::endl;
+    std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
 
     Vector3s x_split = processor_odom2d->getState(t_split);
     FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
@@ -418,7 +422,7 @@ TEST(Odom2D, KF_callback)
     // Split between keyframes, exact timestamp
     int m_split = 4;
     t_split = t0 + m_split*dt;
-//    std::cout << "-----------------------------\nSplit between KFs; time: " << t_split - t0 << std::endl;
+    std::cout << "-----------------------------\nSplit between KFs; time: " << t_split << std::endl;
 
     problem->print(4,1,1,1);
 
@@ -445,6 +449,8 @@ TEST(Odom2D, KF_callback)
     report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
     ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
+    problem->print(4,1,1,1);
+
     // check the split KF
     ASSERT_POSE2D_APPROX(keyframe_1->getState()                  , integrated_pose_vector[m_split], 1e-6);
     ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6); // FIXME test does not pass
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 643c1de6c20deb495455719103fb190794b241e5..861104832b76751b1034ce36c4c794efae2d0454 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -14,7 +14,7 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
     //    std::streamsize nn;
     //    nn = os.precision(TimeStamp::TIME_STAMP_DIGITS_);
 
-    os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) <<_ts.getNanoSeconds(); // write obj to stream
+    os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
     return os;
 }
 
@@ -35,7 +35,7 @@ TimeStamp::TimeStamp(const TimeStamp& _ts) :
 
 TimeStamp::TimeStamp(const Scalar& _ts) :
         //time_stamp_(_ts)
-        time_stamp_nano_((unsigned long int)(_ts*1e9))
+        time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0)
 {
     //
 }
@@ -90,7 +90,7 @@ void TimeStamp::print(std::ostream & ost) const
     //ost.flags(fmtfl);
     //ost.precision(nn);
 
-    ost << this->getSeconds() << "." << std::setfill('0') << std::setw(9) << this->getNanoSeconds();
+    ost << *this;
 }
 
 } // namespace wolf
diff --git a/src/time_stamp.h b/src/time_stamp.h
index fd5593404036e97b03b705c3953ad02fa18df69d..88c65a1cd4387ab049e9a78300932e893d3d5dc5 100644
--- a/src/time_stamp.h
+++ b/src/time_stamp.h
@@ -179,7 +179,7 @@ class TimeStamp
 inline void TimeStamp::set(const Scalar& ts)
 {
     //time_stamp_ = ts;
-    time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
+    time_stamp_nano_ = (ts > 0 ? (unsigned long int)(ts*NANOSECS) : 0);
 }
 
 inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
@@ -267,13 +267,13 @@ inline bool TimeStamp::operator >=(const TimeStamp& ts) const
 inline void TimeStamp::operator +=(const Scalar& dt)
 {
     //time_stamp_ += dt;
-    time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
+    time_stamp_nano_ += (unsigned long int)(dt*1e9);
 }
 
 inline void TimeStamp::operator -=(const Scalar& dt)
 {
     //time_stamp_ -= dt;
-    time_stamp_nano_ -= (unsigned long int)(dt*NANOSECS);
+    time_stamp_nano_ -= (unsigned long int)(dt*1e9);
 }
 
 inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 577291e4f4621d95e1c42c7c66b7ffa83a4735b5..df3a9f5f7d932c5049ba093c59d3206ab19ddcc8 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -88,7 +88,8 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
     for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
         if ((*frm_rit)->isKey())
         {
-            Scalar dt = std::abs((*frm_rit)->getTimeStamp().get() - _ts.get());
+            Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
+            std::cout << "dt " << dt << std::endl;
             if (dt < min_dt)
             {
                 min_dt = dt;