Skip to content
Snippets Groups Projects
Commit c2649bd4 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapted to new Satellite class

parent 7fff19a2
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
Showing
with 104 additions and 66 deletions
include/gnss/capture/capture_gnss.h
+ 6
− 6
View file @ c2649bd4
...@@ -25,8 +25,8 @@ class CaptureGnss : public CaptureBase ...@@ -25,8 +25,8 @@ class CaptureGnss : public CaptureBase
GnssUtils::SnapshotPtr getSnapshot() const; GnssUtils::SnapshotPtr getSnapshot() const;
GnssUtils::ObservationsPtr getObservations() const; GnssUtils::ObservationsPtr getObservations() const;
GnssUtils::NavigationPtr getNavigation() const; GnssUtils::NavigationPtr getNavigation() const;
GnssUtils::SatellitesPositions& getSatellitesPositions(); GnssUtils::Satellites& getSatellites();
const GnssUtils::SatellitesPositions& getSatellitesPositions() const; const GnssUtils::Satellites& getSatellites() const;
}; };
...@@ -45,14 +45,14 @@ inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const ...@@ -45,14 +45,14 @@ inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
return snapshot_->getNavigation(); 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 } //namespace wolf
......
include/gnss/factor/factor_gnss_pseudo_range.h
+ 2
− 2
View file @ c2649bd4
...@@ -50,9 +50,9 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, ...@@ -50,9 +50,9 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
sensor_gnss_ptr_(_sensor_gnss_ptr), sensor_gnss_ptr_(_sensor_gnss_ptr),
mode_(_mode), mode_(_mode),
std_dev_(_std_dev), 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 // Pseudo range combination
switch (mode_) switch (mode_)
......
include/gnss/factor/factor_gnss_tdcp.h
+ 3
− 3
View file @ c2649bd4
...@@ -55,14 +55,14 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 ...@@ -55,14 +55,14 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
sensor_gnss_ptr_(_sensor_gnss_ptr), sensor_gnss_ptr_(_sensor_gnss_ptr),
mode_(_mode), mode_(_mode),
std_dev_(_std_dev), std_dev_(_std_dev),
satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->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->getSatellitePosition() + 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 != _ftr_k);
assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr); assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr);
assert(_ftr_k->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 // Pseudo range combination
// TODO encapsular prange a GnssUtils. // TODO encapsular prange a GnssUtils.
......
include/gnss/feature/feature_gnss_satellite.h
+ 8
− 7
View file @ c2649bd4
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
//std includes //std includes
#include "gnss_utils/gnss_utils.h" #include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/satellite.h"
namespace wolf { namespace wolf {
...@@ -24,17 +25,17 @@ class FeatureGnssSatellite : public FeatureBase ...@@ -24,17 +25,17 @@ class FeatureGnssSatellite : public FeatureBase
* This constructor will take the pseudorange as measurement with 1 m² of variance * 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(){}; virtual ~FeatureGnssSatellite(){};
const obsd_t& getObservation() const; const obsd_t& getObservation() const;
int satNumber() const; int satNumber() const;
const Eigen::Vector3d& getSatellitePosition() const; const GnssUtils::Satellite& getSatellite() const;
private: private:
obsd_t obs_sat_; obsd_t obs_sat_;
Eigen::Vector3d sat_pos_; GnssUtils::Satellite sat_;
}; };
...@@ -43,10 +44,10 @@ class FeatureGnssSatellite : public FeatureBase ...@@ -43,10 +44,10 @@ class FeatureGnssSatellite : public FeatureBase
// IMPLEMENTATION // IMPLEMENTATION
namespace wolf { 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)), FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_obs_sat.P[0]), Eigen::Matrix1d(1.0)),
obs_sat_(_obs_sat), obs_sat_(_obs_sat),
sat_pos_(_sat_pos) sat_(_sat)
{ {
// //
} }
...@@ -61,9 +62,9 @@ inline int FeatureGnssSatellite::satNumber() const ...@@ -61,9 +62,9 @@ inline int FeatureGnssSatellite::satNumber() const
return obs_sat_.sat; 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 } // namespace wolf
......
include/gnss/processor/processor_tracker_gnss.h
+ 4
− 0
View file @ c2649bd4
...@@ -17,12 +17,14 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature ...@@ -17,12 +17,14 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
prcopt_t gnss_opt; prcopt_t gnss_opt;
double enu_map_init_dist_min; double enu_map_init_dist_min;
double max_time_span; double max_time_span;
bool remove_outliers;
ParamsProcessorTrackerGnss() = default; ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server): ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorTrackerFeature(_unique_name, _server) ParamsProcessorTrackerFeature(_unique_name, _server)
{ {
enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min"); 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"); max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
// GNSS OPTIONS (see rtklib.h) // GNSS OPTIONS (see rtklib.h)
...@@ -53,6 +55,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature ...@@ -53,6 +55,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
{ {
return "\n" + ParamsProcessorBase::print() + "\n" return "\n" + ParamsProcessorBase::print() + "\n"
+ "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\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" + "mode: " + std::to_string(gnss_opt.mode) + "\n"
+ "soltype: " + std::to_string(gnss_opt.soltype) + "\n" + "soltype: " + std::to_string(gnss_opt.soltype) + "\n"
+ "nf: " + std::to_string(gnss_opt.nf) + "\n" + "nf: " + std::to_string(gnss_opt.nf) + "\n"
......
src/processor/processor_tracker_gnss.cpp
+ 32
− 10
View file @ c2649bd4
...@@ -13,8 +13,8 @@ void ProcessorTrackerGnss::preProcess() ...@@ -13,8 +13,8 @@ void ProcessorTrackerGnss::preProcess()
GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot(); GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
// compute satellites positions // compute satellites positions
if (!inc_snapshot ->satellitesPositionsComputed()) if (!inc_snapshot ->satellitesComputed())
inc_snapshot ->computeSatellitesPositions(params_tracker_gnss_->gnss_opt.sateph); inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph);
// Compute (and store) auxiliar fix (for computing elevation) // Compute (and store) auxiliar fix (for computing elevation)
GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(), GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(),
...@@ -31,6 +31,8 @@ void ProcessorTrackerGnss::preProcess() ...@@ -31,6 +31,8 @@ void ProcessorTrackerGnss::preProcess()
sensor_gnss_->setEcefEnu(fix_output.pos,true); sensor_gnss_->setEcefEnu(fix_output.pos,true);
//sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true); //sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true);
WOLF_INFO("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef()); 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()); WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose());
...@@ -47,7 +49,7 @@ void ProcessorTrackerGnss::preProcess() ...@@ -47,7 +49,7 @@ void ProcessorTrackerGnss::preProcess()
// create features pseudorange // create features pseudorange
for (auto obs_sat : inc_snapshot->getObservations()->getObservations()) 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); assert(untracked_incoming_features_.count(feat->satNumber()) == 0);
untracked_incoming_features_[feat->satNumber()] = feat; untracked_incoming_features_[feat->satNumber()] = feat;
} }
...@@ -134,11 +136,7 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -134,11 +136,7 @@ void ProcessorTrackerGnss::establishFactors()
FactorBasePtrList new_factors; FactorBasePtrList new_factors;
if (last_ptr_->getStateBlock("T") == nullptr) bool last_clock_drift_set = false;
{
WOLF_DEBUG("last clock offset state block nullptr, adding it");
last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(1), getProblem());
}
// PSEUDO RANGE FACTORS (all sats) // PSEUDO RANGE FACTORS (all sats)
for (auto ftr : last_ptr_->getFeatureList()) for (auto ftr : last_ptr_->getFeatureList())
...@@ -151,6 +149,19 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -151,6 +149,19 @@ void ProcessorTrackerGnss::establishFactors()
if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination) if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
continue; 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 // emplace a pseudo range factor
auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat, auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
GnssUtils::Combination::CODE_L1,//TODO: from params GnssUtils::Combination::CODE_L1,//TODO: from params
...@@ -196,6 +207,16 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -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()); // 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 // // emplace tdcp factor
// auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k, // auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
// GnssUtils::Combination::CARRIER_L1,//TODO: from params // GnssUtils::Combination::CARRIER_L1,//TODO: from params
...@@ -216,7 +237,8 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -216,7 +237,8 @@ void ProcessorTrackerGnss::establishFactors()
// } // }
// remove outliers // 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() void ProcessorTrackerGnss::advanceDerived()
...@@ -279,7 +301,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas ...@@ -279,7 +301,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr); WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
// discard if residual too high evaluated at the current estimation // 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_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose()); WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
......
test/gtest_factor_gnss_pseudo_range.cpp
+ 16
− 13
View file @ c2649bd4
...@@ -17,9 +17,12 @@ Vector3d t_ecef_antena; ...@@ -17,9 +17,12 @@ Vector3d t_ecef_antena;
Vector3d rpy_enu_map; Vector3d rpy_enu_map;
Matrix3d R_ecef_enu, R_enu_map; Matrix3d R_ecef_enu, R_enu_map;
Quaterniond q_map_base; 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; double prange_1, prange_2, prange_3, prange_4;
Vector1d clock_drift; 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 // WOLF
ProblemPtr prb = Problem::create("PO", 3); ProblemPtr prb = Problem::create("PO", 3);
...@@ -57,19 +60,19 @@ void randomGroundtruth() ...@@ -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; 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 // SATELLITES
t_ecef_sat1 = Vector3d::Random() * 1e4; sat1.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat2 = Vector3d::Random() * 1e4; sat2.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat3 = Vector3d::Random() * 1e4; sat3.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat4 = Vector3d::Random() * 1e4; sat4.pos_ = Vector3d::Random() * 1e4;
// clock drift // clock drift
clock_drift = Vector1d::Random() * 1e2; clock_drift = Vector1d::Random() * 1e2;
// pseudo ranges // pseudo ranges
prange_1 = (t_ecef_sat1-t_ecef_antena).norm() + clock_drift(0); prange_1 = (sat1.pos_-t_ecef_antena).norm() + clock_drift(0);
prange_2 = (t_ecef_sat2-t_ecef_antena).norm() + clock_drift(0); prange_2 = (sat2.pos_-t_ecef_antena).norm() + clock_drift(0);
prange_3 = (t_ecef_sat3-t_ecef_antena).norm() + clock_drift(0); prange_3 = (sat3.pos_-t_ecef_antena).norm() + clock_drift(0);
prange_4 = (t_ecef_sat4-t_ecef_antena).norm() + clock_drift(0); prange_4 = (sat4.pos_-t_ecef_antena).norm() + clock_drift(0);
} }
void setUpProblem() void setUpProblem()
...@@ -104,16 +107,16 @@ void setUpProblem() ...@@ -104,16 +107,16 @@ void setUpProblem()
// features // features
obsd_t obs1{0}; obsd_t obs1{0};
obs1.P[0] = prange_1; 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}; obsd_t obs2{0};
obs2.P[0] = prange_2; 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}; obsd_t obs3{0};
obs3.P[0] = prange_3; 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}; obsd_t obs4{0};
obs4.P[0] = prange_4; obs4.P[0] = prange_4;
ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4); ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, sat4);
// factors // factors
fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false); fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false);
......
test/gtest_factor_gnss_tdcp.cpp
+ 33
− 25
View file @ c2649bd4
...@@ -17,10 +17,18 @@ Vector3d t_ecef_antena_r, t_ecef_antena_k; ...@@ -17,10 +17,18 @@ Vector3d t_ecef_antena_r, t_ecef_antena_k;
Vector3d rpy_enu_map; Vector3d rpy_enu_map;
Matrix3d R_ecef_enu, R_enu_map; Matrix3d R_ecef_enu, R_enu_map;
Quaterniond q_map_base_r, q_map_base_k; 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; double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
Vector1d clock_drift_r, clock_drift_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 // WOLF
ProblemPtr prb = Problem::create("PO", 3); ProblemPtr prb = Problem::create("PO", 3);
CeresManagerPtr solver = std::make_shared<CeresManager>(prb); CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
...@@ -60,28 +68,28 @@ void randomGroundtruth() ...@@ -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; 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 // SATELLITES
t_ecef_sat1_r = Vector3d::Random() * 1e4; sat1_r.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat2_r = Vector3d::Random() * 1e4; sat2_r.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat3_r = Vector3d::Random() * 1e4; sat3_r.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat4_r = Vector3d::Random() * 1e4; sat4_r.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat1_k = Vector3d::Random() * 1e4; sat1_k.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat2_k = Vector3d::Random() * 1e4; sat2_k.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat3_k = Vector3d::Random() * 1e4; sat3_k.pos_ = Vector3d::Random() * 1e4;
t_ecef_sat4_k = Vector3d::Random() * 1e4; sat4_k.pos_ = Vector3d::Random() * 1e4;
// clock drift // clock drift
clock_drift_r = Vector1d::Random() * 1e2; clock_drift_r = Vector1d::Random() * 1e2;
clock_drift_k = Vector1d::Random() * 1e2; clock_drift_k = Vector1d::Random() * 1e2;
// pseudo ranges // pseudo ranges
prange1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0); prange1_r = (sat1_r.pos_-t_ecef_antena_r).norm() + clock_drift_r(0);
prange2_r = (t_ecef_sat2_r-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 = (t_ecef_sat3_r-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 = (t_ecef_sat4_r-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 = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0); prange1_k = (sat1_k.pos_-t_ecef_antena_k).norm() + clock_drift_k(0);
prange2_k = (t_ecef_sat2_k-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 = (t_ecef_sat3_k-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 = (t_ecef_sat4_k-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() void setUpProblem()
...@@ -128,30 +136,30 @@ void setUpProblem() ...@@ -128,30 +136,30 @@ void setUpProblem()
// features r // features r
obsd_t obs1_r{0}; obsd_t obs1_r{0};
obs1_r.L[0] = prange1_r; 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}; obsd_t obs2_r{0};
obs2_r.L[0] = prange2_r; 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}; obsd_t obs3_r{0};
obs3_r.L[0] = prange3_r; 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}; obsd_t obs4_r{0};
obs4_r.L[0] = prange4_r; 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 // features k
obsd_t obs1_k{0}; obsd_t obs1_k{0};
obs1_k.L[0] = prange1_k; 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}; obsd_t obs2_k{0};
obs2_k.L[0] = prange2_k; 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}; obsd_t obs3_k{0};
obs3_k.L[0] = prange3_k; 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}; obsd_t obs4_k{0};
obs4_k.L[0] = prange4_k; 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 // factors
fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false); fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment