From d170f8eae31d67a11022ec1b3c0e7c04b80c4993 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 12 May 2020 21:30:21 +0200
Subject: [PATCH] ProcessorTrackerGnss: pseudoranges working

---
 .../gnss/factor/factor_gnss_pseudo_range.h    |  55 +++++--
 .../gnss/processor/processor_tracker_gnss.h   |  12 +-
 src/processor/processor_tracker_gnss.cpp      | 155 +++++++++++++-----
 test/gtest_factor_gnss_fix_2d.cpp             |   5 +-
 test/gtest_factor_gnss_pseudo_range.cpp       |   8 +-
 5 files changed, 170 insertions(+), 65 deletions(-)

diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index 8212bc7e4..510105781 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -13,11 +13,13 @@ namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FactorGnssPseudoRange);
 
-class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1>
+class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>
 {
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
-        Eigen::Vector3d satellite_ENU_;
+        Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
+        double sagnac_correction_;
+        bool not_GPS_;
 
     public:
 
@@ -26,7 +28,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
                               const ProcessorBasePtr& _processor_ptr,
                               bool _apply_loss_function,
                               FactorStatus _status = FAC_ACTIVE) :
-            FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
+            FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
                                                                              nullptr,
                                                                              nullptr,
                                                                              nullptr,
@@ -37,16 +39,31 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
                                                                              _ftr_ptr->getFrame()->getP(),
                                                                              _ftr_ptr->getFrame()->getO(),
                                                                              _ftr_ptr->getCapture()->getStateBlock("T"),
+                                                                             (_ftr_ptr->getSatellite().sys == SYS_GLO ?
+                                                                                     _ftr_ptr->getCapture()->getStateBlock("TG") :
+                                                                                     (_ftr_ptr->getSatellite().sys == SYS_GAL ?
+                                                                                             _ftr_ptr->getCapture()->getStateBlock("TE") :
+                                                                                             _ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used
                                                                              _sensor_gnss_ptr->getP(),
                                                                              _sensor_gnss_ptr->getEnuMapTranslation(),
                                                                              _sensor_gnss_ptr->getEnuMapRoll(),
                                                                              _sensor_gnss_ptr->getEnuMapPitch(),
                                                                              _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr),
-            satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
+            satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
+            satellite_ECEF_(_ftr_ptr->getSatellite().pos),
+            not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS)
         {
             //WOLF_INFO("FactorPseudoRange: sat ", _ftr_ptr->getSatellite().sat,"\n\tpos = ", _ftr_ptr->getSatellite().pos.transpose(), "\n\tprange = ", _ftr_ptr->getMeasurement());
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU");
+
+            /* SAGNAC EFFECT CORRECTION
+             * It should be recomputed with the antenna position in ECEF coordinates,
+             * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
+             * We use instead a precomputed sagnac effect taking ENU origin as antenna position.
+             */
+            Eigen::Vector3d t_ECEF_ENU = -sensor_gnss_ptr_->getREnuEcef().transpose() * sensor_gnss_ptr_->gettEnuEcef();
+            sagnac_correction_ = OMGE*(satellite_ECEF_(0)*t_ECEF_ENU(1)-satellite_ECEF_(1)*t_ECEF_ENU(0))/CLIGHT;// change to antenna_ECEF
         }
 
         virtual ~FactorGnssPseudoRange() = default;
@@ -60,6 +77,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
         bool operator ()(const T* const _x,
                          const T* const _o,
                          const T* const _clock_bias,
+                         const T* const _clock_bias_constellation,
                          const T* const _x_antena,
                          const T* const _t_ENU_map,
                          const T* const _roll_ENU_map,
@@ -73,6 +91,7 @@ template<typename T>
 inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
                                                const T* const _o,
                                                const T* const _clock_bias,
+                                               const T* const _clock_bias_constellation,
                                                const T* const _x_antena,
                                                const T* const _t_ENU_map,
                                                const T* const _roll_ENU_map,
@@ -90,17 +109,27 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
     // Antenna position in ENU coordinates
     Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena);
 
-    //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
-    //std::cout << std::setprecision(10) << "x = " << antena_ENU(0) << " "<<  antena_ENU(1) << " "<<  antena_ENU(2) << "\n";
-
     // Expected pseudo-range
-    T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() - _clock_bias[0];
+    T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() +      // norm
+            sagnac_correction_ +                                // sagnac correction (precomputed)
+            _clock_bias[0] +                                    // receiver clock bias (w.r.t. GPS clock)
+            (not_GPS_ ? _clock_bias_constellation[0] : T(0));   // interconstellation clock bias
 
-    //std::cout << "clock_bias = " <<  _clock_bias[0] << "\n";
-    //std::cout << "norm = " <<  (antena_ENU-satellite_ENU_.cast<T>()).norm() << "\n";
-    //std::cout << "exp  = " <<  exp << "\n";
-    //std::cout << "meas = " << getMeasurement() << "\n";
-    //std::cout << "error = " << exp - getMeasurement()(0) << "\n";
+
+    //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
+    //std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl;
+    //std::cout << std::setprecision(13) <<"\tsat_pos= " <<  satellite_ENU_.transpose() << "\n";
+    //std::cout << "\tx = " << antena_ENU(0) << " "<<  antena_ENU(1) << " "<<  antena_ENU(2) << "\n";
+    //std::cout << "\tprange = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().p << "\n";
+    //std::cout << "\tiono_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().iono_corr << "\n";
+    //std::cout << "\ttropo_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().tropo_corr << "\n";
+    //std::cout << "\tsat_clock_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().sat_clock_corr << "\n";
+    //std::cout << "\tcorrected prange = " << getMeasurement() << "\n";
+    //std::cout << "\tclock_bias = " <<  _clock_bias[0]  << " " << _clock_bias_constellation[0] << "\n";
+    //std::cout << "\tnorm = " <<  (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ << "\n";
+    //std::cout << "\texp_wo = " <<  (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ + _clock_bias[0] << "\n";
+    //std::cout << "\texp = " <<  exp << "\n";
+    //std::cout << "\terror = " << exp - getMeasurement()(0) << "\n";
 
     // Residual
     _residual[0] = (exp - getMeasurement()(0)) / getMeasurementSquareRootInformationUpper()(0,0);
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index c1fa0a82d..3a4cecfac 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -18,6 +18,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     double enu_map_init_dist_min;
     double max_time_span;
     bool remove_outliers;
+    bool init_frames;
 
     ParamsProcessorTrackerGnss() = default;
     ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -25,7 +26,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     {
         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");
+        init_frames             = _server.getParam<bool>    (prefix + _unique_name + "/init_frames");
+        remove_outliers         = _server.getParam<bool>    (prefix + _unique_name + "/remove_outliers");
 
         // GNSS OPTIONS (see rtklib.h)
         gnss_opt = prcopt_default;
@@ -56,6 +58,7 @@ 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"
+            + "init_frames: "           + std::to_string(init_frames)           + "\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"
@@ -186,13 +189,6 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
         void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
 };
 
-inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
-        ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
-        params_tracker_gnss_(_params_tracker_gnss)
-{
-    //
-}
-
 inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
 {
     //
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index bd4118018..8bfc7ee49 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -8,8 +8,16 @@
 namespace wolf
 {
 
+ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
+        ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
+        params_tracker_gnss_(_params_tracker_gnss)
+{
+}
+
 void ProcessorTrackerGnss::preProcess()
 {
+    WOLF_INFO("ProcessorTrackerGnss::preProcess");
+
     GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
 
     // compute satellites positions
@@ -24,12 +32,11 @@ void ProcessorTrackerGnss::preProcess()
     // Set ECEF-ENU
     if (!sensor_gnss_->isEnuDefined() and fix_incoming_.stat != 0)
     {
-        WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+        WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
         sensor_gnss_->setEcefEnu(fix_incoming_.pos,true);
-        WOLF_DEBUG("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef());
     }
 
-    WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
+    WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
 
     // filter observations (available ephemeris, constellations and elevation&SNR)
     inc_snapshot->filterObservations(std::set<int>(), // discarded sats
@@ -38,13 +45,15 @@ void ProcessorTrackerGnss::preProcess()
                                      false, // check carrier phase
                                      params_tracker_gnss_->gnss_opt);
 
-    WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
+    WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
 
     // compute corrected pseudoranges
     inc_snapshot->computePseudoRanges(fix_incoming_.sat_azel,
                                       fix_incoming_.lat_lon,
                                       params_tracker_gnss_->gnss_opt);
 
+    WOLF_INFO("preprocess: pseudo ranges computed");
+
     // create features pseudorange
     for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
     {
@@ -56,7 +65,7 @@ void ProcessorTrackerGnss::preProcess()
         untracked_incoming_features_[feat->satNumber()] = feat;
     }
 
-    WOLF_DEBUG("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
+    WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
 }
 
 unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in,
@@ -64,7 +73,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
                                                  FeatureBasePtrList& _features_out,
                                                  FeatureMatchMap& _feature_correspondences)
 {
-    WOLF_DEBUG("tracking " , _features_in.size() , " features...");
+    WOLF_INFO("tracking " , _features_in.size() , " features...");
 
     assert(_capture == incoming_ptr_);
 
@@ -81,17 +90,19 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
             _features_out.push_back(ftr);
             _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
 
-            WOLF_DEBUG("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
+            WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
         }
     }
 
-    WOLF_DEBUG("After tracking:\n\ttracked features: " , _features_out.size());
+    WOLF_INFO("After tracking:\n\ttracked features: " , _features_out.size());
 
     return _features_out.size();
 }
 
 bool ProcessorTrackerGnss::voteForKeyFrame() const
 {
+    //WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame");
+
     // too old origin
     if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
     {
@@ -100,7 +111,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
     }
 
     // known features
-    WOLF_DEBUG("Nbr. of active feature tracks: " , known_features_incoming_.size() );
+    WOLF_INFO("Nbr. of active feature tracks: " , known_features_incoming_.size() );
     if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe)
     {
         WOLF_INFO( "Vote for KF because of too less known_features_incoming" );
@@ -117,6 +128,8 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
                                                      const CaptureBasePtr& _capture,
                                                      FeatureBasePtrList& _features_out)
 {
+    WOLF_INFO("ProcessorTrackerGnss::detectNewFeatures");
+
     assert(_capture == last_ptr_);
 
     for (auto feat_pair : untracked_last_features_)
@@ -125,15 +138,22 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
             break;
 
         _features_out.push_back(feat_pair.second);
-        WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" );
+        WOLF_INFO("feature " , feat_pair.second->id() , " detected!" );
     }
-    WOLF_DEBUG(_features_out.size() , " features detected!");
+    WOLF_INFO(_features_out.size() , " features detected!");
 
     return _features_out.size();
 }
 
 void ProcessorTrackerGnss::establishFactors()
 {
+    WOLF_INFO("ProcessorTrackerGnss::establishFactors");
+
+    // initialize frame state with antenna position in map coordinates
+    // (since we don't have orientation for removing extrinsics)
+    if (params_tracker_gnss_->init_frames)
+        last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
+
     FactorBasePtrList new_factors;
 
     bool last_clock_bias_set = false;
@@ -145,7 +165,7 @@ void ProcessorTrackerGnss::establishFactors()
         if (ftr_sat == nullptr)
             continue;
 
-        // check valid measurement
+        // Check valid measurement
         if (ftr_sat->getMeasurement()(0) < 1e-12)
         {
             WOLF_INFO("Feature with not valid pseudorange");
@@ -156,25 +176,40 @@ void ProcessorTrackerGnss::establishFactors()
         if (!last_clock_bias_set)
         {
             if (last_ptr_->getStateBlock("T") == nullptr)
-                last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))), getProblem());
+            {
+                last_ptr_->addStateBlock("T",  std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem());
+                last_ptr_->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true),  getProblem());
+                last_ptr_->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), true),  getProblem());
+                last_ptr_->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true),  getProblem());
+            }
             else
-                last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
-
-            //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState().transpose());
+            {
+                last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
+                last_ptr_->getStateBlock("TG")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)));
+                last_ptr_->getStateBlock("TE")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
+                last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
+            }
 
+            //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
             last_clock_bias_set = true;
         }
-
-        // emplace a pseudo range factor
+        // unfix clock bias inter-constellation
+        if (ftr_sat->getSatellite().sys == SYS_GLO)
+            last_ptr_->getStateBlock("TG")->unfix();
+        if (ftr_sat->getSatellite().sys == SYS_GAL)
+            last_ptr_->getStateBlock("TE")->unfix();
+        if (ftr_sat->getSatellite().sys == SYS_CMP)
+            last_ptr_->getStateBlock("TC")->unfix();
+
+        // Emplace a pseudo range factor
         auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
                                                                   ftr_sat,
                                                                   sensor_gnss_,
                                                                   shared_from_this(),
                                                                   params_->apply_loss_function);
-
         new_factors.push_back(new_fac);
 
-        WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
+        WOLF_INFO("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
     }
 
     // TDCP FACTORS (tracked sats)
@@ -187,7 +222,7 @@ void ProcessorTrackerGnss::establishFactors()
 //            auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
 //            assert(ftr_k != nullptr);
 //
-//            WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
+//            WOLF_INFO("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
 //
 //            // emplace a tdcp factor from last to each KF
 //            for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
@@ -202,11 +237,11 @@ void ProcessorTrackerGnss::establishFactors()
 //
 //                if (ftr_k == ftr_r)
 //                {
-//                    WOLF_DEBUG("same KF and feature, skipping...");
+//                    WOLF_INFO("same KF and feature, skipping...");
 //                    break;
 //                }
 //
-//                WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
+//                WOLF_INFO("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
 //
 //                // Initialize (and create) clock bias stateblock in capture
 //                if (!last_clock_bias_set)
@@ -229,17 +264,21 @@ void ProcessorTrackerGnss::establishFactors()
 //                                                                   params_->apply_loss_function);
 //                new_factors.push_back(new_fac);
 //
-//                // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
+//                // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(),
 //                //             " origin: "       , feature_in_origin->id() ,
 //                //             " from last: "    , feature_in_last->id() );
 //            }
-//            WOLF_DEBUG("All TDCP factors emplaced!");
+//            WOLF_INFO("All TDCP factors emplaced!");
 //        }
 //    }
 
     // remove outliers
     if (!new_factors.empty() and params_tracker_gnss_->remove_outliers)
         removeOutliers(new_factors, last_ptr_);
+
+    //getProblem()->print(4, 1, 1, 1);
+
+    WOLF_INFO("ProcessorTrackerGnss::establishFactors done");
 }
 
 void ProcessorTrackerGnss::advanceDerived()
@@ -260,19 +299,23 @@ void ProcessorTrackerGnss::resetDerived()
 
 void ProcessorTrackerGnss::postProcess()
 {
-    if (last_ptr_ && last_ptr_->getFrame() && last_ptr_->getFrame()->isKey())
-        getProblem()->print(4, 1, 1, 1);
+
 }
 
 void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const
 {
+    WOLF_INFO("ProcessorTrackerGnss::removeOutliers");
+
     FactorBasePtrList remove_fac;
 
-    //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+    //WOLF_INFO( "PR ", getName()," rejectOutlier...");
     // copy states
     Eigen::Vector3d x(cap->getFrame()->getP()->getState());
     Eigen::Vector4d o(cap->getFrame()->getO()->getState());
-    Eigen::Vector1d clock_bias(-cap->getStateBlock("T")->getState());
+    Eigen::Vector1d clock_bias(cap->getStateBlock("T")->getState());
+    Eigen::Vector1d clock_bias_glo(cap->getStateBlock("TG")->getState());
+    Eigen::Vector1d clock_bias_gal(cap->getStateBlock("TE")->getState());
+    Eigen::Vector1d clock_bias_cmp(cap->getStateBlock("TC")->getState());
     Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState());
     Eigen::Vector3d t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
     Eigen::Vector1d roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
@@ -289,14 +332,34 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
     //std::cout << "yaw_ENU_map:   " << yaw_ENU_map << std::endl;
 
     // create double* array of a copy of the state for pseudo range factors
-    double* parameters_pr[8] = {x.data(),
-                                o.data(),
-                                clock_bias.data(),
-                                x_antena.data(),
-                                t_ENU_map.data(),
-                                roll_ENU_map.data(),
-                                pitch_ENU_map.data(),
-                                yaw_ENU_map.data()};
+    double* parameters_glo_pr[9] = {x.data(),
+                                    o.data(),
+                                    clock_bias.data(),
+                                    clock_bias_glo.data(),
+                                    x_antena.data(),
+                                    t_ENU_map.data(),
+                                    roll_ENU_map.data(),
+                                    pitch_ENU_map.data(),
+                                    yaw_ENU_map.data()};
+    double* parameters_gal_pr[9] = {x.data(),
+                                    o.data(),
+                                    clock_bias.data(),
+                                    clock_bias_gal.data(),
+                                    x_antena.data(),
+                                    t_ENU_map.data(),
+                                    roll_ENU_map.data(),
+                                    pitch_ENU_map.data(),
+                                    yaw_ENU_map.data()};
+    double* parameters_cmp_pr[9] = {x.data(),
+                                    o.data(),
+                                    clock_bias.data(),
+                                    clock_bias_cmp.data(),
+                                    x_antena.data(),
+                                    t_ENU_map.data(),
+                                    roll_ENU_map.data(),
+                                    pitch_ENU_map.data(),
+                                    yaw_ENU_map.data()};
+
     // create residuals
     double residual_pr;
 
@@ -307,7 +370,21 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
         if (fac_pr)
         {
             // evaluate the factor in this state
-            fac_pr->evaluate(parameters_pr, &residual_pr, nullptr);
+            switch (std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sys)
+            {
+                case SYS_GLO:
+                    fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr);
+                    break;
+                case SYS_GAL:
+                    fac_pr->evaluate(parameters_gal_pr, &residual_pr, nullptr);
+                    break;
+                case SYS_CMP:
+                    fac_pr->evaluate(parameters_cmp_pr, &residual_pr, nullptr);
+                    break;
+                default:
+                    fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr);
+                    break;
+            }
 
             //WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
 
@@ -325,6 +402,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
     // remove outliers
     for (auto fac : remove_fac)
         fac->remove();
+
+    WOLF_INFO("ProcessorTrackerGnss::removeOutliers done");
 }
 
 } // namespace wolf
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index b2d3df14a..b3a1ec3f3 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -5,15 +5,14 @@
  *      \author: jvallve
  */
 
-#include "gnss/factor/factor_gnss_fix_2d.h"
 #include <core/utils/utils_gtest.h>
 
 #include "core/problem/problem.h"
+#include "core/ceres_wrapper/ceres_manager.h"
 #include "gnss/sensor/sensor_gnss.h"
 #include "gnss/processor/processor_gnss_fix.h"
 #include "gnss/capture/capture_gnss_fix.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
+#include "gnss/factor/factor_gnss_fix_2d.h"
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index a5a47b37d..aaafe7765 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -1,11 +1,10 @@
-#include "gnss/factor/factor_gnss_pseudo_range.h"
 #include <core/utils/utils_gtest.h>
 
 #include "core/problem/problem.h"
+#include "core/ceres_wrapper/ceres_manager.h"
 #include "gnss/sensor/sensor_gnss.h"
 #include "gnss/capture/capture_gnss.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
+#include "gnss/factor/factor_gnss_pseudo_range.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -110,6 +109,9 @@ void setUpProblem()
     // capture
     cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
     cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb);
+    cap->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
+    cap->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
+    cap->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
 
     // features
     ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, prange1); // obsd_t data is not used in pseudo range factors
-- 
GitLab