diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h index 3208162fcdde1b783cac2e6b37c3c6d1f8b7337d..6f7548cf56690c76156ebdcff6acf12fc2375746 100644 --- a/include/gnss/capture/capture_gnss.h +++ b/include/gnss/capture/capture_gnss.h @@ -25,8 +25,8 @@ class CaptureGnss : public CaptureBase GnssUtils::SnapshotPtr getSnapshot() const; GnssUtils::ObservationsPtr getObservations() const; GnssUtils::NavigationPtr getNavigation() const; - GnssUtils::SatellitesPositions& getSatellitesPositions(); - const GnssUtils::SatellitesPositions& getSatellitesPositions() const; + GnssUtils::Satellites& getSatellites(); + const GnssUtils::Satellites& getSatellites() const; }; @@ -45,14 +45,14 @@ inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const return snapshot_->getNavigation(); } -inline GnssUtils::SatellitesPositions& CaptureGnss::getSatellitesPositions() +inline GnssUtils::Satellites& CaptureGnss::getSatellites() { - return snapshot_->getSatellitesPositions(); + return snapshot_->getSatellites(); } -inline const GnssUtils::SatellitesPositions& CaptureGnss::getSatellitesPositions() const +inline const GnssUtils::Satellites& CaptureGnss::getSatellites() const { - return snapshot_->getSatellitesPositions(); + return snapshot_->getSatellites(); } } //namespace wolf diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h index 0c12bf9725a6ad0fa9eadfac163d1bbeec044ea9..5c82f25b359734c2e9392b738b10892cd07ecb3b 100644 --- a/include/gnss/factor/factor_gnss_pseudo_range.h +++ b/include/gnss/factor/factor_gnss_pseudo_range.h @@ -50,9 +50,9 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, sensor_gnss_ptr_(_sensor_gnss_ptr), mode_(_mode), std_dev_(_std_dev), - satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellitePosition() + sensor_gnss_ptr_->gettEnuEcef()) + satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef()) { - WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU"); + WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU"); // Pseudo range combination switch (mode_) diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h index 1e126c64e065665c7bf59ba879d273f0e96a8359..6cab32ff777a7e2821a8fd042ef7c503311bf245 100644 --- a/include/gnss/factor/factor_gnss_tdcp.h +++ b/include/gnss/factor/factor_gnss_tdcp.h @@ -55,14 +55,14 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 sensor_gnss_ptr_(_sensor_gnss_ptr), mode_(_mode), std_dev_(_std_dev), - satellite_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()) + satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef()), + satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef()) { assert(_ftr_r != _ftr_k); assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr); assert(_ftr_k->getCapture()->getStateBlock("T") != nullptr); - WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU"); + WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an FactorGnssTdcp without initializing ENU"); // Pseudo range combination // TODO encapsular prange a GnssUtils. diff --git a/include/gnss/feature/feature_gnss_satellite.h b/include/gnss/feature/feature_gnss_satellite.h index b8d5ce30dcc352bc126e4bdc42517f23bde89ebb..cec277f7da3323177a24413003e07f7222d031e6 100644 --- a/include/gnss/feature/feature_gnss_satellite.h +++ b/include/gnss/feature/feature_gnss_satellite.h @@ -6,6 +6,7 @@ //std includes #include "gnss_utils/gnss_utils.h" +#include "gnss_utils/utils/satellite.h" namespace wolf { @@ -24,17 +25,17 @@ class FeatureGnssSatellite : public FeatureBase * This constructor will take the pseudorange as measurement with 1 m² of variance * */ - FeatureGnssSatellite(const obsd_t& _obs_sat, const Eigen::Vector3d& _sat_pos); + FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat); virtual ~FeatureGnssSatellite(){}; const obsd_t& getObservation() const; int satNumber() const; - const Eigen::Vector3d& getSatellitePosition() const; + const GnssUtils::Satellite& getSatellite() const; private: obsd_t obs_sat_; - Eigen::Vector3d sat_pos_; + GnssUtils::Satellite sat_; }; @@ -43,10 +44,10 @@ class FeatureGnssSatellite : public FeatureBase // IMPLEMENTATION namespace wolf { -inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const Eigen::Vector3d& _sat_pos) : +inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat) : FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_obs_sat.P[0]), Eigen::Matrix1d(1.0)), obs_sat_(_obs_sat), - sat_pos_(_sat_pos) + sat_(_sat) { // } @@ -61,9 +62,9 @@ inline int FeatureGnssSatellite::satNumber() const return obs_sat_.sat; } -inline const Eigen::Vector3d& FeatureGnssSatellite::getSatellitePosition() const +inline const GnssUtils::Satellite& FeatureGnssSatellite::getSatellite() const { - return sat_pos_; + return sat_; } } // namespace wolf diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h index ff3a0a5e022fd00cd5abbc1f70d319fa7db3dfde..16cbd7c4cac7cf319c2bb5a8d2227acac2a55c7c 100644 --- a/include/gnss/processor/processor_tracker_gnss.h +++ b/include/gnss/processor/processor_tracker_gnss.h @@ -17,12 +17,14 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature prcopt_t gnss_opt; double enu_map_init_dist_min; double max_time_span; + bool remove_outliers; ParamsProcessorTrackerGnss() = default; ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server): ParamsProcessorTrackerFeature(_unique_name, _server) { enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min"); + remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers"); max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span"); // GNSS OPTIONS (see rtklib.h) @@ -53,6 +55,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature { return "\n" + ParamsProcessorBase::print() + "\n" + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n" + + "remove_outliers: " + std::to_string(remove_outliers) + "\n" + + "max_time_span: " + std::to_string(max_time_span) + "\n" + "mode: " + std::to_string(gnss_opt.mode) + "\n" + "soltype: " + std::to_string(gnss_opt.soltype) + "\n" + "nf: " + std::to_string(gnss_opt.nf) + "\n" diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index c382cba77af6e4886d948e6060e7c59dc8484653..3e9acea97f81501e3a504b01fed32377d1214510 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -13,8 +13,8 @@ void ProcessorTrackerGnss::preProcess() GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot(); // compute satellites positions - if (!inc_snapshot ->satellitesPositionsComputed()) - inc_snapshot ->computeSatellitesPositions(params_tracker_gnss_->gnss_opt.sateph); + if (!inc_snapshot ->satellitesComputed()) + inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph); // Compute (and store) auxiliar fix (for computing elevation) GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(), @@ -31,6 +31,8 @@ void ProcessorTrackerGnss::preProcess() sensor_gnss_->setEcefEnu(fix_output.pos,true); //sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true); WOLF_INFO("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef()); + + WOLF_INFO("should be zero: ", (sensor_gnss_->getREnuEcef() * fix_output.pos + sensor_gnss_->gettEnuEcef()).transpose()); } WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose()); @@ -47,7 +49,7 @@ void ProcessorTrackerGnss::preProcess() // create features pseudorange for (auto obs_sat : inc_snapshot->getObservations()->getObservations()) { - auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellitesPositions().at(obs_sat.sat)); + auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellites().at(obs_sat.sat)); assert(untracked_incoming_features_.count(feat->satNumber()) == 0); untracked_incoming_features_[feat->satNumber()] = feat; } @@ -134,11 +136,7 @@ void ProcessorTrackerGnss::establishFactors() FactorBasePtrList new_factors; - if (last_ptr_->getStateBlock("T") == nullptr) - { - WOLF_DEBUG("last clock offset state block nullptr, adding it"); - last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(1), getProblem()); - } + bool last_clock_drift_set = false; // PSEUDO RANGE FACTORS (all sats) for (auto ftr : last_ptr_->getFeatureList()) @@ -151,6 +149,19 @@ void ProcessorTrackerGnss::establishFactors() if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination) continue; + // Initialize (and create) clock drift stateblock in capture + if (!last_clock_drift_set) + { + if (last_ptr_->getStateBlock("T") == nullptr) + last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(-fix_last_(3))), getProblem()); + else + last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(-fix_last_(3))); + + WOLF_INFO("last clock drift initialized: ", last_ptr_->getStateBlock("T")->getState().transpose()); + + last_clock_drift_set = true; + } + // emplace a pseudo range factor auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat, GnssUtils::Combination::CODE_L1,//TODO: from params @@ -196,6 +207,16 @@ void ProcessorTrackerGnss::establishFactors() // // WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id()); // +// // Initialize (and create) clock drift stateblock in capture +// if (!last_clock_drift_set) +// { +// if (last_ptr_->getStateBlock("T") == nullptr) +// last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(fix_last_(3))), getProblem()); +// else +// last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(fix_last_(3))); +// last_clock_drift_set = true; +// } +// // // emplace tdcp factor // auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k, // GnssUtils::Combination::CARRIER_L1,//TODO: from params @@ -216,7 +237,8 @@ void ProcessorTrackerGnss::establishFactors() // } // remove outliers - removeOutliers(new_factors, last_ptr_); + if (!new_factors.empty() and params_tracker_gnss_->remove_outliers) + removeOutliers(new_factors, last_ptr_); } void ProcessorTrackerGnss::advanceDerived() @@ -279,7 +301,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr); // discard if residual too high evaluated at the current estimation - if (residual_pr(0) > 1e3) + if (std::abs(residual_pr(0)) > 1e3) { WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER"); WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose()); diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index 9812994863bae8a59c69b8eef88fcd9b1d02fa30..ce0723f7a39529595ca59ec3133cc4853629cae9 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -17,9 +17,12 @@ Vector3d t_ecef_antena; Vector3d rpy_enu_map; Matrix3d R_ecef_enu, R_enu_map; Quaterniond q_map_base; -Vector3d t_ecef_sat1, t_ecef_sat2, t_ecef_sat3, t_ecef_sat4; double prange_1, prange_2, prange_3, prange_4; Vector1d clock_drift; +GnssUtils::Satellite sat1(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat2(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat3(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat4(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); // WOLF ProblemPtr prb = Problem::create("PO", 3); @@ -57,19 +60,19 @@ void randomGroundtruth() t_ecef_antena = R_ecef_enu * (R_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu; // SATELLITES - t_ecef_sat1 = Vector3d::Random() * 1e4; - t_ecef_sat2 = Vector3d::Random() * 1e4; - t_ecef_sat3 = Vector3d::Random() * 1e4; - t_ecef_sat4 = Vector3d::Random() * 1e4; + sat1.pos_ = Vector3d::Random() * 1e4; + sat2.pos_ = Vector3d::Random() * 1e4; + sat3.pos_ = Vector3d::Random() * 1e4; + sat4.pos_ = Vector3d::Random() * 1e4; // clock drift clock_drift = Vector1d::Random() * 1e2; // pseudo ranges - prange_1 = (t_ecef_sat1-t_ecef_antena).norm() + clock_drift(0); - prange_2 = (t_ecef_sat2-t_ecef_antena).norm() + clock_drift(0); - prange_3 = (t_ecef_sat3-t_ecef_antena).norm() + clock_drift(0); - prange_4 = (t_ecef_sat4-t_ecef_antena).norm() + clock_drift(0); + prange_1 = (sat1.pos_-t_ecef_antena).norm() + clock_drift(0); + prange_2 = (sat2.pos_-t_ecef_antena).norm() + clock_drift(0); + prange_3 = (sat3.pos_-t_ecef_antena).norm() + clock_drift(0); + prange_4 = (sat4.pos_-t_ecef_antena).norm() + clock_drift(0); } void setUpProblem() @@ -104,16 +107,16 @@ void setUpProblem() // features obsd_t obs1{0}; obs1.P[0] = prange_1; - ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, t_ecef_sat1); + ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, sat1); obsd_t obs2{0}; obs2.P[0] = prange_2; - ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, t_ecef_sat2); + ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, sat2); obsd_t obs3{0}; obs3.P[0] = prange_3; - ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, t_ecef_sat3); + ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, sat3); obsd_t obs4{0}; obs4.P[0] = prange_4; - ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4); + ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, sat4); // factors fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false); diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp index 1cd9dcdf3df75e3a65e978c289b90945ed6a0b85..41f195537055d0e6f6b84c43f6941e98abd1283c 100644 --- a/test/gtest_factor_gnss_tdcp.cpp +++ b/test/gtest_factor_gnss_tdcp.cpp @@ -17,10 +17,18 @@ Vector3d t_ecef_antena_r, t_ecef_antena_k; Vector3d rpy_enu_map; Matrix3d R_ecef_enu, R_enu_map; Quaterniond q_map_base_r, q_map_base_k; -Vector3d t_ecef_sat1_r, t_ecef_sat2_r, t_ecef_sat3_r, t_ecef_sat4_r, t_ecef_sat1_k, t_ecef_sat2_k, t_ecef_sat3_k, t_ecef_sat4_k; double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k; Vector1d clock_drift_r, clock_drift_k; +GnssUtils::Satellite sat1_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat2_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat3_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat4_r(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat1_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat2_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat3_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); +GnssUtils::Satellite sat4_k(1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0); + // WOLF ProblemPtr prb = Problem::create("PO", 3); CeresManagerPtr solver = std::make_shared<CeresManager>(prb); @@ -60,28 +68,28 @@ void randomGroundtruth() t_ecef_antena_k = R_ecef_enu * (R_enu_map * (q_map_base_k * t_base_antena + t_map_base_k) + t_enu_map ) + t_ecef_enu; // SATELLITES - t_ecef_sat1_r = Vector3d::Random() * 1e4; - t_ecef_sat2_r = Vector3d::Random() * 1e4; - t_ecef_sat3_r = Vector3d::Random() * 1e4; - t_ecef_sat4_r = Vector3d::Random() * 1e4; - t_ecef_sat1_k = Vector3d::Random() * 1e4; - t_ecef_sat2_k = Vector3d::Random() * 1e4; - t_ecef_sat3_k = Vector3d::Random() * 1e4; - t_ecef_sat4_k = Vector3d::Random() * 1e4; + sat1_r.pos_ = Vector3d::Random() * 1e4; + sat2_r.pos_ = Vector3d::Random() * 1e4; + sat3_r.pos_ = Vector3d::Random() * 1e4; + sat4_r.pos_ = Vector3d::Random() * 1e4; + sat1_k.pos_ = Vector3d::Random() * 1e4; + sat2_k.pos_ = Vector3d::Random() * 1e4; + sat3_k.pos_ = Vector3d::Random() * 1e4; + sat4_k.pos_ = Vector3d::Random() * 1e4; // clock drift clock_drift_r = Vector1d::Random() * 1e2; clock_drift_k = Vector1d::Random() * 1e2; // pseudo ranges - prange1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0); - prange2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0); - prange3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0); - prange4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0); - prange1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0); - prange2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0); - prange3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0); - prange4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0); + prange1_r = (sat1_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0); + prange2_r = (sat2_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0); + prange3_r = (sat3_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0); + prange4_r = (sat4_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0); + prange1_k = (sat1_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0); + prange2_k = (sat2_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0); + prange3_k = (sat3_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0); + prange4_k = (sat4_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0); } void setUpProblem() @@ -128,30 +136,30 @@ void setUpProblem() // features r obsd_t obs1_r{0}; obs1_r.L[0] = prange1_r; - ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, t_ecef_sat1_r); + ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, sat1_r); obsd_t obs2_r{0}; obs2_r.L[0] = prange2_r; - ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, t_ecef_sat2_r); + ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, sat2_r); obsd_t obs3_r{0}; obs3_r.L[0] = prange3_r; - ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, t_ecef_sat3_r); + ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, sat3_r); obsd_t obs4_r{0}; obs4_r.L[0] = prange4_r; - ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, t_ecef_sat4_r); + ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, sat4_r); // features k obsd_t obs1_k{0}; obs1_k.L[0] = prange1_k; - ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, t_ecef_sat1_k); + ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, sat1_k); obsd_t obs2_k{0}; obs2_k.L[0] = prange2_k; - ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, t_ecef_sat2_k); + ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, sat2_k); obsd_t obs3_k{0}; obs3_k.L[0] = prange3_k; - ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, t_ecef_sat3_k); + ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, sat3_k); obsd_t obs4_k{0}; obs4_k.L[0] = prange4_k; - ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, t_ecef_sat4_k); + ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, sat4_k); // factors fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);