diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index 5c82f25b359734c2e9392b738b10892cd07ecb3b..8212bc7e4ec84440fc60db89388c4ddaafbe8062 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -1,6 +1,6 @@
#ifndef FACTOR_GNSS_PSEUDO_RANGE_H_
#define FACTOR_GNSS_PSEUDO_RANGE_H_
-
+#include <type_traits>
//Wolf includes
#include "core/common/wolf.h"
#include "core/factor/factor_autodiff.h"
@@ -17,16 +17,11 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
- GnssUtils::Combination mode_;
- double pseudo_range_;
- double std_dev_;
Eigen::Vector3d satellite_ENU_;
public:
- FactorGnssPseudoRange(GnssUtils::Combination _mode,
- const double& _std_dev,
- FeatureGnssSatellitePtr& _ftr_ptr,
+ FactorGnssPseudoRange(FeatureGnssSatellitePtr& _ftr_ptr,
const SensorGnssPtr& _sensor_gnss_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
@@ -48,23 +43,10 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
_sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr),
- mode_(_mode),
- std_dev_(_std_dev),
- satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos_ + sensor_gnss_ptr_->gettEnuEcef())
+ satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
{
+ //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");
-
- // Pseudo range combination
- switch (mode_)
- {
- case GnssUtils::Combination::CODE_L1:
- {
- pseudo_range_ = _ftr_ptr->getObservation().P[0];
- break;
- }
- default:
- throw std::runtime_error("not implemented");
- }
}
virtual ~FactorGnssPseudoRange() = default;
@@ -77,7 +59,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
template<typename T>
bool operator ()(const T* const _x,
const T* const _o,
- const T* const _clock_drift,
+ const T* const _clock_bias,
const T* const _x_antena,
const T* const _t_ENU_map,
const T* const _roll_ENU_map,
@@ -90,7 +72,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
template<typename T>
inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
const T* const _o,
- const T* const _clock_drift,
+ const T* const _clock_bias,
const T* const _x_antena,
const T* const _t_ENU_map,
const T* const _roll_ENU_map,
@@ -108,11 +90,20 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
// 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);
+ //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
+ //std::cout << std::setprecision(10) << "x = " << antena_ENU(0) << " "<< antena_ENU(1) << " "<< antena_ENU(2) << "\n";
+
// Expected pseudo-range
- T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() + _clock_drift[0];
+ T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() - _clock_bias[0];
+
+ //std::cout << "clock_bias = " << _clock_bias[0] << "\n";
+ //std::cout << "norm = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() << "\n";
+ //std::cout << "exp = " << exp << "\n";
+ //std::cout << "meas = " << getMeasurement() << "\n";
+ //std::cout << "error = " << exp - getMeasurement()(0) << "\n";
// Residual
- _residual[0] = (exp - pseudo_range_) / std_dev_;
+ _residual[0] = (exp - getMeasurement()(0)) / getMeasurementSquareRootInformationUpper()(0,0);
return true;
}
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index 6cab32ff777a7e2821a8fd042ef7c503311bf245..a34e1743e79cb1feb501a9a6dc98cd0cb419b26f 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -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_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_(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);
diff --git a/include/gnss/feature/feature_gnss_satellite.h b/include/gnss/feature/feature_gnss_satellite.h
index cec277f7da3323177a24413003e07f7222d031e6..f2b0087881933b1be96c44df59921f60b7250247 100644
--- a/include/gnss/feature/feature_gnss_satellite.h
+++ b/include/gnss/feature/feature_gnss_satellite.h
@@ -25,17 +25,19 @@ class FeatureGnssSatellite : public FeatureBase
* This constructor will take the pseudorange as measurement with 1 m² of variance
*
*/
- FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat);
+ FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::PseudoRange& _prange);
virtual ~FeatureGnssSatellite(){};
const obsd_t& getObservation() const;
int satNumber() const;
const GnssUtils::Satellite& getSatellite() const;
+ const GnssUtils::PseudoRange& getPseudoRange() const;
private:
obsd_t obs_sat_;
GnssUtils::Satellite sat_;
+ GnssUtils::PseudoRange prange_;
};
@@ -44,10 +46,11 @@ class FeatureGnssSatellite : public FeatureBase
// IMPLEMENTATION
namespace wolf {
-inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat) :
- FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_obs_sat.P[0]), Eigen::Matrix1d(1.0)),
+inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::PseudoRange& _prange) :
+ FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_prange.prange), Eigen::Matrix1d(_prange.var)),
obs_sat_(_obs_sat),
- sat_(_sat)
+ sat_(_sat),
+ prange_(_prange)
{
//
}
@@ -67,6 +70,11 @@ inline const GnssUtils::Satellite& FeatureGnssSatellite::getSatellite() const
return sat_;
}
+inline const GnssUtils::PseudoRange& FeatureGnssSatellite::getPseudoRange() const
+{
+ return prange_;
+}
+
} // namespace wolf
#endif
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index 16cbd7c4cac7cf319c2bb5a8d2227acac2a55c7c..c1fa0a82d2aa2e6bc6fdf5441e237e27c2820f54 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -91,7 +91,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
ParamsProcessorTrackerGnssPtr params_tracker_gnss_;
SensorGnssPtr sensor_gnss_;
std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
- Eigen::Vector4d fix_incoming_, fix_last_;
+ GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 3e9acea97f81501e3a504b01fed32377d1214510..bd41180186f04794657cae32c742b96bf57060f5 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -17,39 +17,41 @@ void ProcessorTrackerGnss::preProcess()
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(),
- *inc_snapshot->getNavigation(),
- prcopt_default); // take default (less restrictive?)
-
- fix_incoming_.head<3>() = fix_output.pos;
- fix_incoming_(3) = fix_output.rcv_bias(0);
+ fix_incoming_ = GnssUtils::computePos(*inc_snapshot->getObservations(),
+ *inc_snapshot->getNavigation(),
+ prcopt_default); // take default (less restrictive?)
// Set ECEF-ENU
- if (!sensor_gnss_->isEnuDefined() and fix_output.stat != 0)
+ if (!sensor_gnss_->isEnuDefined() and fix_incoming_.stat != 0)
{
- WOLF_INFO("setting ECEF-ENU: ", fix_output.pos.transpose());
- 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_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+ sensor_gnss_->setEcefEnu(fix_incoming_.pos,true);
+ WOLF_DEBUG("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef());
}
- WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose());
+ WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
// filter observations (available ephemeris, constellations and elevation&SNR)
inc_snapshot->filterObservations(std::set<int>(), // discarded sats
- fix_incoming_.head<3>(),
+ fix_incoming_.pos,
false, // check code
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
+ // compute corrected pseudoranges
+ inc_snapshot->computePseudoRanges(fix_incoming_.sat_azel,
+ fix_incoming_.lat_lon,
+ params_tracker_gnss_->gnss_opt);
+
// create features pseudorange
for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
{
- auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellites().at(obs_sat.sat));
+ assert(inc_snapshot->getSatellites().count(obs_sat.sat) != 0 && "satellite not found");
+ assert(inc_snapshot->getPseudoRanges().count(obs_sat.sat) != 0 && "pseudorange not found");
+
+ auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellites().at(obs_sat.sat), inc_snapshot->getPseudoRanges().at(obs_sat.sat));
assert(untracked_incoming_features_.count(feat->satNumber()) == 0);
untracked_incoming_features_[feat->satNumber()] = feat;
}
@@ -132,11 +134,9 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
void ProcessorTrackerGnss::establishFactors()
{
- WOLF_INFO("ProcessorTrackerGnss::establishFactors: Frame = ", last_ptr_->getFrame()->getState().transpose());
-
FactorBasePtrList new_factors;
- bool last_clock_drift_set = false;
+ bool last_clock_bias_set = false;
// PSEUDO RANGE FACTORS (all sats)
for (auto ftr : last_ptr_->getFeatureList())
@@ -146,26 +146,27 @@ void ProcessorTrackerGnss::establishFactors()
continue;
// check valid measurement
- if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
+ if (ftr_sat->getMeasurement()(0) < 1e-12)
+ {
+ WOLF_INFO("Feature with not valid pseudorange");
continue;
+ }
- // Initialize (and create) clock drift stateblock in capture
- if (!last_clock_drift_set)
+ // Initialize (and create) clock bias stateblock in capture
+ if (!last_clock_bias_set)
{
if (last_ptr_->getStateBlock("T") == nullptr)
- last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(-fix_last_(3))), getProblem());
+ last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))), getProblem());
else
- last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(-fix_last_(3)));
+ last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
- WOLF_INFO("last clock drift initialized: ", last_ptr_->getStateBlock("T")->getState().transpose());
+ //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState().transpose());
- last_clock_drift_set = true;
+ last_clock_bias_set = true;
}
// emplace a pseudo range factor
auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
- GnssUtils::Combination::CODE_L1,//TODO: from params
- 0.1,// TODO: compute from params and dt
ftr_sat,
sensor_gnss_,
shared_from_this(),
@@ -207,14 +208,14 @@ 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)
+// // Initialize (and create) clock bias stateblock in capture
+// if (!last_clock_bias_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;
+// last_clock_bias_set = true;
// }
//
// // emplace tdcp factor
@@ -259,7 +260,8 @@ void ProcessorTrackerGnss::resetDerived()
void ProcessorTrackerGnss::postProcess()
{
- getProblem()->print(4, 1, 1, 1);
+ if (last_ptr_ && last_ptr_->getFrame() && last_ptr_->getFrame()->isKey())
+ getProblem()->print(4, 1, 1, 1);
}
void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const
@@ -268,26 +270,35 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
//WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
// copy states
- Eigen::VectorXd x(cap->getFrame()->getP()->getState());
- Eigen::VectorXd o(cap->getFrame()->getO()->getState());
- Eigen::VectorXd clock_drift(cap->getStateBlock("T")->getState());
- Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
- Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
- Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
- Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
- Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+ Eigen::Vector3d x(cap->getFrame()->getP()->getState());
+ Eigen::Vector4d o(cap->getFrame()->getO()->getState());
+ Eigen::Vector1d clock_bias(-cap->getStateBlock("T")->getState());
+ Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState());
+ Eigen::Vector3d t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::Vector1d roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::Vector1d pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::Vector1d yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+
+ //std::cout << "Frame p: " << x.transpose() << std::endl;
+ //std::cout << "Frame o: " << o.transpose() << std::endl;
+ //std::cout << "clock_bias: " << clock_bias << std::endl;
+ //std::cout << "x_antena: " << x_antena.transpose() << std::endl;
+ //std::cout << "t_ENU_map: " << t_ENU_map.transpose() << std::endl;
+ //std::cout << "roll_ENU_map: " << roll_ENU_map << std::endl;
+ //std::cout << "pitch_ENU_map: " << pitch_ENU_map << std::endl;
+ //std::cout << "yaw_ENU_map: " << yaw_ENU_map << std::endl;
// create double* array of a copy of the state for pseudo range factors
double* parameters_pr[8] = {x.data(),
o.data(),
- clock_drift.data(),
+ clock_bias.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
// create residuals
- Eigen::VectorXd residual_pr(1);
+ double residual_pr;
for (auto fac : fac_list)
{
@@ -296,15 +307,15 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
if (fac_pr)
{
// evaluate the factor in this state
- fac_pr->evaluate(parameters_pr, residual_pr.data(), nullptr);
+ fac_pr->evaluate(parameters_pr, &residual_pr, nullptr);
- WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
+ //WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
// discard if residual too high evaluated at the current estimation
- if (std::abs(residual_pr(0)) > 1e3)
+ if (std::abs(residual_pr) > 1e3)
{
WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
- WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
+ WOLF_TRACE("Feature: ", fac->getMeasurement(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
remove_fac.push_back(fac_pr);
}
}
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index ce0723f7a39529595ca59ec3133cc4853629cae9..a5a47b37d5e5c60c9da309eab5ff81ef21edcd1f 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -17,12 +17,15 @@ Vector3d t_ecef_antena;
Vector3d rpy_enu_map;
Matrix3d R_ecef_enu, R_enu_map;
Quaterniond q_map_base;
-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);
+
+GnssUtils::Satellite sat1({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+
+GnssUtils::PseudoRange prange1, prange2, prange3, prange4;
+
// WOLF
ProblemPtr prb = Problem::create("PO", 3);
@@ -60,19 +63,23 @@ void randomGroundtruth()
t_ecef_antena = R_ecef_enu * (R_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
// SATELLITES
- sat1.pos_ = Vector3d::Random() * 1e4;
- sat2.pos_ = Vector3d::Random() * 1e4;
- sat3.pos_ = Vector3d::Random() * 1e4;
- sat4.pos_ = 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 = (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);
+ prange1.prange = (sat1.pos-t_ecef_antena).norm() + clock_drift(0);
+ prange2.prange = (sat2.pos-t_ecef_antena).norm() + clock_drift(0);
+ prange3.prange = (sat3.pos-t_ecef_antena).norm() + clock_drift(0);
+ prange4.prange = (sat4.pos-t_ecef_antena).norm() + clock_drift(0);
+ prange1.var = 1.0;
+ prange2.var = 1.0;
+ prange3.var = 1.0;
+ prange4.var = 1.0;
}
void setUpProblem()
@@ -105,24 +112,16 @@ void setUpProblem()
cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb);
// features
- obsd_t obs1{0};
- obs1.P[0] = prange_1;
- ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, sat1);
- obsd_t obs2{0};
- obs2.P[0] = prange_2;
- ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, sat2);
- obsd_t obs3{0};
- obs3.P[0] = prange_3;
- ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, sat3);
- obsd_t obs4{0};
- obs4.P[0] = prange_4;
- ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, sat4);
+ ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, prange1); // obsd_t data is not used in pseudo range factors
+ ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat2, prange2); // obsd_t data is not used in pseudo range factors
+ ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat3, prange3); // obsd_t data is not used in pseudo range factors
+ ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat4, prange4); // obsd_t data is not used in pseudo range factors
// factors
- fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false);
- fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, Combination::CODE_L1, 0.1, ftr2, gnss_sensor, nullptr, false);
- fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, Combination::CODE_L1, 0.1, ftr3, gnss_sensor, nullptr, false);
- fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, Combination::CODE_L1, 0.1, ftr4, gnss_sensor, nullptr, false);
+ fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false);
+ fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false);
+ fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false);
+ fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false);
// ASSERTS
// ENU-MAP
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index 41f195537055d0e6f6b84c43f6941e98abd1283c..c849714057797f46bca482a59051275960e3756c 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -17,17 +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;
-double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
+//double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
Vector1d clock_drift_r, clock_drift_k;
-GnssUtils::Satellite sat1_r(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);
+GnssUtils::Satellite sat1_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat1_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::PseudoRange prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
// WOLF
ProblemPtr prb = Problem::create("PO", 3);
@@ -68,28 +69,45 @@ void randomGroundtruth()
t_ecef_antena_k = R_ecef_enu * (R_enu_map * (q_map_base_k * t_base_antena + t_map_base_k) + t_enu_map ) + t_ecef_enu;
// SATELLITES
- 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;
+ 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 = (sat1_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange2_r = (sat2_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange3_r = (sat3_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange4_r = (sat4_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange1_k = (sat1_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange2_k = (sat2_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange3_k = (sat3_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange4_k = (sat4_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
+// prange1_r = (sat1_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+// prange2_r = (sat2_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+// prange3_r = (sat3_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+// prange4_r = (sat4_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+// prange1_k = (sat1_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+// prange2_k = (sat2_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+// prange3_k = (sat3_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+// prange4_k = (sat4_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+
+ prange1_r.prange = (sat1_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange2_r.prange = (sat2_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange3_r.prange = (sat3_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange4_r.prange = (sat4_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange1_k.prange = (sat1_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange2_k.prange = (sat2_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange3_k.prange = (sat3_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange4_k.prange = (sat4_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange1_r.var = 1;
+ prange2_r.var = 1;
+ prange3_r.var = 1;
+ prange4_r.var = 1;
+ prange1_k.var = 1;
+ prange2_k.var = 1;
+ prange3_k.var = 1;
+ prange4_k.var = 1;
}
void setUpProblem()
@@ -135,31 +153,31 @@ void setUpProblem()
// features r
obsd_t obs1_r{0};
- obs1_r.L[0] = prange1_r;
- ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, sat1_r);
+ obs1_r.L[0] = prange1_r.prange;
+ ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, sat1_r, prange1_r);
obsd_t obs2_r{0};
- obs2_r.L[0] = prange2_r;
- ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, sat2_r);
+ obs2_r.L[0] = prange2_r.prange;
+ ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, sat2_r, prange2_r);
obsd_t obs3_r{0};
- obs3_r.L[0] = prange3_r;
- ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, sat3_r);
+ obs3_r.L[0] = prange3_r.prange;
+ ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, sat3_r, prange3_r);
obsd_t obs4_r{0};
- obs4_r.L[0] = prange4_r;
- ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, sat4_r);
+ obs4_r.L[0] = prange4_r.prange;
+ ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, sat4_r, prange4_r);
// features k
obsd_t obs1_k{0};
- obs1_k.L[0] = prange1_k;
- ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, sat1_k);
+ obs1_k.L[0] = prange1_k.prange;
+ ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, sat1_k, prange1_k);
obsd_t obs2_k{0};
- obs2_k.L[0] = prange2_k;
- ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, sat2_k);
+ obs2_k.L[0] = prange2_k.prange;
+ ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, sat2_k, prange2_k);
obsd_t obs3_k{0};
- obs3_k.L[0] = prange3_k;
- ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, sat3_k);
+ obs3_k.L[0] = prange3_k.prange;
+ ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, sat3_k, prange3_k);
obsd_t obs4_k{0};
- obs4_k.L[0] = prange4_k;
- ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, sat4_k);
+ obs4_k.L[0] = prange4_k.prange;
+ ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, sat4_k, prange4_k);
// factors
fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);