residual evaluated in ENU (more efficient)

include/gnss/factor/factor_gnss_pseudo_range.h

@@ -20,7 +20,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
         GnssUtils::Combination mode_;
         double pseudo_range_;
         double std_dev_;
-        Eigen::Vector3d satellite_position_;
+        Eigen::Vector3d satellite_ENU_;
 
     public:
 
@@ -50,7 +50,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
             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->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef())
         {
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
 
@@ -102,16 +102,14 @@ 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() + _clock_drift[0];
 
     // Residual
     _residual[0] = (exp - pseudo_range_) / std_dev_;

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,8 +55,8 @@ 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->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef()),
+            satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef())
         {
             assert(_ftr_r != _ftr_k);
             assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr);
@@ -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_;