diff --git a/src/problem.cpp b/src/problem.cpp
index 9074a5af59f86b7080ebe6b410737d202d7c2fe1..63775a5ad6fedd519e993604ea745679c37a3f2d 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -704,7 +704,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                 {
                     if (i==0) cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (i==2) cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlockPtrAuto(i);
+                    auto sb = S->getStateBlockPtrDynamic(i);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 18ee9a8e134ca356f027dcbb3bc8a1eb618edc1a..83578afef1d86452eeac5473b397ae87403617b4 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -218,6 +218,23 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
     noise_cov_ = _noise_cov;
 }
 
+CaptureBasePtr SensorBase::lastCapture(void)
+{
+    // we search for the most recent Capture of this sensor
+    CaptureBasePtr capture = nullptr;
+    FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+    FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+    while (frame_rev_it != frame_list.rend())
+    {
+        CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+        if (capture)
+            // found the most recent Capture made by this sensor !
+            break;
+        frame_rev_it++;
+    }
+    return capture;
+}
+
 CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
 {
     // we search for the most recent Capture of this sensor before _ts
@@ -255,44 +272,17 @@ StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
 
 StateBlockPtr SensorBase::getPPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getPPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[0];
+    return getStateBlockPtrDynamic(0);
 }
 
 StateBlockPtr SensorBase::getOPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getOPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[1];
+    return getStateBlockPtrDynamic(1);
 }
 
 StateBlockPtr SensorBase::getIntrinsicPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getIntrinsicPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[2];
+    return getStateBlockPtrDynamic(2);
 }
 
 wolf::Size SensorBase::computeCalibSize() const
@@ -346,30 +336,32 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
     return _proc_ptr;
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
 {
-    assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-    if (((_i < 2) && extrinsicsInCaptures()) || ((_i >= 2) && intrinsicsInCaptures()))
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        CaptureBasePtr cap = lastCapture(_ts);
-        return cap->getStateBlockPtr(_i);
+        CaptureBasePtr cap = lastCapture();
+        if (cap)
+            return cap->getStateBlockPtr(_i);
+        else
+            return getStateBlockPtrStatic(_i);
     }
     else
-        return state_block_vec_[_i];
+        return getStateBlockPtrStatic(_i);
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrAuto(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
 {
-    ProblemPtr P = getProblem();
-    if (P)
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getStateBlockPtrDynamic(_i, KF->getTimeStamp());
-        }
+        CaptureBasePtr cap = lastCapture(_ts);
+        if (cap)
+            return cap->getStateBlockPtr(_i);
+        else
+            return getStateBlockPtrStatic(_i);
     }
-    return state_block_vec_[_i];
+    else
+        return getStateBlockPtrStatic(_i);
 }
 
 } // namespace wolf
diff --git a/src/sensor_base.h b/src/sensor_base.h
index ba16f298480705f209e9ceda483854e946e630cf..8c6b6c168d96023e12f80f819bd467aa1aa7217a 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -102,6 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
         ProcessorBaseList& getProcessorList();
 
+        CaptureBasePtr lastCapture(void);
         CaptureBasePtr lastCapture(const TimeStamp& _ts);
 
         bool process(const CaptureBasePtr capture_ptr);
@@ -110,8 +111,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
         StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
+        StateBlockPtr getStateBlockPtrDynamic(unsigned int _i);
         StateBlockPtr getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts);
-        StateBlockPtr getStateBlockPtrAuto(unsigned int _i);
         void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(int _size);