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

ProcessorTrackerGnss: pseudoranges working

parent 17cd990d
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
......@@ -13,11 +13,13 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorGnssPseudoRange);
class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1>
class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
Eigen::Vector3d satellite_ENU_;
Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
double sagnac_correction_;
bool not_GPS_;
public:
......@@ -26,7 +28,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
nullptr,
nullptr,
nullptr,
......@@ -37,16 +39,31 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getStateBlock("T"),
(_ftr_ptr->getSatellite().sys == SYS_GLO ?
_ftr_ptr->getCapture()->getStateBlock("TG") :
(_ftr_ptr->getSatellite().sys == SYS_GAL ?
_ftr_ptr->getCapture()->getStateBlock("TE") :
_ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used
_sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr),
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()),
satellite_ECEF_(_ftr_ptr->getSatellite().pos),
not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS)
{
//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");
/* SAGNAC EFFECT CORRECTION
* It should be recomputed with the antenna position in ECEF coordinates,
* but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
* We use instead a precomputed sagnac effect taking ENU origin as antenna position.
*/
Eigen::Vector3d t_ECEF_ENU = -sensor_gnss_ptr_->getREnuEcef().transpose() * sensor_gnss_ptr_->gettEnuEcef();
sagnac_correction_ = OMGE*(satellite_ECEF_(0)*t_ECEF_ENU(1)-satellite_ECEF_(1)*t_ECEF_ENU(0))/CLIGHT;// change to antenna_ECEF
}
virtual ~FactorGnssPseudoRange() = default;
......@@ -60,6 +77,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
bool operator ()(const T* const _x,
const T* const _o,
const T* const _clock_bias,
const T* const _clock_bias_constellation,
const T* const _x_antena,
const T* const _t_ENU_map,
const T* const _roll_ENU_map,
......@@ -73,6 +91,7 @@ template<typename T>
inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
const T* const _o,
const T* const _clock_bias,
const T* const _clock_bias_constellation,
const T* const _x_antena,
const T* const _t_ENU_map,
const T* const _roll_ENU_map,
......@@ -90,17 +109,27 @@ 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_bias[0];
T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() + // norm
sagnac_correction_ + // sagnac correction (precomputed)
_clock_bias[0] + // receiver clock bias (w.r.t. GPS clock)
(not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias
//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";
//std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
//std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl;
//std::cout << std::setprecision(13) <<"\tsat_pos= " << satellite_ENU_.transpose() << "\n";
//std::cout << "\tx = " << antena_ENU(0) << " "<< antena_ENU(1) << " "<< antena_ENU(2) << "\n";
//std::cout << "\tprange = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().p << "\n";
//std::cout << "\tiono_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().iono_corr << "\n";
//std::cout << "\ttropo_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().tropo_corr << "\n";
//std::cout << "\tsat_clock_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().sat_clock_corr << "\n";
//std::cout << "\tcorrected prange = " << getMeasurement() << "\n";
//std::cout << "\tclock_bias = " << _clock_bias[0] << " " << _clock_bias_constellation[0] << "\n";
//std::cout << "\tnorm = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ << "\n";
//std::cout << "\texp_wo = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ + _clock_bias[0] << "\n";
//std::cout << "\texp = " << exp << "\n";
//std::cout << "\terror = " << exp - getMeasurement()(0) << "\n";
// Residual
_residual[0] = (exp - getMeasurement()(0)) / getMeasurementSquareRootInformationUpper()(0,0);
......
......@@ -18,6 +18,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
double enu_map_init_dist_min;
double max_time_span;
bool remove_outliers;
bool init_frames;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
......@@ -25,7 +26,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
{
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");
init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames");
remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
// GNSS OPTIONS (see rtklib.h)
gnss_opt = prcopt_default;
......@@ -56,6 +58,7 @@ 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"
+ "init_frames: " + std::to_string(init_frames) + "\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"
......@@ -186,13 +189,6 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
};
inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
params_tracker_gnss_(_params_tracker_gnss)
{
//
}
inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
{
//
......
......@@ -8,8 +8,16 @@
namespace wolf
{
ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
params_tracker_gnss_(_params_tracker_gnss)
{
}
void ProcessorTrackerGnss::preProcess()
{
WOLF_INFO("ProcessorTrackerGnss::preProcess");
GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
// compute satellites positions
......@@ -24,12 +32,11 @@ void ProcessorTrackerGnss::preProcess()
// Set ECEF-ENU
if (!sensor_gnss_->isEnuDefined() and fix_incoming_.stat != 0)
{
WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
WOLF_INFO("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_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
WOLF_INFO("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
......@@ -38,13 +45,15 @@ void ProcessorTrackerGnss::preProcess()
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
WOLF_INFO("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);
WOLF_INFO("preprocess: pseudo ranges computed");
// create features pseudorange
for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
{
......@@ -56,7 +65,7 @@ void ProcessorTrackerGnss::preProcess()
untracked_incoming_features_[feat->satNumber()] = feat;
}
WOLF_DEBUG("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
}
unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in,
......@@ -64,7 +73,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences)
{
WOLF_DEBUG("tracking " , _features_in.size() , " features...");
WOLF_INFO("tracking " , _features_in.size() , " features...");
assert(_capture == incoming_ptr_);
......@@ -81,17 +90,19 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
_features_out.push_back(ftr);
_feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
WOLF_DEBUG("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
}
}
WOLF_DEBUG("After tracking:\n\ttracked features: " , _features_out.size());
WOLF_INFO("After tracking:\n\ttracked features: " , _features_out.size());
return _features_out.size();
}
bool ProcessorTrackerGnss::voteForKeyFrame() const
{
//WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame");
// too old origin
if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
{
......@@ -100,7 +111,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
}
// known features
WOLF_DEBUG("Nbr. of active feature tracks: " , known_features_incoming_.size() );
WOLF_INFO("Nbr. of active feature tracks: " , known_features_incoming_.size() );
if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe)
{
WOLF_INFO( "Vote for KF because of too less known_features_incoming" );
......@@ -117,6 +128,8 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out)
{
WOLF_INFO("ProcessorTrackerGnss::detectNewFeatures");
assert(_capture == last_ptr_);
for (auto feat_pair : untracked_last_features_)
......@@ -125,15 +138,22 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
break;
_features_out.push_back(feat_pair.second);
WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" );
WOLF_INFO("feature " , feat_pair.second->id() , " detected!" );
}
WOLF_DEBUG(_features_out.size() , " features detected!");
WOLF_INFO(_features_out.size() , " features detected!");
return _features_out.size();
}
void ProcessorTrackerGnss::establishFactors()
{
WOLF_INFO("ProcessorTrackerGnss::establishFactors");
// initialize frame state with antenna position in map coordinates
// (since we don't have orientation for removing extrinsics)
if (params_tracker_gnss_->init_frames)
last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
FactorBasePtrList new_factors;
bool last_clock_bias_set = false;
......@@ -145,7 +165,7 @@ void ProcessorTrackerGnss::establishFactors()
if (ftr_sat == nullptr)
continue;
// check valid measurement
// Check valid measurement
if (ftr_sat->getMeasurement()(0) < 1e-12)
{
WOLF_INFO("Feature with not valid pseudorange");
......@@ -156,25 +176,40 @@ void ProcessorTrackerGnss::establishFactors()
if (!last_clock_bias_set)
{
if (last_ptr_->getStateBlock("T") == nullptr)
last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))), getProblem());
{
last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem());
last_ptr_->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true), getProblem());
last_ptr_->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), true), getProblem());
last_ptr_->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true), getProblem());
}
else
last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
//WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState().transpose());
{
last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
last_ptr_->getStateBlock("TG")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)));
last_ptr_->getStateBlock("TE")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
}
//WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
last_clock_bias_set = true;
}
// emplace a pseudo range factor
// unfix clock bias inter-constellation
if (ftr_sat->getSatellite().sys == SYS_GLO)
last_ptr_->getStateBlock("TG")->unfix();
if (ftr_sat->getSatellite().sys == SYS_GAL)
last_ptr_->getStateBlock("TE")->unfix();
if (ftr_sat->getSatellite().sys == SYS_CMP)
last_ptr_->getStateBlock("TC")->unfix();
// Emplace a pseudo range factor
auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
ftr_sat,
sensor_gnss_,
shared_from_this(),
params_->apply_loss_function);
new_factors.push_back(new_fac);
WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
WOLF_INFO("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
}
// TDCP FACTORS (tracked sats)
......@@ -187,7 +222,7 @@ void ProcessorTrackerGnss::establishFactors()
// auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
// assert(ftr_k != nullptr);
//
// WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
// WOLF_INFO("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
//
// // emplace a tdcp factor from last to each KF
// for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
......@@ -202,11 +237,11 @@ void ProcessorTrackerGnss::establishFactors()
//
// if (ftr_k == ftr_r)
// {
// WOLF_DEBUG("same KF and feature, skipping...");
// WOLF_INFO("same KF and feature, skipping...");
// break;
// }
//
// WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
// WOLF_INFO("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
//
// // Initialize (and create) clock bias stateblock in capture
// if (!last_clock_bias_set)
......@@ -229,17 +264,21 @@ void ProcessorTrackerGnss::establishFactors()
// params_->apply_loss_function);
// new_factors.push_back(new_fac);
//
// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
// // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(),
// // " origin: " , feature_in_origin->id() ,
// // " from last: " , feature_in_last->id() );
// }
// WOLF_DEBUG("All TDCP factors emplaced!");
// WOLF_INFO("All TDCP factors emplaced!");
// }
// }
// remove outliers
if (!new_factors.empty() and params_tracker_gnss_->remove_outliers)
removeOutliers(new_factors, last_ptr_);
//getProblem()->print(4, 1, 1, 1);
WOLF_INFO("ProcessorTrackerGnss::establishFactors done");
}
void ProcessorTrackerGnss::advanceDerived()
......@@ -260,19 +299,23 @@ void ProcessorTrackerGnss::resetDerived()
void ProcessorTrackerGnss::postProcess()
{
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
{
WOLF_INFO("ProcessorTrackerGnss::removeOutliers");
FactorBasePtrList remove_fac;
//WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
//WOLF_INFO( "PR ", getName()," rejectOutlier...");
// copy states
Eigen::Vector3d x(cap->getFrame()->getP()->getState());
Eigen::Vector4d o(cap->getFrame()->getO()->getState());
Eigen::Vector1d clock_bias(-cap->getStateBlock("T")->getState());
Eigen::Vector1d clock_bias(cap->getStateBlock("T")->getState());
Eigen::Vector1d clock_bias_glo(cap->getStateBlock("TG")->getState());
Eigen::Vector1d clock_bias_gal(cap->getStateBlock("TE")->getState());
Eigen::Vector1d clock_bias_cmp(cap->getStateBlock("TC")->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());
......@@ -289,14 +332,34 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
//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_bias.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
double* parameters_glo_pr[9] = {x.data(),
o.data(),
clock_bias.data(),
clock_bias_glo.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
double* parameters_gal_pr[9] = {x.data(),
o.data(),
clock_bias.data(),
clock_bias_gal.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
double* parameters_cmp_pr[9] = {x.data(),
o.data(),
clock_bias.data(),
clock_bias_cmp.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
// create residuals
double residual_pr;
......@@ -307,7 +370,21 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
if (fac_pr)
{
// evaluate the factor in this state
fac_pr->evaluate(parameters_pr, &residual_pr, nullptr);
switch (std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sys)
{
case SYS_GLO:
fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr);
break;
case SYS_GAL:
fac_pr->evaluate(parameters_gal_pr, &residual_pr, nullptr);
break;
case SYS_CMP:
fac_pr->evaluate(parameters_cmp_pr, &residual_pr, nullptr);
break;
default:
fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr);
break;
}
//WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
......@@ -325,6 +402,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
// remove outliers
for (auto fac : remove_fac)
fac->remove();
WOLF_INFO("ProcessorTrackerGnss::removeOutliers done");
}
} // namespace wolf
......
......@@ -5,15 +5,14 @@
* \author: jvallve
*/
#include "gnss/factor/factor_gnss_fix_2d.h"
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss_fix.h"
#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
using namespace Eigen;
using namespace wolf;
......
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/capture/capture_gnss.h"
#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
using namespace Eigen;
using namespace wolf;
......@@ -110,6 +109,9 @@ void setUpProblem()
// capture
cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb);
cap->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
cap->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
cap->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
// features
ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, prange1); // obsd_t data is not used in pseudo range factors
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment