diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h index 1360153f3be5cdf907f8d4d036d586311cc1b26c..0c12bf9725a6ad0fa9eadfac163d1bbeec044ea9 100644 --- a/include/gnss/factor/factor_gnss_pseudo_range.h +++ b/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_; diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h index 5a0d843dfe2c36781f109dae078ae277eaff9fde..1e126c64e065665c7bf59ba879d273f0e96a8359 100644 --- a/include/gnss/factor/factor_gnss_tdcp.h +++ b/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_;