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

include/gnss/factor/factor_gnss_pseudo_range.h

 #ifndef FACTOR_GNSS_PSEUDO_RANGE_H_
 #define FACTOR_GNSS_PSEUDO_RANGE_H_
-
+#include <type_traits>
 //Wolf includes
 #include "core/common/wolf.h"
 #include "core/factor/factor_autodiff.h"
@@ -13,25 +13,22 @@ 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_;
-        GnssUtils::Combination mode_;
-        double pseudo_range_;
-        double std_dev_;
-        Eigen::Vector3d satellite_position_;
+        Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
+        double sagnac_correction_;
+        bool not_GPS_;
 
     public:
 
-        FactorGnssPseudoRange(GnssUtils::Combination _mode,
-                              const double& _std_dev,
-                              FeatureGnssSatellitePtr& _ftr_ptr,
+        FactorGnssPseudoRange(FeatureGnssSatellitePtr& _ftr_ptr,
                               const SensorGnssPtr& _sensor_gnss_ptr,
                               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,
@@ -42,29 +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),
-            mode_(_mode),
-            std_dev_(_std_dev),
-            satellite_position_(_ftr_ptr->getSatellitePosition())
+            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_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
-
-            // Pseudo range combination
-            switch (mode_)
-            {
-                case GnssUtils::Combination::CODE_L1:
-                {
-                    pseudo_range_ = _ftr_ptr->getObservation().P[0];
-                    break;
-                }
-                default:
-                    throw std::runtime_error("not implemented");
-            }
+            //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;
@@ -77,7 +76,8 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
         template<typename T>
         bool operator ()(const T* const _x,
                          const T* const _o,
-                         const T* const _clock_drift,
+                         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,7 +90,8 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
 template<typename T>
 inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
                                                const T* const _o,
-                                               const T* const _clock_drift,
+                                               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,
@@ -102,19 +103,36 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
     Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
 
-    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
-    Eigen::Matrix<T,3,1> t_ECEF_ENU = R_ECEF_ENU * -sensor_gnss_ptr_->gettEnuEcef().cast<T>();
     Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
-    // Antenna position in ECEF coordinates
-    Eigen::Matrix<T,3,1> antena_ECEF = t_ECEF_ENU + R_ECEF_ENU * (t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena));
+    // 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);
 
     // Expected pseudo-range
-    T exp = (antena_ECEF-satellite_position_.cast<T>()).norm() + _clock_drift[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 << "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 - pseudo_range_) / std_dev_;
+    _residual[0] = (exp - getMeasurement()(0)) / getMeasurementSquareRootInformationUpper()(0,0);
 
     return true;
 }

include/gnss/factor/factor_gnss_tdcp.h

@@ -21,7 +21,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
         GnssUtils::Combination mode_;
         double d_pseudo_range_;
         double std_dev_;
-        Eigen::Vector3d satellite_position_k_, satellite_position_r_;
+        Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
 
     public:
 
@@ -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_position_k_(_ftr_k->getSatellitePosition()),
-            satellite_position_r_(_ftr_r->getSatellitePosition())
+            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.
@@ -126,17 +126,15 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     Eigen::Map<const Eigen::Quaternion<T> > q_map_base_k(_o_k);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
 
-    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
-    Eigen::Matrix<T,3,1> t_ECEF_ENU = R_ECEF_ENU * -sensor_gnss_ptr_->gettEnuEcef().cast<T>();
     Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
     // Antenna position in ECEF coordinates
-    Eigen::Matrix<T,3,1> antena_r_ECEF = t_ECEF_ENU + R_ECEF_ENU * (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena));
-    Eigen::Matrix<T,3,1> antena_k_ECEF = t_ECEF_ENU + R_ECEF_ENU * (t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena));
+    Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena);
+    Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena);
 
     // Expected tdcp
-    T exp = (antena_k_ECEF-satellite_position_k_.cast<T>()).norm() - (antena_r_ECEF-satellite_position_r_.cast<T>()).norm() + (_clock_drift_k[0] - _clock_drift_r[0]);
+    T exp = (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm() - (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm() + (_clock_drift_k[0] - _clock_drift_r[0]);
 
     // Residual
     _residual[0] = (exp - d_pseudo_range_) / std_dev_;

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,19 @@ 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, const GnssUtils::PseudoRange& _prange);
 
         virtual ~FeatureGnssSatellite(){};
 
         const obsd_t& getObservation() const;
         int satNumber() const;
-        const Eigen::Vector3d& getSatellitePosition() const;
+        const GnssUtils::Satellite& getSatellite() const;
+        const GnssUtils::PseudoRange& getPseudoRange() const;
 
     private:
       obsd_t obs_sat_;
-      Eigen::Vector3d sat_pos_;
+      GnssUtils::Satellite sat_;
+      GnssUtils::PseudoRange prange_;
 };
 
 
@@ -43,10 +46,11 @@ class FeatureGnssSatellite : public FeatureBase
 // IMPLEMENTATION
 namespace wolf {
 
-inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const Eigen::Vector3d& _sat_pos) :
-    FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_obs_sat.P[0]), Eigen::Matrix1d(1.0)),
+inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::PseudoRange& _prange) :
+    FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_prange.prange), Eigen::Matrix1d(_prange.var)),
     obs_sat_(_obs_sat),
-    sat_pos_(_sat_pos)
+    sat_(_sat),
+    prange_(_prange)
 {
     //
 }
@@ -61,9 +65,14 @@ 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_;
+}
+
+inline const GnssUtils::PseudoRange& FeatureGnssSatellite::getPseudoRange() const
+{
+    return prange_;
 }
 
 } // namespace wolf

include/gnss/processor/processor_tracker_gnss.h

@@ -17,13 +17,17 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     prcopt_t gnss_opt;
     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):
         ParamsProcessorTrackerFeature(_unique_name, _server)
     {
         enu_map_init_dist_min   = _server.getParam<double>  (prefix + _unique_name + "/enu_map_init_dist_min");
-        max_time_span           = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
+        remove_outliers         = _server.getParam<bool>    (prefix + _unique_name + "/remove_outliers");
+        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;
@@ -53,6 +57,9 @@ 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"
             + "nf: "                    + std::to_string(gnss_opt.nf)           + "\n"
@@ -87,6 +94,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
         ParamsProcessorTrackerGnssPtr params_tracker_gnss_;
         SensorGnssPtr sensor_gnss_;
         std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
+        GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
 
         /** \brief Track provided features in \b _capture
          * \param _features_in input list of features in \b last to track
@@ -173,18 +181,13 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
          *   - resetting and/or clearing variables and/or algorithms at the end of processing
          *   - drawing / printing / logging the results of the processing
          */
-        virtual void postProcess() override{};
+        virtual void postProcess() override;
 
         void advanceDerived() override;
         void resetDerived() override;
-};
 
-inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
-        ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
-        params_tracker_gnss_(_params_tracker_gnss)
-{
-    //
-}
+        void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
+};
 
 inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
 {

include/gnss/sensor/sensor_gnss.h

@@ -32,7 +32,8 @@ struct ParamsSensorGnss : public ParamsSensorBase
             yaw_fixed          = _server.getParam<bool>(prefix + _unique_name + "/ENU-MAP/yaw_fixed");
             translation_fixed  = _server.getParam<bool>(prefix + _unique_name + "/ENU-MAP/translation_fixed");
             set_ENU            = _server.getParam<bool>(prefix + _unique_name + "/set_ENU");
-            ENU_latlonalt      = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU_latlonalt");
+            if (set_ENU)
+                ENU_latlonalt      = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU_latlonalt");
         }
         std::string print() const
         {

src/sensor/sensor_gnss.cpp

 #include "gnss/sensor/sensor_gnss.h"
 #include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
 #include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/utils/transformations.h"
 
@@ -22,9 +22,9 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
     assert(_extrinsics.size() == 3 && "Bad extrinsics size");
 
     addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed));
-    addStateBlock("r", std::make_shared<StateBlock>(1, params_->roll_fixed));
-    addStateBlock("p", std::make_shared<StateBlock>(1, params_->pitch_fixed));
-    addStateBlock("y", std::make_shared<StateBlock>(1, params_->yaw_fixed));
+    addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed));
+    addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed));
+    addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed));
 
     if (params_->set_ENU)
         setEcefEnu(params_->ENU_latlonalt, false);

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;

test/gtest_factor_gnss_pseudo_range.cpp

-#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;
@@ -17,10 +16,16 @@ 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({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+
+GnssUtils::PseudoRange prange1, prange2, prange3, prange4;
+
+
 // WOLF
 ProblemPtr prb = Problem::create("PO", 3);
 CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
@@ -57,19 +62,23 @@ 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);
+    prange1.prange = (sat1.pos-t_ecef_antena).norm() + clock_drift(0);
+    prange2.prange = (sat2.pos-t_ecef_antena).norm() + clock_drift(0);
+    prange3.prange = (sat3.pos-t_ecef_antena).norm() + clock_drift(0);
+    prange4.prange = (sat4.pos-t_ecef_antena).norm() + clock_drift(0);
+    prange1.var = 1.0;
+    prange2.var = 1.0;
+    prange3.var = 1.0;
+    prange4.var = 1.0;
 }
 
 void setUpProblem()
@@ -100,26 +109,21 @@ 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
-    obsd_t obs1{0};
-    obs1.P[0] = prange_1;
-    ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, t_ecef_sat1);
-    obsd_t obs2{0};
-    obs2.P[0] = prange_2;
-    ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, t_ecef_sat2);
-    obsd_t obs3{0};
-    obs3.P[0] = prange_3;
-    ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, t_ecef_sat3);
-    obsd_t obs4{0};
-    obs4.P[0] = prange_4;
-    ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4);
+    ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, prange1); // obsd_t data is not used in pseudo range factors
+    ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat2, prange2); // obsd_t data is not used in pseudo range factors
+    ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat3, prange3); // obsd_t data is not used in pseudo range factors
+    ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat4, prange4); // obsd_t data is not used in pseudo range factors
 
     // factors
-    fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false);
-    fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, Combination::CODE_L1, 0.1, ftr2, gnss_sensor, nullptr, false);
-    fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, Combination::CODE_L1, 0.1, ftr3, gnss_sensor, nullptr, false);
-    fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, Combination::CODE_L1, 0.1, ftr4, gnss_sensor, nullptr, false);
+    fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false);
+    fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false);
+    fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false);
+    fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false);
 
     // ASSERTS
     // ENU-MAP

test/gtest_factor_gnss_tdcp.cpp

@@ -17,10 +17,19 @@ 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;
+//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({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat1_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::PseudoRange prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
+
 // WOLF
 ProblemPtr prb = Problem::create("PO", 3);
 CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
@@ -60,28 +69,45 @@ 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);
+
+    prange1_r.prange = (sat1_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange2_r.prange = (sat2_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange3_r.prange = (sat3_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange4_r.prange = (sat4_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+    prange1_k.prange = (sat1_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange2_k.prange = (sat2_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange3_k.prange = (sat3_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange4_k.prange = (sat4_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+    prange1_r.var = 1;
+    prange2_r.var = 1;
+    prange3_r.var = 1;
+    prange4_r.var = 1;
+    prange1_k.var = 1;
+    prange2_k.var = 1;
+    prange3_k.var = 1;
+    prange4_k.var = 1;
 }
 
 void setUpProblem()
@@ -127,31 +153,31 @@ 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);
+    obs1_r.L[0] = prange1_r.prange;
+    ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, sat1_r, prange1_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);
+    obs2_r.L[0] = prange2_r.prange;
+    ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, sat2_r, prange2_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);
+    obs3_r.L[0] = prange3_r.prange;
+    ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, sat3_r, prange3_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);
+    obs4_r.L[0] = prange4_r.prange;
+    ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, sat4_r, prange4_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);
+    obs1_k.L[0] = prange1_k.prange;
+    ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, sat1_k, prange1_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);
+    obs2_k.L[0] = prange2_k.prange;
+    ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, sat2_k, prange2_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);
+    obs3_k.L[0] = prange3_k.prange;
+    ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, sat3_k, prange3_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);
+    obs4_k.L[0] = prange4_k.prange;
+    ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, sat4_k, prange4_k);
 
     // factors
     fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);