diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index ade207815d753393d15110d0a703c860d6dafe1e..1ca871af231c0b3f7fbe7c0cbb42011c9ed217df 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -17,6 +17,9 @@
 
 using namespace wolf;
 
+/// Set to true if you want debug info
+bool debug = false;
+
 TEST(TrajectoryBase, ClosestKeyFrame)
 {
 
@@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     TrajectoryBasePtr T = P->getTrajectoryPtr();
 
     // Trajectory status:
-    //
     //  kf1   kf2    f3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
@@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     T->addFrame(f2);
     T->addFrame(f3);
 
-    P->print();
-
     FrameBasePtr kf; // closest key-frame queried
 
     kf = T->closestKeyFrameToTimeStamp(-0.8);                // before all keyframes    --> return f0
@@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     TrajectoryBasePtr T = P->getTrajectoryPtr();
 
     // Trajectory status:
-    //
     //  kf1   kf2    f3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
@@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame)
 
     // add frames and keyframes
     T->addFrame(f1); // KF
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 1);
     ASSERT_EQ(P->getStateBlockList().            size(), 2);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
 
     T->addFrame(f2); // KF
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 2);
     ASSERT_EQ(P->getStateBlockList().            size(), 4);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
 
     T->addFrame(f3); // F
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 3);
     ASSERT_EQ(P->getStateBlockList().            size(), 4);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
@@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
 
     // remove frames and keyframes
     f2->remove(); // KF
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 2);
     ASSERT_EQ(P->getStateBlockList().            size(), 2);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
@@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
 
     f3->remove(); // F
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 1);
     ASSERT_EQ(P->getStateBlockList().            size(), 2);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
@@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
 
     f1->remove(); // KF
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), 0);
     ASSERT_EQ(P->getStateBlockList().            size(), 0);
     ASSERT_EQ(P->getStateBlockNotificationList().size(), 0);
@@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     TrajectoryBasePtr T = P->getTrajectoryPtr();
 
     // Trajectory status:
-    //
     //  kf1   kf2    f3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
@@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
 
     // add frames and keyframes in random order --> keyframes must be sorted after that
     T->addFrame(f2); // KF2
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f2->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
 
     T->addFrame(f3); // F3
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
 
     T->addFrame(f1); // KF1
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
 
     f3->setKey(); // set KF3
-    P->print(2,0,0,0);
+    if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
 
     FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
     T->addFrame(f4);
-    P->print(2,0,1,0);
+    // Trajectory status:
+    //  kf1   kf2   kf3     f4       frames
+    //   1     2     3     1.5       time stamps
+    // --+-----+-----+------+--->    time
+    if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f4->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
 
     f4->setKey();
-    P->print(2,0,1,0);
+    // Trajectory status:
+    //  kf1   kf4   kf2    kf3       frames
+    //   1    1.5    2      3        time stamps
+    // --+-----+-----+------+--->    time
+    if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFramePtr()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
 
+    f2->setTimeStamp(4);
+    // Trajectory status:
+    //  kf1   kf4   kf3    kf2       frames
+    //   1    1.5    3      4        time stamps
+    // --+-----+-----+------+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFramePtr()   ->id(), f2->id());
+    ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+
+    f4->setTimeStamp(0.5);
+    // Trajectory status:
+    //  kf4   kf2   kf3    kf2       frames
+    //  0.5    1     3      4        time stamps
+    // --+-----+-----+------+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
+
 }
 
 int main(int argc, char **argv)
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 6eee6707e25f5db2c0d786e89f048b7bf4480057..20c5427cf393f1b5cfb2c1757fa8b50dfea65321 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
 void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
 {
     moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
+    last_key_frame_ptr_ = findLastKeyFramePtr();
 }
 
 FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)