From bf092839d08ff523f91681b864f83f6585630671 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sun, 14 Jun 2020 19:21:12 +0200
Subject: [PATCH] clock biases in sensorGnss
---
include/gnss/sensor/sensor_gnss.h | 41 +++++++++++++------
src/capture/capture_gnss.cpp | 16 +++++++-
src/processor/processor_tracker_gnss.cpp | 52 +++++++++---------------
src/sensor/sensor_gnss.cpp | 15 +++++--
test/gtest_factor_gnss_pseudo_range.cpp | 6 +--
test/gtest_factor_gnss_tdcp.cpp | 6 ++-
6 files changed, 80 insertions(+), 56 deletions(-)
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 3b8fcccbd..96d9dc986 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -8,27 +8,41 @@
namespace wolf {
+static std::string CLOCK_BIAS_KEY = "T";
+static std::string CLOCK_BIAS_GPS_GLO_KEY = "G";
+static std::string CLOCK_BIAS_GPS_GAL_KEY = "E";
+static std::string CLOCK_BIAS_GPS_CMP_KEY = "M";
+
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorGnss);
WOLF_PTR_TYPEDEFS(SensorGnss);
struct ParamsSensorGnss : public ParamsSensorBase
{
- // add GNSS parameters here
- std::string ENU_mode;
+ // extrinsics and intrinsics
bool extrinsics_fixed;
+ bool clock_bias_GPS_GLO_dynamic;
+ bool clock_bias_GPS_GAL_dynamic;
+ bool clock_bias_GPS_CMP_dynamic;
+
+ // ENU
+ std::string ENU_mode;
bool roll_fixed = false;
bool pitch_fixed = false;
bool yaw_fixed = false;
bool translation_fixed = false;
Eigen::Vector3d ENU_latlonalt = Eigen::Vector3d::Zero();
+
~ParamsSensorGnss() override = default;
ParamsSensorGnss() = default;
ParamsSensorGnss(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- extrinsics_fixed = _server.getParam<bool>(prefix + _unique_name + "/extrinsics_fixed");
- ENU_mode = _server.getParam<std::string>(prefix + _unique_name + "/ENU/mode");
+ extrinsics_fixed = _server.getParam<bool>(prefix + _unique_name + "/extrinsics_fixed");
+ clock_bias_GPS_GLO_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GLO_dynamic");
+ clock_bias_GPS_GAL_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GAL_dynamic");
+ clock_bias_GPS_CMP_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_CMP_dynamic");
+ ENU_mode = _server.getParam<std::string>(prefix + _unique_name + "/ENU/mode");
if (ENU_mode == "manual" or ENU_mode == "auto")
{
roll_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/roll_fixed");
@@ -45,14 +59,17 @@ struct ParamsSensorGnss : public ParamsSensorBase
}
std::string print() const
{
- return "\n" + ParamsSensorBase::print() + "\n"
- + "extrinsics_fixed: " + std::to_string(extrinsics_fixed) + "\n"
- + "ENU_mode: " + ENU_mode + "\n"
- + "roll_fixed: " + std::to_string(roll_fixed) + "\n"
- + "pitch_fixed: " + std::to_string(pitch_fixed ) + "\n"
- + "yaw_fixed: " + std::to_string(yaw_fixed) + "\n"
- + "translation_fixed: " + std::to_string(translation_fixed) + "\n"
- + "ENU_latlonalt: to_string not implemented yet!" + "\n";
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "extrinsics_fixed: " + std::to_string(extrinsics_fixed) + "\n"
+ + "clock_bias_GPS_GLO_dynamic: " + std::to_string(clock_bias_GPS_GLO_dynamic) + "\n"
+ + "clock_bias_GPS_GAL_dynamic: " + std::to_string(clock_bias_GPS_GAL_dynamic) + "\n"
+ + "clock_bias_GPS_CMP_dynamic: " + std::to_string(clock_bias_GPS_CMP_dynamic) + "\n"
+ + "ENU_mode: " + ENU_mode + "\n"
+ + "roll_fixed: " + std::to_string(roll_fixed) + "\n"
+ + "pitch_fixed: " + std::to_string(pitch_fixed ) + "\n"
+ + "yaw_fixed: " + std::to_string(yaw_fixed) + "\n"
+ + "translation_fixed: " + std::to_string(translation_fixed) + "\n"
+ + "ENU_latlonalt: to_string not implemented yet!" + "\n";
}
};
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
index 5e49047fc..9294f3c60 100644
--- a/src/capture/capture_gnss.cpp
+++ b/src/capture/capture_gnss.cpp
@@ -7,7 +7,21 @@ CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUt
CaptureBase("CaptureGnss", _ts, _sensor_ptr),
snapshot_(_snapshot)
{
- //
+ // Clock bias
+ assert(_sensor_ptr->getStateBlock("T") != nullptr and _sensor_ptr->isStateBlockDynamic("T"));
+ addStateBlock("T", std::make_shared<StateBlock>(1,true), nullptr);
+
+ // interconstellation clock bias
+ assert(_sensor_ptr->getStateBlock("G") != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic("G"))
+ addStateBlock("G", std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock("E") != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic("E"))
+ addStateBlock("E", std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock("M") != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic("M"))
+ addStateBlock("M", std::make_shared<StateBlock>(1,true), nullptr);
+
}
CaptureGnss::~CaptureGnss()
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 518aef0eb..969c7b89e 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -35,11 +35,24 @@ void ProcessorTrackerGnss::preProcess()
* - set ENU
* - compute azimuths and elevations
* - Take the eventual discarded sats by RAIM
+ * - initialize clock bias
**/
fix_incoming_ = GnssUtils::computePos(*inc_snapshot->getObservations(),
*inc_snapshot->getNavigation(),
params_tracker_gnss_->fix_opt);
+ // Initialize clock bias stateblocks in capture
+ if (fix_incoming_.success)
+ {
+ incoming_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(0)));
+ if (sensor_gnss_->isStateBlockDynamic("G"))
+ incoming_ptr_->getStateBlock("G")->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(1)));
+ if (sensor_gnss_->isStateBlockDynamic("E"))
+ incoming_ptr_->getStateBlock("E")->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(2)));
+ if (sensor_gnss_->isStateBlockDynamic("M"))
+ incoming_ptr_->getStateBlock("M")->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(3)));
+ }
+
// Set ECEF-ENU
if (!sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuModeAuto() and fix_incoming_.success)
{
@@ -174,8 +187,6 @@ void ProcessorTrackerGnss::establishFactors()
FactorBasePtrList new_factors;
- bool last_clock_bias_init = false;
-
// PSEUDO RANGE FACTORS (all sats)
for (auto ftr : last_ptr_->getFeatureList())
{
@@ -190,27 +201,9 @@ void ProcessorTrackerGnss::establishFactors()
continue;
}
- // Add clock bias stateblocks in capture
- 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("G", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true), getProblem());
- last_ptr_->addStateBlock("E", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), 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());
- }
- // Initialize clock bias stateblocks in capture
- else if (!last_clock_bias_init)
- {
- last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
- last_ptr_->getStateBlock("G")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)));
- last_ptr_->getStateBlock("E")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
- last_ptr_->getStateBlock("M")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
- //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
- last_clock_bias_init = true;
- }
-
- // unfix clock bias inter-constellation
+ // unfix clock bias
+ last_ptr_->getStateBlock("T")->unfix();
+ // unfix clock bias inter-constellation (if observed)
if (ftr_sat->getSatellite().sys == SYS_GLO)
last_ptr_->getStateBlock("G")->unfix();
if (ftr_sat->getSatellite().sys == SYS_GAL)
@@ -245,6 +238,9 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
+ // unfix clock bias
+ last_ptr_->getStateBlock("T")->unfix();
+
// emplace a tdcp factor from last to each KF
auto ts_ftr_r_map = track_matrix_.trackAtKeyframes(ftr_k_pair.first);
for (auto ts_ftr_r_it = ts_ftr_r_map.rbegin(); ts_ftr_r_it != ts_ftr_r_map.rend(); ts_ftr_r_it++)
@@ -274,16 +270,6 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
- // Add clock bias stateblock in capture
- if (last_ptr_->getStateBlock("T") == nullptr)
- last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem());
- // Initialize clock bias stateblock in capture
- else if (!last_clock_bias_init)
- {
- last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
- last_clock_bias_init = true;
- }
-
// emplace tdcp factor
double var_tdcp = dt * std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_atm,2) + std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_carrier,2);
auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index f709e3414..36adee111 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -23,10 +23,17 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
{
assert(_extrinsics.size() == 3 && "Bad extrinsics size");
- addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed));
- addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed));
- addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed));
- addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed));
+ // ENU
+ addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+ addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
+ addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
+ addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
+
+ // clock bias
+ addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+ addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
// Mode "manual": ENU provided via params
if (params_->ENU_mode == "manual")
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index 5ffe7c923..bc88eb28b 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -108,10 +108,8 @@ 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("G", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
- cap->addStateBlock("E", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
- cap->addStateBlock("M", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
+ cap->getStateBlock("T")->unfix();
+ cap->getStateBlock("T")->setState(clock_drift);
// features
ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, range1); // obsd_t data is not used in pseudo range factors
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index 17166ed4b..aa2d3d47f 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -128,11 +128,13 @@ void setUpProblem()
// capture r
cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr);
- cap_r->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_r, false), prb);
+ cap_r->getStateBlock("T")->unfix();
+ cap_r->getStateBlock("T")->setState(clock_drift_r);
// capture k
cap_k = CaptureBase::emplace<CaptureGnss>(frm_k, TimeStamp(1), gnss_sensor, nullptr);
- cap_k->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_k, false), prb);
+ cap_k->getStateBlock("T")->unfix();
+ cap_k->getStateBlock("T")->setState(clock_drift_k);
// features r
ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat1_r, range1_r);
--
GitLab