diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
index 940b759d652096d080c5cd5a909b2426ffa09aa4..13645d45322506f10858940014b25ee1afcd3fcc 100644
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -50,6 +50,7 @@ class ProcessorGnssSingleDiff : public ProcessorBase
         ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss);
         virtual ~ProcessorGnssSingleDiff();
         virtual void configure(SensorBasePtr _sensor);
+        FrameBasePtr getLastKF();
 
     protected:
         /** \brief process an incoming capture
@@ -93,6 +94,11 @@ inline bool ProcessorGnssSingleDiff::triggerInCapture(CaptureBasePtr)
     return true;
 }
 
+inline wolf::FrameBasePtr ProcessorGnssSingleDiff::getLastKF()
+{
+    return last_KF_;
+}
+
 } // namespace wolf
 
 #endif //WOLF_PROCESSOR_GNSS_SINGLE_DIFF_H
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index cb37515ef9340980143db6dcc14d65740ec9b6d6..c8684af15b80e015f78d87a8c222397c46d84a84 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -28,26 +28,33 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
 
     // discard capture with null or non-key origin frame
     if (incoming_capture_->getOriginFrame() == nullptr || !incoming_capture_->getOriginFrame()->isKey())
+    {
+        WOLF_WARN("process single difference with null frame origin, skipping...");
         return;
+    }
+
+    if (last_KF_ == nullptr)
+        last_KF_ = incoming_capture_->getOriginFrame();
 
+    // NEW KF? ------------------------------------------------
     FrameBasePtr new_frame = nullptr;
 
     // ALREADY CREATED KF
-    PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture, params_->time_tolerance);
+    PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_->time_tolerance);
     if (KF_pack && KF_pack->key_frame != incoming_capture_->getOriginFrame())
     {
         new_frame = KF_pack->key_frame;
-        WOLF_DEBUG( "PR ",getName()," - capture ", _capture->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
+        WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
     }
     // MAKE KF
     else if (voteForKeyFrame() && permittedKeyFrame())
     {
-        new_frame = getProblem()->emplaceFrame(KEY, _capture->getTimeStamp());
+        new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
         getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance);
-        WOLF_DEBUG( "PR ",getName()," - capture ", _capture->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
+        WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
     }
 
-    // ESTABLISH FACTOR
+    // ESTABLISH FACTOR ------------------------------------------------
     if (new_frame)
     {
         // LINK CAPTURE
@@ -56,14 +63,14 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
         // EMPLACE FEATURE
         auto ftr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
 
-        // ADD CONSTRAINT
+        // ADD FACTOR
         emplaceFactor(ftr);
 
         // store last KF
         last_KF_ = new_frame;
     }
 
-    // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
+    // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS ------------------------------------------------
     //      1 - ENU-ECEF defined
     //      2 - not initialized
     //      3 - current capture in key-frame with factor
@@ -83,7 +90,6 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
 
 FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr)
 {
-    // CREATE CONSTRAINT --------------------
     //WOLF_DEBUG("creating the factor...");
     // 2D
     if (getProblem()->getDim() == 2)
@@ -97,14 +103,17 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr)
 
 bool ProcessorGnssSingleDiff::voteForKeyFrame()
 {
+    if (last_KF_==nullptr)
+        return true;
+
     // Depending on time since the last KF with gnssfix capture
-    if (last_KF_==nullptr || (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
+    if ((last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
         return true;
 
     // Distance criterion
-    std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
-    std::cout << "incoming_capture_->getData().norm()" << incoming_capture_->getData().norm() << std::endl;
-    if (incoming_capture_->getData().norm() > params_gnss_->dist_traveled)
+    Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+    Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
+    if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
         return true;
 
     // TODO: more alternatives?