diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 75a336731cf3e0599719261d5fb3f850fc67b5b1..9c3729a61c80e868a0b8af2b6421ffdfddc0731d 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -96,6 +96,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
                    bool state_blocks,
                    std::ostream& stream = std::cout,
                    std::string _tabs = "") const;
+        void printDebug();
     private:
         FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
         void removeFrame(FrameBasePtr _frame_ptr);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 36425f7946971b25c1921f6d93cef800fbf0a81a..d8b3e67446fb83a815c6330c587a0634b8e35b41 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -459,9 +459,10 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
+    auto new_ts = origin_ts + 1e-9;
     auto new_frame_ptr  = getProblem()->emplaceFrame(NON_ESTIMATED,
                                                      _origin_frame->getState(),
-                                                     origin_ts);
+                                                     new_ts);
                                         
     // emplace (emtpy) last Capture
     last_ptr_ = emplaceCapture(new_frame_ptr,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index f326eb329e7a1b687acf258352df211ea79ce166..ae9444543bb82649d5a549b86dfa9aeeb2456445 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -107,8 +107,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         case SECOND_TIME_WITHOUT_PACK :
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
-
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+auto test_stamp = incoming_ptr_->getTimeStamp()+2e-9;
+WOLF_DEBUG("TIME STAMP ", test_stamp);
+            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()+2e-9);
             incoming_ptr_->link(frm);
 
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index cffa28dcfa965f4dc216808fe0beda9f1de44d12..1cf3a913e95271cd82a90cbb3d139b301ad1deba 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -20,8 +20,10 @@ TrajectoryBase::~TrajectoryBase()
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
-
+    std::cout << "Inserting " << _frame_ptr->getTimeStamp() << " " << _frame_ptr << std::endl;
+    auto success = frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
+    std::cout << "Sucess? " << success.second << std::endl;
+    printDebug();
     if (_frame_ptr->isKeyOrAux())
     {
         // update last_estimated and last_key
@@ -35,7 +37,7 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
     // frame_list_.erase(_frame_ptr);
-    frame_list_.erase(_frame_ptr->id());
+    frame_list_.erase(_frame_ptr->getTimeStamp());
 
     // update last_estimated and last_key
     if (_frame_ptr->isKeyOrAux())
@@ -124,4 +126,9 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
             F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 
 }
+void TrajectoryBase::printDebug()
+{
+    for( auto it : frame_list_ )
+        WOLF_DEBUG("KEY ", it.first, " VALUE ", it.second);
+}
 } // namespace wolf
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1665ca2df5c419a55f58ae427316710ccff6e0fd..25da38b6b7452d810cb29c8dca795b8edcd27c18 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -115,7 +115,6 @@ TEST(ProcessorBase, KeyFrameCallback)
 
         capt_odo->setTimeStamp(t);
         std::cout << "2\n";
-        problem->check(1);
 //        auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
 //        auto last_ptr = proc_odo_motion->last_ptr_;
 //        auto last_ptr_frame = last_ptr->getFrame();
@@ -125,6 +124,7 @@ TEST(ProcessorBase, KeyFrameCallback)
         // Track
         capt_trk = make_shared<CaptureVoid>(t, sens_trk);
         std::cout << "4\n";
+        problem->print(4,1,1,1, std::cout);
         proc_trk->captureCallback(capt_trk);
         std::cout << "5\n";
 
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 604cd09b4f20ed68e1c44273f85a93c8c1a88c1a..096dd25542b869f8ee1b1d41969c7a5dc8378a25 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -311,6 +311,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
     //4TH TIME
     WOLF_INFO("Forth time...");
     CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+    problem->getTrajectory()->printDebug();
     cap4->process();
 
     ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);