diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index c445a61eb439fe96cd94d8ff1c4fe6ba7509b91e..1cd9dcdf3df75e3a65e978c289b90945ed6a0b85 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -18,7 +18,7 @@ Vector3d rpy_enu_map;
Matrix3d R_ecef_enu, R_enu_map;
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, prange_2_r, prange_3_r, prange_4_r, prange1_k, prange_2_k, prange_3_k, prange_4_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;
// WOLF
@@ -74,14 +74,14 @@ void randomGroundtruth()
clock_drift_k = Vector1d::Random() * 1e2;
// pseudo ranges
- prange_1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange_2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange_3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange_4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0);
- prange_1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange_2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange_3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0);
- prange_4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0);
+ prange1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0);
+ prange4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0);
}
void setUpProblem()
@@ -109,31 +109,55 @@ void setUpProblem()
Vector7d frm_r_state;
frm_r_state.head<3>() = t_map_base_r;
frm_r_state.tail<4>() = q_map_base_r.coeffs();
- frm_r = prb->emplaceFrame(KEY, frm_state, TimeStamp(0));
-
- // capture
- cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
- 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, t_ecef_sat1);
- obsd_t obs2{0};
- obs2.P[0] = prange_2;
- ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, t_ecef_sat2);
- obsd_t obs3{0};
- obs3.P[0] = prange_3;
- ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, t_ecef_sat3);
- obsd_t obs4{0};
- obs4.P[0] = prange_4;
- ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4);
+ frm_r = prb->emplaceFrame(KEY, frm_r_state, TimeStamp(0));
+
+ // Frame k
+ Vector7d frm_k_state;
+ frm_k_state.head<3>() = t_map_base_k;
+ frm_k_state.tail<4>() = q_map_base_k.coeffs();
+ frm_k = prb->emplaceFrame(KEY, frm_k_state, TimeStamp(1));
+
+ // 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);
+
+ // 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);
+
+ // features r
+ obsd_t obs1_r{0};
+ obs1_r.L[0] = prange1_r;
+ ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, t_ecef_sat1_r);
+ obsd_t obs2_r{0};
+ obs2_r.L[0] = prange2_r;
+ ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, t_ecef_sat2_r);
+ obsd_t obs3_r{0};
+ obs3_r.L[0] = prange3_r;
+ ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, t_ecef_sat3_r);
+ obsd_t obs4_r{0};
+ obs4_r.L[0] = prange4_r;
+ ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, t_ecef_sat4_r);
+
+ // features k
+ obsd_t obs1_k{0};
+ obs1_k.L[0] = prange1_k;
+ ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, t_ecef_sat1_k);
+ obsd_t obs2_k{0};
+ obs2_k.L[0] = prange2_k;
+ ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, t_ecef_sat2_k);
+ obsd_t obs3_k{0};
+ obs3_k.L[0] = prange3_k;
+ ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, t_ecef_sat3_k);
+ obsd_t obs4_k{0};
+ obs4_k.L[0] = prange4_k;
+ ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, t_ecef_sat4_k);
// 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<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);
+ fac2 = FactorBase::emplace<FactorGnssTdcp>(ftr2_r, Combination::CARRIER_L1, 0.1, ftr2_r, ftr2_k, gnss_sensor, nullptr, false);
+ fac3 = FactorBase::emplace<FactorGnssTdcp>(ftr3_r, Combination::CARRIER_L1, 0.1, ftr3_r, ftr3_k, gnss_sensor, nullptr, false);
+ fac4 = FactorBase::emplace<FactorGnssTdcp>(ftr4_r, Combination::CARRIER_L1, 0.1, ftr4_r, ftr4_k, gnss_sensor, nullptr, false);
// ASSERTS
// ENU-MAP
@@ -143,11 +167,15 @@ void setUpProblem()
ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-4);
// Antena
ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-4);
- // Frame
- ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
- ASSERT_MATRIX_APPROX(frm->getO()->getState(), q_map_base.coeffs(), 1e-4);
- // clock drift
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
+ // Frame r
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
+ ASSERT_MATRIX_APPROX(frm_r->getO()->getState(), q_map_base_r.coeffs(), 1e-4);
+ // Frame k
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
+ ASSERT_MATRIX_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), 1e-4);
+ // clock drift
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
}
void fixAllStates()
@@ -159,14 +187,16 @@ void fixAllStates()
gnss_sensor->getEnuMapTranslation()->fix();
// Antena
gnss_sensor->getP()->fix();
- // Frame
- frm->fix();
+ // Frames
+ frm_r->fix();
+ frm_k->fix();
// clock drift
- cap->fix();
+ cap_r->fix();
+ cap_k->fix();
}
////////////////////////////////////////////////////////
-TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
+TEST(FactorGnssTdcpTest, observe_clock_drift_r)
{
for (auto i = 0; i < 100; i++)
{
@@ -176,10 +206,10 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
// fix/unfix
fixAllStates();
- cap->getStateBlock("T")->unfix();
+ cap_r->getStateBlock("T")->unfix();
// perturb
- cap->getStateBlock("T")->perturb(1e2);
+ cap_r->getStateBlock("T")->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
@@ -190,11 +220,11 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
}
}
-TEST(FactorGnssPreusoRangeTest, observe_frame_p)
+TEST(FactorGnssTdcpTest, observe_clock_drift_k)
{
for (auto i = 0; i < 100; i++)
{
@@ -204,10 +234,38 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p)
// fix/unfix
fixAllStates();
- frm->getP()->unfix();
+ cap_k->getStateBlock("T")->unfix();
// perturb
- frm->getP()->perturb(1);
+ cap_k->getStateBlock("T")->perturb(1e2);
+
+ // Only 1 factor
+ fac2->setStatus(FAC_INACTIVE);
+ fac3->setStatus(FAC_INACTIVE);
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_r)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_r->getP()->unfix();
+
+ // perturb
+ frm_r->getP()->perturb(1);
// Only 3 factors
fac4->setStatus(FAC_INACTIVE);
@@ -216,11 +274,65 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_k)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_k->getP()->unfix();
+
+ // perturb
+ frm_k->getP()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_k->getP()->unfix();
+ cap_k->getStateBlock("T")->unfix();
+
+ // perturb
+ frm_k->getP()->perturb(1);
+ cap_k->getStateBlock("T")->perturb(1e2);
+
+ // all 4 factors
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
}
}
-TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
+TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
{
for (auto i = 0; i < 100; i++)
{
@@ -230,12 +342,12 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
// fix/unfix
fixAllStates();
- frm->getP()->unfix();
- cap->getStateBlock("T")->unfix();
+ frm_r->getP()->unfix();
+ cap_r->getStateBlock("T")->unfix();
// perturb
- frm->getP()->perturb(1);
- cap->getStateBlock("T")->perturb(1e2);
+ frm_r->getP()->perturb(1);
+ cap_r->getStateBlock("T")->perturb(1e2);
// all 4 factors
@@ -243,12 +355,12 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
}
}
-TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
+TEST(FactorGnssTdcpTest, observe_enumap_p)
{
for (auto i = 0; i < 100; i++)
{
@@ -274,7 +386,7 @@ TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
}
}
-TEST(FactorGnssPreusoRangeTest, observe_enumap_o)
+TEST(FactorGnssTdcpTest, observe_enumap_o)
{
for (auto i = 0; i < 100; i++)
{