Merge branch 'include-clock-bias-factors' into 'devel'

Include clock bias factors

See merge request !41

CMakeLists.txt

@@ -105,6 +105,7 @@ SET(HDRS_CAPTURE
 SET(HDRS_FACTOR
   include/${PROJECT_NAME}/factor/factor_gnss_fix_2d.h
   include/${PROJECT_NAME}/factor/factor_gnss_fix_3d.h
+  include/${PROJECT_NAME}/factor/factor_gnss_fix_3d_with_clock.h
   include/${PROJECT_NAME}/factor/factor_gnss_pseudo_range.h
   include/${PROJECT_NAME}/factor/factor_gnss_tdcp.h
   include/${PROJECT_NAME}/factor/factor_gnss_tdcp_2d.h

include/gnss/capture/capture_gnss_fix.h

@@ -42,10 +42,10 @@ class CaptureGnssFix : public CaptureBase
         TimeStamp ts_GPST_; ///< Time stamp in GPS time
 
     public:
-        CaptureGnssFix(const TimeStamp& _ts, 
-                       SensorBasePtr _sensor_ptr, 
-                       const Eigen::Vector4d& _fix,
-                       const Eigen::Matrix4d& _covariance,
+        CaptureGnssFix(const TimeStamp&, 
+                       SensorBasePtr, 
+                       const Eigen::Vector4d&,
+                       const Eigen::Matrix4d&,
                        bool _ecef_coordinates = true);
         ~CaptureGnssFix() override;
 
@@ -55,9 +55,14 @@ class CaptureGnssFix : public CaptureBase
         double getClockBiasVariance() const;
         Eigen::Vector3d getPositionEcef() const;
         Eigen::Matrix3d getPositionCovarianceEcef() const;
-        void getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const;
+        void getPositionAndCovarianceEcef(Eigen::Vector3d&, Eigen::Matrix3d&) const;
         TimeStamp getGpsTimeStamp() const;
-        void setGpsTimeStamp(const TimeStamp& _ts_GPST);
+
+        void setFixEcef(const Eigen::Vector4d&);
+        void setFixCovarianceEcef(const Eigen::Matrix4d&);
+        void setClockBias(const double&);
+        void setPositionEcef(const Eigen::Vector3d&);
+        void setGpsTimeStamp(const TimeStamp&);
 };
 
 inline Eigen::Vector4d CaptureGnssFix::getFixEcef() const
@@ -101,6 +106,26 @@ inline wolf::TimeStamp CaptureGnssFix::getGpsTimeStamp() const
     return ts_GPST_;
 }
 
+inline void CaptureGnssFix::setFixEcef(const Eigen::Vector4d& _fix_ECEF)
+{
+    fix_ECEF_ = _fix_ECEF;
+}
+
+inline void CaptureGnssFix::setFixCovarianceEcef(const Eigen::Matrix4d& _covariance_ECEF)
+{
+    covariance_ECEF_ = _covariance_ECEF;
+}
+
+inline void CaptureGnssFix::setClockBias(const double& _clock_bias)
+{
+    fix_ECEF_(3) = _clock_bias;
+}
+
+inline void CaptureGnssFix::setPositionEcef(const Eigen::Vector3d& _fix_position)
+{
+    fix_ECEF_.head<3>() = _fix_position;
+}
+        
 inline void CaptureGnssFix::setGpsTimeStamp(const TimeStamp& _ts_GPST)
 {
     ts_GPST_ = _ts_GPST;

include/gnss/factor/factor_gnss_fix_2d.h

@@ -89,34 +89,34 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
 {
     Eigen::Map<const Eigen::Matrix<T,2,1> > t_map_base(_x);
     Eigen::Matrix<T,2,2> R_map_base = Eigen::Rotation2D<T>(_o[0]).matrix();
-    Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+    Eigen::Map<const Eigen::Matrix<T,2,1> > t_base_antena(_x_antena);
     Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+    Eigen::Quaternion<T> q_ENU_ECEF = sensor_gnss_ptr_->getQEnuEcef().cast<T>();
     Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
     Eigen::Matrix<T,2,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose().topRows(2);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
     //std::cout << "computeREnuMap():\n" << sensor_gnss_ptr_->computeREnuMap((const double) _roll[0], (const double) _pitch[0], (const double) _yaw[0]).transpose().topRows(2) << std::endl;
-    //std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl;
+    //std::cout << "getQEnuEcef():\n" << sensor_gnss_ptr_->getQEnuEcef() << std::endl;
     //std::cout << "gettEnuEcef(): " << sensor_gnss_ptr_->gettEnuEcef().transpose() << std::endl;
 
     // Transform ECEF 3d Fix Feature to 2d Fix in Map coordinates (removing z)
     //std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl;
-    //std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl;
+    //std::cout << "q_ENU_ECEF:\t" << q_ENU_ECEF.x() << "\t" << q_ENU_ECEF.y() << "\t" << q_ENU_ECEF.z() << "\t" << q_ENU_ECEF.w() << std::endl;
     //std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl;
-    Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map);
+    Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (q_ENU_ECEF * getMeasurement().head<3>().cast<T>() + t_ENU_ECEF - t_ENU_map);
     //std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
     //std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl;
 
     // Antena position in Map coordinates
-    Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena.head(2) + t_map_base;
+    Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena + t_map_base;
     //std::cout << "antena_map:\n\t" << antena_map[0] << "\n\t" << antena_map[1] << std::endl;
 
     // Compute residual rotating information matrix to 2d Map coordinates
     // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
-    // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+    // In this case R = R_map_ENU * q_ENU_ECEF, then R' = q_ENU_ECEF' * R_map_ENU'
+    residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * q_ENU_ECEF.conjugate() * R_map_ENU.transpose()) * (antena_map - fix_map);
     //std::cout << "residuals_ECEF:\n\t" << _residuals[0] << "\n\t" << _residuals[1] << "\n\t" << _residuals[2]<< std::endl;
 
     return true;

include/gnss/factor/factor_gnss_fix_3d.h

@@ -89,24 +89,18 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_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::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
+    Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+    Eigen::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().cast<T>().conjugate();
     Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
-    Eigen::Matrix<T,3,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose();
+    Eigen::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_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);
 
-    // Transform ECEF 3d Fix Feature to Map coordinates
-    Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map);
-
-    // Antena position in Map coordinates
-    Eigen::Matrix<T,3,1> antena_map = q_map_base * t_base_antena + t_map_base;
-
-    // Compute residual rotating information matrix in Map coordinates
-    // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
-    // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+    // Expected antena position in ECEF coordinates
+    Eigen::Matrix<T,3,1> fix_expected = q_ECEF_ENU * (q_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF);
 
+    // Compute residual 
+    residuals = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * (fix_expected - getMeasurement().head<3>());
     return true;
 }
 

include/gnss/factor/factor_gnss_fix_3d_with_clock.h

@@ -97,18 +97,18 @@ inline bool FactorGnssFix3dWithClock::operator ()(const T* const _x,
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
     Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().cast<T>().transpose();
+    Eigen::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().cast<T>().conjugate();
     Eigen::Matrix<T,3,1> t_ENU_ECEF = 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::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_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);
 
     // Expected antena position in ECEF coordinates
     Eigen::Matrix<T,4,1> fix_expected;
-    fix_expected.head<3>() = R_ECEF_ENU * (R_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF);
-    fix_expected(3) = _clock_bias;
+    fix_expected << q_ECEF_ENU * (q_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF),
+                    _clock_bias[0];
 
     // Compute residual 
-    residuals_ECEF = getMeasurementSquareRootInformationUpper() * (antena_ecef - getMeasurement());
+    residuals = getMeasurementSquareRootInformationUpper() * (fix_expected - getMeasurement());
 
     return true;
 }

include/gnss/factor/factor_gnss_pseudo_range.h

@@ -38,9 +38,10 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
 {
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
-        Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
+        mutable Eigen::Vector3d satellite_ENU_;
         mutable double sagnac_correction_;
-        mutable bool sagnac_set_;
+        mutable bool apply_sagnac_correction_;
+        mutable bool sats_ENU_computed_;
         bool not_GPS_;
 
     public:
@@ -48,6 +49,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
         FactorGnssPseudoRange(FeatureGnssSatellitePtr& _ftr_ptr,
                               const SensorGnssPtr& _sensor_gnss_ptr,
                               const ProcessorBasePtr& _processor_ptr,
+                              bool _apply_sagnac_correction,
                               bool _apply_loss_function,
                               FactorStatus _status = FAC_ACTIVE) :
             FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
@@ -74,16 +76,27 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
                                                                                 _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_ECEF_(_ftr_ptr->getSatellite().pos),
+            //satellite_ENU_(sensor_gnss_ptr_->getQEnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
+            satellite_ENU_(_ftr_ptr->getSatellite().pos),
             sagnac_correction_(0),
-            sagnac_set_(false),
+            apply_sagnac_correction_(_apply_sagnac_correction),
+            sats_ENU_computed_(false),
             not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS)
         {
+            computeSatsEnu();
             //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");
         }
 
+        void computeSatsEnu() const
+        {
+            if (not sensor_gnss_ptr_->isEnuDefined())
+                return;
+
+            satellite_ENU_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_ + sensor_gnss_ptr_->gettEnuEcef();
+            sats_ENU_computed_ = true;
+        }
+
         ~FactorGnssPseudoRange() override = default;
 
         template<typename T>
@@ -115,41 +128,37 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
                                                const T* const _yaw_ENU_map,
                                                T* _residual) const
 {
+    if (not sats_ENU_computed_)
+        computeSatsEnu();
+
+    if (not sats_ENU_computed_)
+    {
+        WOLF_WARN("Evaluating a FactorGnssPseudoRange without initializing ENU! returning zero residual.");
+        _residual[0] = T(0);
+        return true;
+    }
+
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_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::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
-    /* NOT EFFICIENT IMPLE;ENTATION
-
-    Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
-
-    // 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);
-    */
-
     /* SAGNAC EFFECT CORRECTION
     * It should be recomputed with the new 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 the first value of antenna position
     */
-    if (!sagnac_set_)
+    if (apply_sagnac_correction_)
     {
-        Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
+        Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
         initSagnac(antena_ENU);
     }
 
     // Expected pseudo-range
-    T exp = (t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena) - satellite_ENU_.cast<T>()).norm() +      // norm
+    T exp = (t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena) - 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
-    /*
-    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;
@@ -180,9 +189,10 @@ void FactorGnssPseudoRange::initSagnac(const Eigen::Matrix<T,3,1>& antena_ENU) c
 template<>
 void FactorGnssPseudoRange::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_ENU) const
 {
-    Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
-    sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, satellite_ECEF_);
-    sagnac_set_ = true;
+    Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d sat_ECEF    = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_ - sensor_gnss_ptr_->gettEnuEcef());
+    sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, sat_ECEF);
+    apply_sagnac_correction_ = false;
 }
 
 } // namespace wolf

include/gnss/factor/factor_gnss_tdcp.h

@@ -39,9 +39,10 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
         mutable double d_pseudo_range_;
-        mutable bool sagnac_set_;
+        mutable bool apply_sagnac_correction_;
+        mutable bool sats_ENU_computed_;
         double std_dev_;
-        Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
+        mutable Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
 
     public:
 
@@ -50,6 +51,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
                        FeatureGnssSatellitePtr& _ftr_k,
                        const SensorGnssPtr& _sensor_gnss_ptr,
                        const ProcessorBasePtr& _processor_ptr,
+                       bool _apply_sagnac_correction,
                        bool _apply_loss_function,
                        FactorStatus _status = FAC_ACTIVE) :
             FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp",
@@ -74,20 +76,31 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
                                                                                _sensor_gnss_ptr->getEnuMapPitch(),
                                                                                _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr),
-            sagnac_set_(false),
+            apply_sagnac_correction_(_apply_sagnac_correction),
+            sats_ENU_computed_(false),
             std_dev_(_std_dev),
-            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())
+            satellite_ENU_k_(_ftr_k->getSatellite().pos),
+            satellite_ENU_r_(_ftr_r->getSatellite().pos)
         {
             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 FactorGnssTdcp without initializing ENU");
+            computeSatsEnu();
 
             d_pseudo_range_ = _ftr_k->getRange().L_corrected - _ftr_r->getRange().L_corrected;
         }
 
+        void computeSatsEnu() const
+        {
+            if (not sensor_gnss_ptr_->isEnuDefined())
+                return;
+
+            satellite_ENU_k_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_k_ + sensor_gnss_ptr_->gettEnuEcef();
+            satellite_ENU_r_ = sensor_gnss_ptr_->getQEnuEcef() * satellite_ENU_r_ + sensor_gnss_ptr_->gettEnuEcef();
+            sats_ENU_computed_ = true;
+        }
+
         ~FactorGnssTdcp() override = default;
 
         template<typename T>
@@ -106,7 +119,6 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
 
         template<typename T>
         void initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const;
-
 };
 
 template<typename T>
@@ -123,6 +135,16 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
                                         const T* const _yaw_ENU_map,
                                         T* _residual) const
 {
+    if (not sats_ENU_computed_)
+        computeSatsEnu();
+
+    if (not sats_ENU_computed_)
+    {
+        WOLF_WARN("Evaluating a FactorGnssTdcp without initializing ENU! returning zero residual.");
+        _residual[0] = T(0);
+        return true;
+    }
+
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_r(_x_r);
     Eigen::Map<const Eigen::Quaternion<T> > q_map_base_r(_o_r);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_k(_x_k);
@@ -130,7 +152,7 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
-    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::Quaternion<T> q_ENU_map = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
 
     /* INEFFICIENT IMPLEMENTATION
     // Antenna position in ECEF coordinates
@@ -170,17 +192,17 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
     * We use instead a precomputed sagnac effect taking the first values of antenna position
     */
-    if (not sagnac_set_)
+    if (apply_sagnac_correction_)
     {
-        Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
-        Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
+        Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
+        Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
         initSagnac(antena_r_ENU, antena_k_ENU);
     }
 
     // Residual
-    _residual[0] = ((t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
+    _residual[0] = ((t_ENU_map + q_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
                       + _clock_bias_k[0]
-                      - (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
+                      - (t_ENU_map + q_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
                       - _clock_bias_r[0]
                       - d_pseudo_range_) / std_dev_;
 
@@ -195,17 +217,17 @@ void FactorGnssTdcp::initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const
 template<>
 void FactorGnssTdcp::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_r_ENU, const Eigen::Matrix<double,3,1>& antena_k_ENU) const
 {
-    Eigen::Vector3d antena_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
-    Eigen::Vector3d sat_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d antena_r_ECEF   = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d sat_r_ECEF      = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
     double sagnac_corr_r = GnssUtils::computeSagnacCorrection(antena_r_ECEF, sat_r_ECEF);
 
-    Eigen::Vector3d antena_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
-    Eigen::Vector3d sat_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d antena_k_ECEF   = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d sat_k_ECEF      = sensor_gnss_ptr_->getQEnuEcef().conjugate() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
     double sagnac_corr_k = GnssUtils::computeSagnacCorrection(antena_k_ECEF, sat_k_ECEF);
 
     d_pseudo_range_ += -sagnac_corr_k + sagnac_corr_r;
 
-    sagnac_set_ = true;
+    apply_sagnac_correction_ = false;
 }
 
 } // namespace wolf

include/gnss/factor/factor_gnss_tdcp_2d.h

@@ -41,24 +41,24 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
     public:
 
         FactorGnssTdcp2d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
-            FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d",
-                                                                        TOP_GEOM,
-                                                                        _ftr_ptr,
-                                                                        _frame_other_ptr,
-                                                                        nullptr,
-                                                                        nullptr,
-                                                                        nullptr,
-                                                                        _processor_ptr,
-                                                                        _apply_loss_function,
-                                                                        _status,
-                                                                        _frame_other_ptr->getP(),
-                                                                        _frame_other_ptr->getO(),
-                                                                        _ftr_ptr->getFrame()->getP(),
-                                                                        _ftr_ptr->getFrame()->getO(),
-                                                                        _sensor_gnss_ptr->getP(),
-                                                                        _sensor_gnss_ptr->getEnuMapRoll(),
-                                                                        _sensor_gnss_ptr->getEnuMapPitch(),
-                                                                        _sensor_gnss_ptr->getEnuMapYaw()),
+            FactorAutodiff("FactorGnssTdcp2d",
+                           TOP_GEOM,
+                           _ftr_ptr,
+                           _frame_other_ptr,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           _processor_ptr,
+                           _apply_loss_function,
+                           _status,
+                           _frame_other_ptr->getP(),
+                           _frame_other_ptr->getO(),
+                           _ftr_ptr->getFrame()->getP(),
+                           _ftr_ptr->getFrame()->getO(),
+                           _sensor_gnss_ptr->getP(),
+                           _sensor_gnss_ptr->getEnuMapRoll(),
+                           _sensor_gnss_ptr->getEnuMapPitch(),
+                           _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
@@ -81,35 +81,35 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
 
 template<typename T>
 inline bool FactorGnssTdcp2d::operator ()(const T* const _x1,
-                                                const T* const _o1,
-                                                const T* const _x2,
-                                                const T* const _o2,
-                                                const T* const _x_antena,
-                                                const T* const _roll_ENU_MAP,
-                                                const T* const _pitch_ENU_MAP,
-                                                const T* const _yaw_ENU_MAP,
-                                                T* _residuals) const
+                                          const T* const _o1,
+                                          const T* const _x2,
+                                          const T* const _o2,
+                                          const T* const _x_antena,
+                                          const T* const _roll_ENU_MAP,
+                                          const T* const _pitch_ENU_MAP,
+                                          const T* const _yaw_ENU_MAP,
+                                          T* _residuals) const
 {
     Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE1(_x1);
-    Eigen::Matrix<T,2,2> R_MAP_BASE1 = Eigen::Rotation2D<T>(_o1[0]).matrix();
+    Eigen::Rotation2D<T> R_MAP_BASE1(_o1[0]);
     Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE2(_x2);
-    Eigen::Matrix<T,2,2> R_MAP_BASE2 = Eigen::Rotation2D<T>(_o2[0]).matrix();
-    Eigen::Map<const Eigen::Matrix<T,3,1> > t_BASE_ANTENA(_x_antena);
+    Eigen::Rotation2D<T> R_MAP_BASE2(_o2[0]);
+    Eigen::Map<const Eigen::Matrix<T,2,1> > t_BASE_ANTENA(_x_antena);
     Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+    Eigen::Quaternion<T> q_ENU_ECEF = sensor_gnss_ptr_->getQEnuEcef().cast<T>();
     Eigen::Matrix<T,2,3> R_MAP_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]).transpose().topRows(2);
 
     // Transform ECEF 3D SingleDiff Feature to 2D SingleDiff in Map coordinates (removing z)
-    Eigen::Matrix<T,2,1> measured_diff_2D_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
+    Eigen::Matrix<T,2,1> measured_diff_2D_MAP = R_MAP_ENU * (q_ENU_ECEF * getMeasurement().cast<T>());
 
     // Substraction of expected antena positions in Map coordinates
-    Eigen::Matrix<T,2,1> expected_diff_2D_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
+    Eigen::Matrix<T,2,1> expected_diff_2D_MAP = (R_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1);
 
     // Compute residual rotating information matrix to 2D Map coordinates
     // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
-    // In this case R = R_2DMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2DMAP_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2D_MAP - measured_diff_2D_MAP);
+    // In this case R = R_2DMAP_ENU * q_ENU_ECEF, then R' = q_ENU_ECEF' * R_2DMAP_ENU'
+    residuals_ECEF = (getMeasurementSquareRootInformationUpper() * q_ENU_ECEF.conjugate() * R_MAP_ENU.transpose()) * (expected_diff_2D_MAP - measured_diff_2D_MAP);
 
     //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
     //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;

include/gnss/factor/factor_gnss_tdcp_3d.h

@@ -103,14 +103,14 @@ inline bool FactorGnssTdcp3d::operator ()(const T* const _x1,
     Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
     Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().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::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().conjugate().cast<T>();
+    Eigen::Quaternion<T> q_ENU_MAP = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
 
     // Expected displacement
-    Eigen::Matrix<T,3,1> expected_ECEF = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+    Eigen::Matrix<T,3,1> expected_ECEF = q_ECEF_ENU * q_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
 
     // Compute residual
-    residuals_ECEF = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+    residuals_ECEF = getMeasurementSquareRootInformationUpper() * (expected_ECEF - getMeasurement());
 
     //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
     //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;

include/gnss/factor/factor_gnss_tdcp_batch.h

@@ -109,20 +109,20 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
     Eigen::Map<Eigen::Matrix<T,3,1> > disp_residuals(_residuals);
     Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
 
-    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().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::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().conjugate().cast<T>();
+    Eigen::Quaternion<T> q_ENU_MAP = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
 
     // Expected d
     Eigen::Matrix<T,4,1> exp;
 
     // Expected displacement in ECEF
-    exp.head(3) = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+    exp.head(3) = q_ECEF_ENU * q_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
 
     // clock error
     exp(3) = *_t2 - *_t1;
 
     // Compute residual
-    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (exp - getMeasurement().cast<T>());
+    residuals = getMeasurementSquareRootInformationUpper() * (exp - getMeasurement());
 
     //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
     //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;

include/gnss/processor/processor_tracker_gnss.h

@@ -40,13 +40,10 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     GnssUtils::Options gnss_opt;
     GnssUtils::Options fix_opt{GnssUtils::default_options};
     GnssUtils::TdcpOptions tdcp_params;
-    double max_time_span;
     bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
-    double outlier_residual_th;
-    bool init_frames, pseudo_ranges, fix;
-    double enu_map_fix_dist;
+    double max_time_span, outlier_residual_th, enu_map_fix_dist;
+    bool init_frames, pseudo_ranges, fix, tdcp_enabled, apply_sagnac_correction;
     int min_sbas_sats;
-    bool tdcp_enabled;
     std::string tdcp_structure;
 
     ParamsProcessorTrackerGnss() = default;
@@ -62,6 +59,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
         fix                         = _server.getParam<bool>    (prefix + _unique_name + "/fix");
         pseudo_ranges               = _server.getParam<bool>    (prefix + _unique_name + "/pseudo_ranges");
         min_sbas_sats               = _server.getParam<int>     (prefix + _unique_name + "/gnss/min_sbas_sats");
+        apply_sagnac_correction     = _server.getParam<bool>    (prefix + _unique_name + "/gnss/apply_sagnac_correction");
 
         // GNSS OPTIONS (see rtklib.h)
         gnss_opt.sateph     =        _server.getParam<int>   (prefix + _unique_name + "/gnss/sateph");  // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
@@ -85,17 +83,17 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
         tdcp_enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
         if (tdcp_enabled)
         {
-            tdcp_structure                      = _server.getParam<std::string>(prefix + _unique_name + "/gnss/tdcp/structure");
-            remove_outliers_tdcp                = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
-            tdcp_params.batch                   = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/batch");
-            gnss_opt.carrier_opt.corr_iono      = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/corr_iono");
-            gnss_opt.carrier_opt.corr_tropo     = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
-            gnss_opt.carrier_opt.corr_clock     = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/corr_clock");
-            tdcp_params.loss_function           = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/loss_function");
-            tdcp_params.time_window             = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
-            tdcp_params.sigma_atm               = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
-            tdcp_params.sigma_carrier           = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
-            tdcp_params.multi_freq              = _server.getParam<bool>  (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+            tdcp_structure                      = _server.getParam<std::string> (prefix + _unique_name + "/gnss/tdcp/structure");
+            remove_outliers_tdcp                = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+            tdcp_params.batch                   = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/batch");
+            gnss_opt.carrier_opt.corr_iono      = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/corr_iono");
+            gnss_opt.carrier_opt.corr_tropo     = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
+            gnss_opt.carrier_opt.corr_clock     = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/corr_clock");
+            tdcp_params.loss_function           = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/loss_function");
+            tdcp_params.time_window             = _server.getParam<double>      (prefix + _unique_name + "/gnss/tdcp/time_window");
+            tdcp_params.sigma_atm               = _server.getParam<double>      (prefix + _unique_name + "/gnss/tdcp/sigma_atm");
+            tdcp_params.sigma_carrier           = _server.getParam<double>      (prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
+            tdcp_params.multi_freq              = _server.getParam<bool>        (prefix + _unique_name + "/gnss/tdcp/multi_freq");
             if (tdcp_params.batch)
             {
                 tdcp_params.min_common_sats         = _server.getParam<int>       (prefix + _unique_name + "/gnss/tdcp/min_common_sats");

include/gnss/sensor/sensor_gnss.h

@@ -103,7 +103,7 @@ class SensorGnss : public SensorBase
     protected:
         ParamsSensorGnssPtr params_;
         bool ENU_defined_, t_ENU_MAP_initialized_, R_ENU_MAP_initialized_;
-        Eigen::Matrix3d R_ENU_ECEF_;
+        Eigen::Quaterniond q_ENU_ECEF_;
         Eigen::Vector3d t_ENU_ECEF_;
 
     public:
@@ -122,9 +122,9 @@ class SensorGnss : public SensorBase
         StateBlockPtr getEnuMapPitch();
         StateBlockConstPtr getEnuMapYaw() const;
         StateBlockPtr getEnuMapYaw();
-        const Eigen::Matrix3d& getREnuEcef() const;
-        const Eigen::Vector3d& gettEnuEcef() const;
-        Eigen::Matrix3d getREnuMap() const;
+        Eigen::Quaterniond getQEnuEcef() const;
+        Eigen::Vector3d gettEnuEcef() const;
+        Eigen::Quaterniond getQEnuMap() const;
         Eigen::Vector3d gettEnuMap() const;
         bool isEnuDefined() const;
         bool isEnuMapInitialized() const;
@@ -145,16 +145,24 @@ class SensorGnss : public SensorBase
         void setEnuMapTranslationInitialized(const bool& _init);
         void setEnuMapRotationInitialized(const bool& _init);
         void fixEnuMap();
+        void resetEnu();
 
         // compute
         template<typename T>
         Eigen::Matrix<T,3,3> computeREnuMap(const T& _r,const T& _p,const T& _y) const;
+        template<typename T>
+        Eigen::Quaternion<T> computeQEnuMap(const T& _r,const T& _p,const T& _y) const;
         void initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
                               const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2);
         void initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2,
                                  const Eigen::Vector3d& _v_ECEF_antena1_antena2);
         Eigen::Vector3d computeFrameAntennaPosEcef(const FrameBasePtr&) const;
 };
+}
+
+#include <core/math/rotations.h>
+
+namespace wolf {
 
 inline bool SensorGnss::isEnuDefined() const
 {
@@ -226,12 +234,12 @@ inline StateBlockPtr SensorGnss::getEnuMapYaw()
     return getStateBlock('y');
 }
 
-inline const Eigen::Matrix3d& SensorGnss::getREnuEcef() const
+inline Eigen::Quaterniond SensorGnss::getQEnuEcef() const
 {
-    return R_ENU_ECEF_;
+    return q_ENU_ECEF_;
 }
 
-inline const Eigen::Vector3d& SensorGnss::gettEnuEcef() const
+inline Eigen::Vector3d SensorGnss::gettEnuEcef() const
 {
     return t_ENU_ECEF_;
 }
@@ -244,14 +252,18 @@ inline Eigen::Vector3d SensorGnss::gettEnuMap() const
 template<typename T>
 inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
 {
-    return Eigen::Matrix<T,3,3>(Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX()) *
-                                Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY()) *
-                                Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ()));
+    return e2R((Eigen::Matrix<T,3,1>() << _r, _p, _y).finished());
+}
+
+template<typename T>
+inline Eigen::Quaternion<T> SensorGnss::computeQEnuMap(const T& _r,const T& _p,const T& _y) const
+{
+    return e2q((Eigen::Matrix<T,3,1>() << _r, _p, _y).finished());
 }
 
-inline Eigen::Matrix3d SensorGnss::getREnuMap() const
+inline Eigen::Quaterniond SensorGnss::getQEnuMap() const
 {
-    return computeREnuMap(getEnuMapRoll() ->getState()(0),
+    return computeQEnuMap(getEnuMapRoll() ->getState()(0),
                           getEnuMapPitch()->getState()(0),
                           getEnuMapYaw()  ->getState()(0));
 }
@@ -288,6 +300,11 @@ inline void SensorGnss::fixEnuMap()
     getEnuMapYaw()->fix();
 }
 
+inline void SensorGnss::resetEnu()
+{
+    ENU_defined_ = false;
+}
+
 } // namespace wolf
 
 #endif //SENSOR_GPS_H_

src/processor/processor_gnss_tdcp.cpp

@@ -239,7 +239,7 @@ bool ProcessorGnssTdcp::voteForKeyFrame() const
 //    }
 //
 //    // Distance criterion: From the last KF with gnssfix capture
-//    Eigen::Vector2d v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+//    Eigen::Vector2d v_origin_current = (sensor_gnss_->getQEnuMap().conjugate() * sensor_gnss_->getQEnuEcef() * incoming_capture_->getData()).head<2>();
 //    Eigen::Vector2d v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
 //	//std::cout << "params_tdcp_->dist_traveled" << params_tdcp_->dist_traveled << std::endl;
 //	//std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;

src/processor/processor_tracker_gnss.cpp

@@ -339,7 +339,7 @@ void 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 and fix_last_.success)
-        last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
+        last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
 
     FactorBasePtrList new_factors;
 
@@ -395,6 +395,7 @@ void ProcessorTrackerGnss::establishFactors()
                                                                       ftr_sat,
                                                                       sensor_gnss_,
                                                                       shared_from_this(),
+                                                                      params_tracker_gnss_->apply_sagnac_correction,
                                                                       params_->apply_loss_function);
             new_factors.push_back(new_fac);
 
@@ -578,6 +579,7 @@ void ProcessorTrackerGnss::establishFactors()
                                                                        ftr_k,
                                                                        sensor_gnss_,
                                                                        shared_from_this(),
+                                                                       params_tracker_gnss_->apply_sagnac_correction,
                                                                        params_tracker_gnss_->tdcp_params.loss_function,
                                                                        factor_status);
                     new_factors.push_back(new_fac);
@@ -653,7 +655,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
     if (cap == last_ptr_ and fix_last_.stat != 0 and params_tracker_gnss_->remove_outliers_with_fix)
     {
         WOLF_DEBUG("OUTLIERS COMPUTED USING fix_last");
-        x = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
+        x = sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
         o << 0,0,0,1;
         clock_bias <<  CLIGHT * fix_last_.rcv_bias(0);
         clock_bias_glo << CLIGHT * fix_last_.rcv_bias(1);

src/sensor/sensor_gnss.cpp

@@ -39,7 +39,7 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
         ENU_defined_(false),
         t_ENU_MAP_initialized_(false),
         R_ENU_MAP_initialized_(false),
-        R_ENU_ECEF_(Eigen::Matrix3d::Identity()),
+        q_ENU_ECEF_(Eigen::Quaterniond::Identity()),
         t_ENU_ECEF_(Eigen::Vector3d::Zero())
 {
     assert(_extrinsics.size() == 3 && "Bad extrinsics size");
@@ -96,13 +96,15 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates)
     WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
     assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
 
+    Eigen::Matrix3d R_ENU_ECEF;
     if (_ECEF_coordinates)
-        GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
+        GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF, t_ENU_ECEF_);
     else
         GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU, not params_->latlon_in_degrees), 
-                                          R_ENU_ECEF_, 
+                                          R_ENU_ECEF, 
                                           t_ENU_ECEF_);
 
+    q_ENU_ECEF_ = R2q(R_ENU_ECEF);
     ENU_defined_ = true;
 
     WOLF_INFO("SensorGnss: ECEF-ENU set.")
@@ -113,7 +115,7 @@ void SensorGnss::setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vec
     WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
     assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
 
-    R_ENU_ECEF_ = _R_ENU_ECEF;
+    q_ENU_ECEF_ = R2q(_R_ENU_ECEF);
     t_ENU_ECEF_ = _t_ENU_ECEF;
     ENU_defined_ = true;
 }
@@ -124,7 +126,7 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
     assert(ENU_defined_ && "initializing ENU-MAP before setting ENU");
 
     // compute fix vector (from 1 to 2) in ENU coordinates
-    Eigen::Vector3d v_ENU = R_ENU_ECEF_ * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
+    Eigen::Vector3d v_ENU = q_ENU_ECEF_ * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
 
     // 2d
     if (getProblem()->getDim() == 2)
@@ -136,28 +138,23 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
         Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
         Eigen::Vector2d v_MAP =  t_MAP_antena2 - t_MAP_antenaENU;
 
-        // ENU-MAP Rotation
-        Eigen::Matrix2d R_ENU_MAP;
-        // get it if already initialized
-        if (R_ENU_MAP_initialized_)
-        	R_ENU_MAP = getREnuMap().topLeftCorner<2,2>();
+        // ENU-MAP Rotation (considering yaw only)
 		// initialize yaw if not initialized
-        else
+        if (not R_ENU_MAP_initialized_)
         {
 			double theta_ENU = atan2(v_ENU(1),v_ENU(0));
 			double theta_MAP = atan2(v_MAP(1),v_MAP(0));
 			double yaw_ENU_MAP = theta_ENU-theta_MAP;
 			setEnuMapYawState(yaw_ENU_MAP);
 
-			R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
-									   getEnuMapPitch()->getState()(0),
-									   getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
 
 	        WOLF_INFO("SensorGnss: ENU-MAP rotation initialized.");
         }
+        auto R_ENU_MAP = Eigen::Rotation2Dd(getEnuMapYaw()->getState()(0));
+
 		// ENU-MAP translation
 		Eigen::Vector3d t_ENU_MAP(Eigen::Vector3d::Zero());
-        t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
+        t_ENU_MAP.head<2>() = -(R_ENU_MAP * t_MAP_antenaENU);
         setEnuMapTranslationState(t_ENU_MAP);
 
         WOLF_INFO("SensorGnss: ENU-MAP translation initialized.");
@@ -202,7 +199,7 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, co
     Eigen::Vector2d t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
     Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
     Eigen::Vector2d v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
-    Eigen::Vector3d v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
+    Eigen::Vector3d v_ENU_antena1_antena2 = q_ENU_ECEF_ * _v_ECEF_antena1_antena2;
 
     double theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
     double theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
@@ -246,7 +243,7 @@ Eigen::Vector3d SensorGnss::computeFrameAntennaPosEcef(const FrameBasePtr& frm)
     assert(isEnuDefined());
     assert(frm and frm->getP() and frm->getO());
 
-    return R_ENU_ECEF_.transpose() * (-t_ENU_ECEF_ + gettEnuMap() + getREnuMap() * (frm->getP()->getState() + Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())) * this->getP()->getState()));
+    return q_ENU_ECEF_.conjugate() * (-t_ENU_ECEF_ + gettEnuMap() + getQEnuMap() * (frm->getP()->getState() + Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())) * this->getP()->getState()));
 }
 
 } // namespace wolf

test/CMakeLists.txt

@@ -11,6 +11,12 @@ add_subdirectory(gtest)
 # FactorGnssFix2d test
 wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp)
 
+# FactorGnssFix3d test
+wolf_add_gtest(gtest_factor_gnss_fix_3d gtest_factor_gnss_fix_3d.cpp)
+
+# FactorGnssFix3dWithClock test
+wolf_add_gtest(gtest_factor_gnss_fix_3d_with_clock gtest_factor_gnss_fix_3d_with_clock.cpp)
+
 # FactorGnssPseudoRange test
 wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp)
 

test/gtest_factor_gnss_fix_3d.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "gnss/internal/config.h"
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_3d.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+int N = 10;
+
+class FactorGnssFix3dTest : public testing::Test
+{
+    public:
+
+        // groundtruth transformations
+        Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena, t_ecef_antena;
+        Vector3d rpy_enu_map;
+        Quaterniond q_ecef_enu, q_enu_map, q_map_base;
+        double clock_bias;
+
+        // WOLF
+        ProblemPtr          problem;
+        SolverCeresPtr      solver_ceres;
+        SensorGnssPtr       gnss_sensor;
+        FrameBasePtr        frame;
+        CaptureGnssFixPtr   cap_gnss;
+        FeatureBasePtr      feat_gnss;
+        FactorGnssFix3dPtr  fac_gnss_fix_3d;
+
+        void randomSetup()
+        {
+            ASSERT_TRUE(frame);
+            ASSERT_TRUE(cap_gnss);
+
+            if (fac_gnss_fix_3d)
+                feat_gnss->remove();
+
+            ASSERT_TRUE(frame);
+            ASSERT_TRUE(cap_gnss);
+
+            // groundtruth transformations
+            rpy_enu_map    = Vector3d::Random() * M_PI * 0.1;
+            t_enu_map      = Vector3d::Random() * 20;
+            t_map_base     = Vector3d::Random() * 50;
+            t_base_antena  = Vector3d::Random() * 3;// Antena extrinsics
+            t_ecef_enu     = Vector3d::Random().cwiseAbs() * 1e3;
+            q_ecef_enu     = Quaterniond::UnitRandom();
+            q_enu_map      = e2q(rpy_enu_map);
+            q_map_base     = Quaterniond::UnitRandom();
+            t_ecef_antena  = q_ecef_enu * (q_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+            clock_bias     = Vector1d::Random()(0);
+         
+            // Fill groundtruth ----------------
+            // ENU-MAP
+            gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+            gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+            gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+            gnss_sensor->setEnuMapTranslationState(t_enu_map);
+            // ECEF-ENU
+            Vector3d t_enu_ecef;
+            Quaterniond q_enu_ecef;
+            SE3::inverse(t_ecef_enu, q_ecef_enu, t_enu_ecef, q_enu_ecef);
+            gnss_sensor->resetEnu();
+            gnss_sensor->setEnuEcef(q2R(q_enu_ecef), t_enu_ecef);
+            // Antena
+            gnss_sensor->getP()->setState(t_base_antena);
+            // Frame
+            frame->getP()->setState(t_map_base);
+            frame->getO()->setState(q_map_base.coeffs());
+            // capture
+            cap_gnss->setPositionEcef(t_ecef_antena);
+            cap_gnss->getStateBlock('T')->setState(Vector1d(clock_bias));
+            
+            // Fake measurement ---------------------
+            // feature
+            GnssUtils::ComputePosOutput pos_output;
+            pos_output.pos = t_ecef_antena;
+            pos_output.sol_cov = cap_gnss->getFixCovarianceEcef();
+            pos_output.rcv_bias(0) = cap_gnss->getClockBias();
+            feat_gnss = FeatureBase::emplace<FeatureGnssFix>(cap_gnss, pos_output);
+            // factor
+            fac_gnss_fix_3d = FactorBase::emplace<FactorGnssFix3d>(feat_gnss, feat_gnss, gnss_sensor, nullptr, false);
+        }
+
+        void checkStates()
+        {
+            // clock bias
+            ASSERT_NEAR(cap_gnss->getStateBlock('T')->getState()(0), clock_bias, Constants::EPS);
+            // ENU-MAP
+            ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0),    rpy_enu_map(0), Constants::EPS);
+            ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0),   rpy_enu_map(1), Constants::EPS);
+            ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0),     rpy_enu_map(2), Constants::EPS);
+            ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
+            // Antena
+            ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena,Constants::EPS);
+            // Frame
+            ASSERT_MATRIX_APPROX(frame->getP()->getState(), t_map_base, Constants::EPS);
+            ASSERT_QUATERNION_VECTOR_APPROX(frame->getO()->getState(), q_map_base.coeffs(), Constants::EPS);
+        }
+
+        void SetUp() override
+        {
+            // WOLF
+            problem      = Problem::create("PO", 3);
+            solver_ceres = std::make_shared<SolverCeres>(problem);
+            solver_ceres->getSolverOptions().max_num_iterations = 100;
+            solver_ceres->getSolverOptions().gradient_tolerance = 1e-8;
+            solver_ceres->getSolverOptions().function_tolerance = 1e-8;
+
+            // sensor
+            gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor("SensorGnss", 
+                                                                                      "gnss",
+                                                                                      t_base_antena,
+                                                                                      std::make_shared<ParamsSensorGnss>()));
+
+            // frame
+            frame = problem->emplaceFrame(TimeStamp(0));
+            problem->keyFrameCallback(frame, nullptr);
+
+            // capture
+            cap_gnss = CaptureBase::emplace<CaptureGnssFix>(frame,
+                                                            TimeStamp(0), 
+                                                            gnss_sensor,
+                                                            Vector4d::Zero(),
+                                                            1e-6*Matrix4d::Identity());
+        }
+
+        void fixAllStates()
+        {
+            cap_gnss->fix();
+            // ENU-MAP
+            gnss_sensor->getEnuMapRoll()->fix();
+            gnss_sensor->getEnuMapPitch()->fix();
+            gnss_sensor->getEnuMapYaw()->fix();
+            gnss_sensor->getEnuMapTranslation()->fix();
+            // Antena
+            gnss_sensor->getP()->fix();
+            // Frame
+            frame->getP()->fix();
+            frame->getO()->fix();
+        }
+};
+
+
+////////////////////////////////////////////////////////
+
+TEST_F(FactorGnssFix3dTest, configure_tree)
+{
+    EXPECT_TRUE(problem->check(1));
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_map_base_position)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        frame->getP()->unfix();
+
+        // --------------------------- perturb: map base position
+        frame->getP()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE orientation.
+ * Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_map_base_orientation)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        frame->getO()->unfix();
+
+        // --------------------------- perturb: map base position
+        frame->getO()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+        // --------------------------- check solver solution
+        // NOT OBSERVABLE checkStates();
+    }
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP yaw.
+ * Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_enu_map_yaw)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        gnss_sensor->getEnuMapYaw()->unfix();
+
+        // --------------------------- perturb: map base position
+        gnss_sensor->getEnuMapYaw()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP position.
+ * Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_enu_map_position)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        gnss_sensor->getEnuMapTranslation()->unfix();
+
+        // --------------------------- perturb: map base position
+        gnss_sensor->getEnuMapTranslation()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: BASE_ANTENA (2 first components that are observable).
+ * Fixed: ENU_MAP, MAP_BASE.
+ */
+TEST_F(FactorGnssFix3dTest, gnss_1_base_antena)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        gnss_sensor->getP()->unfix();
+
+        // --------------------------- perturb: map base position
+        gnss_sensor->getP()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/gtest_factor_gnss_fix_3d_with_clock.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "gnss/internal/config.h"
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_3d_with_clock.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+int N = 10;
+
+class FactorGnssFix3dWithClockTest : public testing::Test
+{
+    public:
+
+        // groundtruth transformations
+        Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena, t_ecef_antena;
+        Vector3d rpy_enu_map;
+        Quaterniond q_ecef_enu, q_enu_map, q_map_base;
+        double clock_bias;
+
+        // WOLF
+        ProblemPtr                   problem;
+        SolverCeresPtr               solver_ceres;
+        SensorGnssPtr                gnss_sensor;
+        FrameBasePtr                 frame;
+        CaptureGnssFixPtr            cap_gnss;
+        FeatureBasePtr               feat_gnss;
+        FactorGnssFix3dWithClockPtr  fac_gnss_fix_3d;
+
+        void randomSetup()
+        {
+            ASSERT_TRUE(frame);
+            ASSERT_TRUE(cap_gnss);
+
+            if (fac_gnss_fix_3d)
+                feat_gnss->remove();
+
+            ASSERT_TRUE(frame);
+            ASSERT_TRUE(cap_gnss);
+
+            // groundtruth transformations
+            rpy_enu_map    = Vector3d::Random() * M_PI * 0.2;
+            t_enu_map      = Vector3d::Random() * 20;
+            t_map_base     = Vector3d::Random() * 50;
+            t_base_antena  = Vector3d::Random() * 3;// Antena extrinsics
+            t_ecef_enu     = Vector3d::Random().cwiseAbs() * 1e3;
+            q_ecef_enu     = Quaterniond::UnitRandom();
+            q_enu_map      = e2q(rpy_enu_map);
+            q_map_base     = Quaterniond::UnitRandom();
+            t_ecef_antena  = q_ecef_enu * (q_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+            clock_bias     = Vector1d::Random()(0);
+         
+            // Fill groundtruth --------
+            // ENU-MAP
+            gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+            gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+            gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+            gnss_sensor->setEnuMapTranslationState(t_enu_map);
+            // ECEF-ENU
+            Vector3d t_enu_ecef;
+            Quaterniond q_enu_ecef;
+            SE3::inverse(t_ecef_enu, q_ecef_enu, t_enu_ecef, q_enu_ecef);
+            gnss_sensor->resetEnu();
+            gnss_sensor->setEnuEcef(q2R(q_enu_ecef), t_enu_ecef);
+            // Antena
+            gnss_sensor->getP()->setState(t_base_antena);
+            // Frame
+            frame->getP()->setState(t_map_base);
+            frame->getO()->setState(q_map_base.coeffs());
+            // capture
+            cap_gnss->setPositionEcef(t_ecef_antena);
+            cap_gnss->setClockBias(clock_bias);
+            cap_gnss->getStateBlock('T')->setState(Vector1d(clock_bias));
+            
+            // Create fake measurement ----------------
+            // feature
+            GnssUtils::ComputePosOutput pos_output;
+            pos_output.pos = t_ecef_antena;
+            pos_output.sol_cov = Matrix4d::Identity() * 1e-6;
+            pos_output.rcv_bias(0) = cap_gnss->getClockBias();
+            feat_gnss = FeatureBase::emplace<FeatureGnssFix>(cap_gnss, pos_output);
+            // factor
+            fac_gnss_fix_3d = FactorBase::emplace<FactorGnssFix3dWithClock>(feat_gnss, feat_gnss, gnss_sensor, nullptr, false);
+        }
+
+        void checkStates()
+        {
+            // clock bias
+            ASSERT_NEAR(cap_gnss->getStateBlock('T')->getState()(0), clock_bias, Constants::EPS);
+            // ENU-MAP
+            ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0),    rpy_enu_map(0), Constants::EPS*10);
+            ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0),   rpy_enu_map(1), Constants::EPS*10);
+            ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0),     rpy_enu_map(2), Constants::EPS*10);
+            ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
+            // Antena
+            ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena,Constants::EPS);
+            // Frame
+            ASSERT_MATRIX_APPROX(frame->getP()->getState(), t_map_base, Constants::EPS);
+            ASSERT_QUATERNION_VECTOR_APPROX(frame->getO()->getState(), q_map_base.coeffs(), Constants::EPS);
+        }
+
+        void SetUp() override
+        {
+            // WOLF
+            problem      = Problem::create("PO", 3);
+            solver_ceres = std::make_shared<SolverCeres>(problem);
+            solver_ceres->getSolverOptions().max_num_iterations = 100;
+            solver_ceres->getSolverOptions().gradient_tolerance = 1e-8;
+            solver_ceres->getSolverOptions().function_tolerance = 1e-9;
+
+            // sensor
+            gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor("SensorGnss", 
+                                                                                      "gnss",
+                                                                                      t_base_antena,
+                                                                                      std::make_shared<ParamsSensorGnss>()));
+
+            // frame
+            frame = problem->emplaceFrame(TimeStamp(0));
+            problem->keyFrameCallback(frame, nullptr);
+
+            // capture
+            cap_gnss = CaptureBase::emplace<CaptureGnssFix>(frame,
+                                                            TimeStamp(0), 
+                                                            gnss_sensor,
+                                                            Vector4d::Zero(),
+                                                            1e-6*Matrix4d::Identity());
+        }
+
+        void fixAllStates()
+        {
+            cap_gnss->fix();
+            // ENU-MAP
+            gnss_sensor->getEnuMapRoll()->fix();
+            gnss_sensor->getEnuMapPitch()->fix();
+            gnss_sensor->getEnuMapYaw()->fix();
+            gnss_sensor->getEnuMapTranslation()->fix();
+            // Antena
+            gnss_sensor->getP()->fix();
+            // Frame
+            frame->getP()->fix();
+            frame->getO()->fix();
+        }
+};
+
+
+////////////////////////////////////////////////////////
+
+TEST_F(FactorGnssFix3dWithClockTest, configure_tree)
+{
+    EXPECT_TRUE(problem->check(1));
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_map_base_position)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        cap_gnss->getStateBlock('T')->unfix();
+        frame->getP()->unfix();
+
+        // --------------------------- perturb: map base position
+        cap_gnss->getStateBlock('T')->perturb(0.2);
+        frame->getP()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE orientation.
+ * Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_map_base_orientation)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        cap_gnss->getStateBlock('T')->unfix();
+        frame->getO()->unfix();
+
+        // --------------------------- perturb: map base position
+        cap_gnss->getStateBlock('T')->perturb(0.2);
+        frame->getO()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 5);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+        // --------------------------- check solver solution
+        // NOT OBSERVABLE checkStates();
+    }
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP yaw.
+ * Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_enu_map_yaw)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        cap_gnss->getStateBlock('T')->unfix();
+        gnss_sensor->getEnuMapYaw()->unfix();
+
+        // --------------------------- perturb: map base position
+        cap_gnss->getStateBlock('T')->perturb(0.2);
+        gnss_sensor->getEnuMapYaw()->perturb(0.2);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP position.
+ * Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_enu_map_position)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        cap_gnss->getStateBlock('T')->unfix();
+        gnss_sensor->getEnuMapTranslation()->unfix();
+
+        // --------------------------- perturb: map base position
+        cap_gnss->getStateBlock('T')->perturb(0.2);
+        gnss_sensor->getEnuMapTranslation()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+/* Test with one GNSS fix capture.
+ *
+ * Estimating: BASE_ANTENA (2 first components that are observable).
+ * Fixed: ENU_MAP, MAP_BASE.
+ */
+TEST_F(FactorGnssFix3dWithClockTest, gnss_1_base_antena)
+{
+    for (auto i = 0; i<N; i++)
+    {
+        randomSetup();
+
+        // --------------------------- fixing/unfixing states
+        fixAllStates();
+        cap_gnss->getStateBlock('T')->unfix();
+        gnss_sensor->getP()->unfix();
+
+        // --------------------------- perturb: map base position
+        cap_gnss->getStateBlock('T')->perturb(0.2);
+        gnss_sensor->getP()->perturb(0.5);
+
+        // --------------------------- solve
+        std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
+        // std::cout << report << std::endl;
+
+        // --------------------------- check summary parameters & residuals
+        EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 2);
+        EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 4);
+        EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+        EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 4);
+
+        // --------------------------- check solver solution
+        checkStates();
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/gtest_factor_gnss_pseudo_range.cpp

@@ -60,17 +60,11 @@ void randomGroundtruth()
 {
     // ECEF-ENU
     t_ecef_enu = Vector3d::Random() * 1e3;
-    Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
-    R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
-                  AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
-                  AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
-
+    R_ecef_enu = q2R(Quaterniond::UnitRandom());
     // ENU-MAP
     t_enu_map = Vector3d::Random() * 1e3;
     rpy_enu_map = M_PI*Vector3d::Random();
-    R_enu_map  = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
-                  AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
-                  AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
+    R_enu_map  = e2R(rpy_enu_map);
 
     // MAP-BASE
     t_map_base = Vector3d::Random() * 1e2;
@@ -109,6 +103,8 @@ void setUpProblem()
         frm->remove();
 
     solver->getSolverOptions().max_num_iterations = 100;
+    solver->getSolverOptions().gradient_tolerance = 1e-8;
+    solver->getSolverOptions().function_tolerance = 1e-8;
 
     // Sensor
     // ENU-MAP
@@ -139,10 +135,10 @@ void setUpProblem()
     ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat4, range4); // obsd_t data is not used in pseudo range factors
 
     // factors
-    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);
+    fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false, false); // without sagnac correction
+    fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false, false); // without sagnac correction
+    fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false, false); // without sagnac correction
+    fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false, false); // without sagnac correction
 
     // ASSERTS
     // ENU-MAP