diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h
index 3208162fcdde1b783cac2e6b37c3c6d1f8b7337d..6f7548cf56690c76156ebdcff6acf12fc2375746 100644
--- a/include/gnss/capture/capture_gnss.h
+++ b/include/gnss/capture/capture_gnss.h
@@ -25,8 +25,8 @@ class CaptureGnss : public CaptureBase
       GnssUtils::SnapshotPtr getSnapshot() const;
       GnssUtils::ObservationsPtr getObservations() const;
       GnssUtils::NavigationPtr getNavigation() const;
-      GnssUtils::SatellitesPositions& getSatellitesPositions();
-      const GnssUtils::SatellitesPositions& getSatellitesPositions() const;
+      GnssUtils::Satellites& getSatellites();
+      const GnssUtils::Satellites& getSatellites() const;
 
 };
 
@@ -45,14 +45,14 @@ inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
   return snapshot_->getNavigation();
 }
 
-inline GnssUtils::SatellitesPositions& CaptureGnss::getSatellitesPositions()
+inline GnssUtils::Satellites& CaptureGnss::getSatellites()
 {
-  return snapshot_->getSatellitesPositions();
+  return snapshot_->getSatellites();
 }
 
-inline const GnssUtils::SatellitesPositions& CaptureGnss::getSatellitesPositions() const
+inline const GnssUtils::Satellites& CaptureGnss::getSatellites() const
 {
-  return snapshot_->getSatellitesPositions();
+  return snapshot_->getSatellites();
 }
 
 } //namespace wolf
diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index 0c12bf9725a6ad0fa9eadfac163d1bbeec044ea9..5c82f25b359734c2e9392b738b10892cd07ecb3b 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -50,9 +50,9 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
             sensor_gnss_ptr_(_sensor_gnss_ptr),
             mode_(_mode),
             std_dev_(_std_dev),
-            satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef())
+            satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef())
         {
-            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
+            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU");
 
             // Pseudo range combination
             switch (mode_)
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index 1e126c64e065665c7bf59ba879d273f0e96a8359..6cab32ff777a7e2821a8fd042ef7c503311bf245 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -55,14 +55,14 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
             sensor_gnss_ptr_(_sensor_gnss_ptr),
             mode_(_mode),
             std_dev_(_std_dev),
-            satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef()),
-            satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef())
+            satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef()),
+            satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef())
         {
             assert(_ftr_r != _ftr_k);
             assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr);
             assert(_ftr_k->getCapture()->getStateBlock("T") != nullptr);
 
-            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
+            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an FactorGnssTdcp without initializing ENU");
 
             // Pseudo range combination
             // TODO encapsular prange a GnssUtils.
diff --git a/include/gnss/feature/feature_gnss_satellite.h b/include/gnss/feature/feature_gnss_satellite.h
index b8d5ce30dcc352bc126e4bdc42517f23bde89ebb..cec277f7da3323177a24413003e07f7222d031e6 100644
--- a/include/gnss/feature/feature_gnss_satellite.h
+++ b/include/gnss/feature/feature_gnss_satellite.h
@@ -6,6 +6,7 @@
 
 //std includes
 #include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/satellite.h"
 
 namespace wolf {
 
@@ -24,17 +25,17 @@ class FeatureGnssSatellite : public FeatureBase
          * This constructor will take the pseudorange as measurement with 1 m² of variance
          *
          */
-        FeatureGnssSatellite(const obsd_t& _obs_sat, const Eigen::Vector3d& _sat_pos);
+        FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat);
 
         virtual ~FeatureGnssSatellite(){};
 
         const obsd_t& getObservation() const;
         int satNumber() const;
-        const Eigen::Vector3d& getSatellitePosition() const;
+        const GnssUtils::Satellite& getSatellite() const;
 
     private:
       obsd_t obs_sat_;
-      Eigen::Vector3d sat_pos_;
+      GnssUtils::Satellite sat_;
 };
 
 
@@ -43,10 +44,10 @@ class FeatureGnssSatellite : public FeatureBase
 // IMPLEMENTATION
 namespace wolf {
 
-inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const Eigen::Vector3d& _sat_pos) :
+inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat) :
     FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_obs_sat.P[0]), Eigen::Matrix1d(1.0)),
     obs_sat_(_obs_sat),
-    sat_pos_(_sat_pos)
+    sat_(_sat)
 {
     //
 }
@@ -61,9 +62,9 @@ inline int FeatureGnssSatellite::satNumber() const
     return obs_sat_.sat;
 }
 
-inline const Eigen::Vector3d& FeatureGnssSatellite::getSatellitePosition() const
+inline const GnssUtils::Satellite& FeatureGnssSatellite::getSatellite() const
 {
-    return sat_pos_;
+    return sat_;
 }
 
 } // namespace wolf
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index ff3a0a5e022fd00cd5abbc1f70d319fa7db3dfde..16cbd7c4cac7cf319c2bb5a8d2227acac2a55c7c 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -17,12 +17,14 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     prcopt_t gnss_opt;
     double enu_map_init_dist_min;
     double max_time_span;
+    bool remove_outliers;
 
     ParamsProcessorTrackerGnss() = default;
     ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorTrackerFeature(_unique_name, _server)
     {
         enu_map_init_dist_min   = _server.getParam<double>  (prefix + _unique_name + "/enu_map_init_dist_min");
+        remove_outliers         = _server.getParam<bool>    (prefix + _unique_name + "/remove_outliers");
         max_time_span           = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
 
         // GNSS OPTIONS (see rtklib.h)
@@ -53,6 +55,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     {
         return "\n"                     + ParamsProcessorBase::print()          + "\n"
             + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n"
+            + "remove_outliers: "       + std::to_string(remove_outliers)       + "\n"
+            + "max_time_span: "         + std::to_string(max_time_span)         + "\n"
             + "mode: "                  + std::to_string(gnss_opt.mode)         + "\n"
             + "soltype: "               + std::to_string(gnss_opt.soltype)      + "\n"
             + "nf: "                    + std::to_string(gnss_opt.nf)           + "\n"
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index c382cba77af6e4886d948e6060e7c59dc8484653..3e9acea97f81501e3a504b01fed32377d1214510 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -13,8 +13,8 @@ void ProcessorTrackerGnss::preProcess()
     GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
 
     // compute satellites positions
-    if (!inc_snapshot ->satellitesPositionsComputed())
-        inc_snapshot ->computeSatellitesPositions(params_tracker_gnss_->gnss_opt.sateph);
+    if (!inc_snapshot ->satellitesComputed())
+        inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph);
 
     // Compute (and store) auxiliar fix (for computing elevation)
     GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(),
@@ -31,6 +31,8 @@ void ProcessorTrackerGnss::preProcess()
         sensor_gnss_->setEcefEnu(fix_output.pos,true);
         //sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true);
         WOLF_INFO("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef());
+
+        WOLF_INFO("should be zero: ", (sensor_gnss_->getREnuEcef() * fix_output.pos + sensor_gnss_->gettEnuEcef()).transpose());
     }
 
     WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose());
@@ -47,7 +49,7 @@ void ProcessorTrackerGnss::preProcess()
     // create features pseudorange
     for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
     {
-        auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellitesPositions().at(obs_sat.sat));
+        auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellites().at(obs_sat.sat));
         assert(untracked_incoming_features_.count(feat->satNumber()) == 0);
         untracked_incoming_features_[feat->satNumber()] = feat;
     }
@@ -134,11 +136,7 @@ void ProcessorTrackerGnss::establishFactors()
 
     FactorBasePtrList new_factors;
 
-    if (last_ptr_->getStateBlock("T") == nullptr)
-    {
-        WOLF_DEBUG("last clock offset state block nullptr, adding it");
-        last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(1), getProblem());
-    }
+    bool last_clock_drift_set = false;
 
     // PSEUDO RANGE FACTORS (all sats)
     for (auto ftr : last_ptr_->getFeatureList())
@@ -151,6 +149,19 @@ void ProcessorTrackerGnss::establishFactors()
         if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
             continue;
 
+        // Initialize (and create) clock drift stateblock in capture
+        if (!last_clock_drift_set)
+        {
+            if (last_ptr_->getStateBlock("T") == nullptr)
+                last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(-fix_last_(3))), getProblem());
+            else
+                last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(-fix_last_(3)));
+
+            WOLF_INFO("last clock drift initialized: ", last_ptr_->getStateBlock("T")->getState().transpose());
+
+            last_clock_drift_set = true;
+        }
+
         // emplace a pseudo range factor
         auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
                                                                   GnssUtils::Combination::CODE_L1,//TODO: from params
@@ -196,6 +207,16 @@ void ProcessorTrackerGnss::establishFactors()
 //
 //                WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
 //
+//                // Initialize (and create) clock drift stateblock in capture
+//                if (!last_clock_drift_set)
+//                {
+//                    if (last_ptr_->getStateBlock("T") == nullptr)
+//                        last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(fix_last_(3))), getProblem());
+//                    else
+//                        last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(fix_last_(3)));
+//                    last_clock_drift_set = true;
+//                }
+//
 //                // emplace tdcp factor
 //                auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
 //                                                                   GnssUtils::Combination::CARRIER_L1,//TODO: from params
@@ -216,7 +237,8 @@ void ProcessorTrackerGnss::establishFactors()
 //    }
 
     // remove outliers
-    removeOutliers(new_factors, last_ptr_);
+    if (!new_factors.empty() and params_tracker_gnss_->remove_outliers)
+        removeOutliers(new_factors, last_ptr_);
 }
 
 void ProcessorTrackerGnss::advanceDerived()
@@ -279,7 +301,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
             WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
 
             // discard if residual too high evaluated at the current estimation
-            if (residual_pr(0) > 1e3)
+            if (std::abs(residual_pr(0)) > 1e3)
             {
                 WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
                 WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index 9812994863bae8a59c69b8eef88fcd9b1d02fa30..ce0723f7a39529595ca59ec3133cc4853629cae9 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -17,9 +17,12 @@ Vector3d t_ecef_antena;
 Vector3d rpy_enu_map;
 Matrix3d R_ecef_enu, R_enu_map;
 Quaterniond q_map_base;
-Vector3d t_ecef_sat1, t_ecef_sat2, t_ecef_sat3, t_ecef_sat4;
 double prange_1, prange_2, prange_3, prange_4;
 Vector1d clock_drift;
+GnssUtils::Satellite sat1(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat2(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat3(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat4(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
 
 // WOLF
 ProblemPtr prb = Problem::create("PO", 3);
@@ -57,19 +60,19 @@ void randomGroundtruth()
     t_ecef_antena = R_ecef_enu * (R_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
 
     // SATELLITES
-    t_ecef_sat1 = Vector3d::Random() * 1e4;
-    t_ecef_sat2 = Vector3d::Random() * 1e4;
-    t_ecef_sat3 = Vector3d::Random() * 1e4;
-    t_ecef_sat4 = Vector3d::Random() * 1e4;
+    sat1.pos_ = Vector3d::Random() * 1e4;
+    sat2.pos_ = Vector3d::Random() * 1e4;
+    sat3.pos_ = Vector3d::Random() * 1e4;
+    sat4.pos_ = Vector3d::Random() * 1e4;
 
     // clock drift
     clock_drift = Vector1d::Random() * 1e2;
 
     // pseudo ranges
-    prange_1 = (t_ecef_sat1-t_ecef_antena).norm() + clock_drift(0);
-    prange_2 = (t_ecef_sat2-t_ecef_antena).norm() + clock_drift(0);
-    prange_3 = (t_ecef_sat3-t_ecef_antena).norm() + clock_drift(0);
-    prange_4 = (t_ecef_sat4-t_ecef_antena).norm() + clock_drift(0);
+    prange_1 = (sat1.pos_-t_ecef_antena).norm() + clock_drift(0);
+    prange_2 = (sat2.pos_-t_ecef_antena).norm() + clock_drift(0);
+    prange_3 = (sat3.pos_-t_ecef_antena).norm() + clock_drift(0);
+    prange_4 = (sat4.pos_-t_ecef_antena).norm() + clock_drift(0);
 }
 
 void setUpProblem()
@@ -104,16 +107,16 @@ void setUpProblem()
     // features
     obsd_t obs1{0};
     obs1.P[0] = prange_1;
-    ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, t_ecef_sat1);
+    ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, sat1);
     obsd_t obs2{0};
     obs2.P[0] = prange_2;
-    ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, t_ecef_sat2);
+    ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, sat2);
     obsd_t obs3{0};
     obs3.P[0] = prange_3;
-    ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, t_ecef_sat3);
+    ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, sat3);
     obsd_t obs4{0};
     obs4.P[0] = prange_4;
-    ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4);
+    ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, sat4);
 
     // factors
     fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false);
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index 1cd9dcdf3df75e3a65e978c289b90945ed6a0b85..41f195537055d0e6f6b84c43f6941e98abd1283c 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -17,10 +17,18 @@ Vector3d t_ecef_antena_r, t_ecef_antena_k;
 Vector3d rpy_enu_map;
 Matrix3d R_ecef_enu, R_enu_map;
 Quaterniond q_map_base_r, q_map_base_k;
-Vector3d t_ecef_sat1_r, t_ecef_sat2_r, t_ecef_sat3_r, t_ecef_sat4_r, t_ecef_sat1_k, t_ecef_sat2_k, t_ecef_sat3_k, t_ecef_sat4_k;
 double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
 Vector1d clock_drift_r, clock_drift_k;
 
+GnssUtils::Satellite sat1_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat2_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat3_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat4_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat1_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat2_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat3_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+GnssUtils::Satellite sat4_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0);
+
 // WOLF
 ProblemPtr prb = Problem::create("PO", 3);
 CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
@@ -60,28 +68,28 @@ void randomGroundtruth()
     t_ecef_antena_k = R_ecef_enu * (R_enu_map * (q_map_base_k * t_base_antena + t_map_base_k) + t_enu_map ) + t_ecef_enu;
 
     // SATELLITES
-    t_ecef_sat1_r = Vector3d::Random() * 1e4;
-    t_ecef_sat2_r = Vector3d::Random() * 1e4;
-    t_ecef_sat3_r = Vector3d::Random() * 1e4;
-    t_ecef_sat4_r = Vector3d::Random() * 1e4;
-    t_ecef_sat1_k = Vector3d::Random() * 1e4;
-    t_ecef_sat2_k = Vector3d::Random() * 1e4;
-    t_ecef_sat3_k = Vector3d::Random() * 1e4;
-    t_ecef_sat4_k = Vector3d::Random() * 1e4;
+    sat1_r.pos_ = Vector3d::Random() * 1e4;
+    sat2_r.pos_ = Vector3d::Random() * 1e4;
+    sat3_r.pos_ = Vector3d::Random() * 1e4;
+    sat4_r.pos_ = Vector3d::Random() * 1e4;
+    sat1_k.pos_ = Vector3d::Random() * 1e4;
+    sat2_k.pos_ = Vector3d::Random() * 1e4;
+    sat3_k.pos_ = Vector3d::Random() * 1e4;
+    sat4_k.pos_ = Vector3d::Random() * 1e4;
 
     // clock drift
     clock_drift_r = Vector1d::Random() * 1e2;
     clock_drift_k = Vector1d::Random() * 1e2;
 
     // pseudo ranges
-    prange1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0);
-    prange2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0);
-    prange3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0);
-    prange4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0);
-    prange1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0);
-    prange2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0);
-    prange3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0);
-    prange4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange1_r = (sat1_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange2_r = (sat2_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange3_r = (sat3_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange4_r = (sat4_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange1_k = (sat1_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange2_k = (sat2_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange3_k = (sat3_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange4_k = (sat4_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
 }
 
 void setUpProblem()
@@ -128,30 +136,30 @@ void setUpProblem()
     // features r
     obsd_t obs1_r{0};
     obs1_r.L[0] = prange1_r;
-    ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, t_ecef_sat1_r);
+    ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, sat1_r);
     obsd_t obs2_r{0};
     obs2_r.L[0] = prange2_r;
-    ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, t_ecef_sat2_r);
+    ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, sat2_r);
     obsd_t obs3_r{0};
     obs3_r.L[0] = prange3_r;
-    ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, t_ecef_sat3_r);
+    ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, sat3_r);
     obsd_t obs4_r{0};
     obs4_r.L[0] = prange4_r;
-    ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, t_ecef_sat4_r);
+    ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, sat4_r);
 
     // features k
     obsd_t obs1_k{0};
     obs1_k.L[0] = prange1_k;
-    ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, t_ecef_sat1_k);
+    ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, sat1_k);
     obsd_t obs2_k{0};
     obs2_k.L[0] = prange2_k;
-    ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, t_ecef_sat2_k);
+    ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, sat2_k);
     obsd_t obs3_k{0};
     obs3_k.L[0] = prange3_k;
-    ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, t_ecef_sat3_k);
+    ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, sat3_k);
     obsd_t obs4_k{0};
     obs4_k.L[0] = prange4_k;
-    ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, t_ecef_sat4_k);
+    ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, sat4_k);
 
     // factors
     fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);