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

changed state blocks keys of more than one char

parent 459c065d
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
include/gnss/factor/factor_gnss_pseudo_range.h
+ 3
− 3
View file @ f5667b33
...@@ -42,10 +42,10 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, ...@@ -42,10 +42,10 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getStateBlock("T"), _ftr_ptr->getCapture()->getStateBlock("T"),
(_ftr_ptr->getSatellite().sys == SYS_GLO ? (_ftr_ptr->getSatellite().sys == SYS_GLO ?
_ftr_ptr->getCapture()->getStateBlock("TG") : _ftr_ptr->getCapture()->getStateBlock("G") :
(_ftr_ptr->getSatellite().sys == SYS_GAL ? (_ftr_ptr->getSatellite().sys == SYS_GAL ?
_ftr_ptr->getCapture()->getStateBlock("TE") : _ftr_ptr->getCapture()->getStateBlock("E") :
_ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used _ftr_ptr->getCapture()->getStateBlock("M"))),//in case GPS, M is set but anyway not used
_sensor_gnss_ptr->getP(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(), _sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getEnuMapRoll(),
......
src/processor/processor_tracker_gnss.cpp
+ 15
− 15
View file @ f5667b33
...@@ -194,29 +194,29 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -194,29 +194,29 @@ void ProcessorTrackerGnss::establishFactors()
if (last_ptr_->getStateBlock("T") == nullptr) if (last_ptr_->getStateBlock("T") == nullptr)
{ {
last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), 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("G", 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("E", 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()); last_ptr_->addStateBlock("M", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true), getProblem());
//WOLF_INFO("last clock bias set: ", last_ptr_->getStateBlock("T")->getState()); //WOLF_INFO("last clock bias set: ", last_ptr_->getStateBlock("T")->getState());
} }
// Initialize clock bias stateblocks in capture // Initialize clock bias stateblocks in capture
else if (!last_clock_bias_init) else if (!last_clock_bias_init)
{ {
last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))); 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("G")->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("E")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3))); last_ptr_->getStateBlock("M")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
//WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState()); //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
last_clock_bias_init = true; last_clock_bias_init = true;
} }
// unfix clock bias inter-constellation // unfix clock bias inter-constellation
if (ftr_sat->getSatellite().sys == SYS_GLO) if (ftr_sat->getSatellite().sys == SYS_GLO)
last_ptr_->getStateBlock("TG")->unfix(); last_ptr_->getStateBlock("G")->unfix();
if (ftr_sat->getSatellite().sys == SYS_GAL) if (ftr_sat->getSatellite().sys == SYS_GAL)
last_ptr_->getStateBlock("TE")->unfix(); last_ptr_->getStateBlock("E")->unfix();
if (ftr_sat->getSatellite().sys == SYS_CMP) if (ftr_sat->getSatellite().sys == SYS_CMP)
last_ptr_->getStateBlock("TC")->unfix(); last_ptr_->getStateBlock("M")->unfix();
// 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,
...@@ -373,9 +373,9 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas ...@@ -373,9 +373,9 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
//std::cout << "frame p: " << cap->getFrame()->getP()->getState().transpose() << std::endl; //std::cout << "frame p: " << cap->getFrame()->getP()->getState().transpose() << std::endl;
//std::cout << "frame o: " << cap->getFrame()->getO()->getState().transpose() << std::endl; //std::cout << "frame o: " << cap->getFrame()->getO()->getState().transpose() << std::endl;
//std::cout << "sb clock: " << cap->getStateBlock("T")->getState() << std::endl; //std::cout << "sb clock: " << cap->getStateBlock("T")->getState() << std::endl;
//std::cout << "sb clock glo: " << cap->getStateBlock("TG")->getState() << std::endl; //std::cout << "sb clock glo: " << cap->getStateBlock("G")->getState() << std::endl;
//std::cout << "sb clock gal: " << cap->getStateBlock("TE")->getState() << std::endl; //std::cout << "sb clock gal: " << cap->getStateBlock("E")->getState() << std::endl;
//std::cout << "sb clock cmp: " << cap->getStateBlock("TC")->getState() << std::endl; //std::cout << "sb clock cmp: " << cap->getStateBlock("M")->getState() << std::endl;
//std::cout << "sb antena: " << sensor_gnss_->getP()->getState().transpose() << std::endl; //std::cout << "sb antena: " << sensor_gnss_->getP()->getState().transpose() << std::endl;
} }
else else
...@@ -383,9 +383,9 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas ...@@ -383,9 +383,9 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
x = cap->getFrame()->getP()->getState(); x = cap->getFrame()->getP()->getState();
o = cap->getFrame()->getO()->getState(); o = cap->getFrame()->getO()->getState();
clock_bias = cap->getStateBlock("T")->getState(); clock_bias = cap->getStateBlock("T")->getState();
clock_bias_glo = cap->getStateBlock("TG")->getState(); clock_bias_glo = cap->getStateBlock("G")->getState();
clock_bias_gal = cap->getStateBlock("TE")->getState(); clock_bias_gal = cap->getStateBlock("E")->getState();
clock_bias_cmp = cap->getStateBlock("TC")->getState(); clock_bias_cmp = cap->getStateBlock("M")->getState();
x_antena_pr = sensor_gnss_->getP()->getState(); x_antena_pr = sensor_gnss_->getP()->getState();
} }
......
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