diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp index 989c47e970a98a7d1fcb08ac19b1dfad3e6f281b..7120539b8ab19fe5355afb5f1bd0156f9fb475b5 100644 --- a/test/gtest_tdcp.cpp +++ b/test/gtest_tdcp.cpp @@ -49,11 +49,146 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt, range = (sat_ECEF - t_ECEF_ENU).norm(); } - TEST(Tdcp, Tdcp) { int N_sats = 20; + TdcpBatchParams tdcp_params; + tdcp_params.max_iterations = 10; + tdcp_params.min_common_sats = 6; + tdcp_params.raim_n = 0; + tdcp_params.raim_min_residual = 0; + tdcp_params.relinearize_jacobian = true; + tdcp_params.tdcp.multi_freq = false; + + Vector3d sat_ENU, sat_ECEF; + Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF; + Matrix3d R_ENU_ECEF; + Vector2d azel, azel2; + Vector4d d, d_gt; + Matrix4d cov_d; + double range, residual; + + // Snapshots + auto snapshot_r = std::make_shared<Snapshot>(); + auto snapshot_k = std::make_shared<Snapshot>(); + + // Observations (just needed for the GPST) + snapshot_r->setObservations(std::make_shared<Observations>()); + snapshot_k->setObservations(std::make_shared<Observations>()); + obsd_t obs_r; + obs_r.time.sec = 0; + obs_r.time.time = 0; + snapshot_r->getObservations()->addObservation(obs_r); + obsd_t obs_k; + obs_k.time.sec = 0; + obs_k.time.time = 1; + snapshot_k->getObservations()->addObservation(obs_k); + + // Random receiver position + for (auto i = 0; i<100; i++) + { + // clear satellites and ranges + snapshot_r->getSatellites().clear(); + snapshot_k->getSatellites().clear(); + snapshot_r->getRanges().clear(); + snapshot_k->getRanges().clear(); + + // random setup + x_r_LLA = computeRandomReceiverLatLonAlt(); + x_r_ECEF = latLonAltToEcef(x_r_LLA); + d_ECEF = Vector3d::Random() * 10; + x_k_ECEF = x_r_ECEF + d_ECEF; + x_k_LLA = ecefToLatLonAlt(x_k_ECEF); + + double clock_r = Vector2d::Random()(0) * 1e-6; + double clock_k = Vector2d::Random()(0) * 1e-6; + + // groundtruth + d_gt.head<3>() = d_ECEF; + d_gt(3) = (clock_k - clock_r) * CLIGHT; + + std::cout << "Iteration " << i << ":\n"; + std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n"; + std::cout << "\tclock_r " << clock_r << ":\n"; + std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n"; + std::cout << "\tclock_k " << clock_k << ":\n"; + std::cout << "\td_gt " << d_gt.transpose() << ":\n"; + + std::set<int> common_sats; + std::set<int> raim_discarded_sats; + + // random visible satellites + for (auto j = 0; j<N_sats; j++) + { + common_sats.insert(j); + + // Satellite r (random) + computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range); + ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3); + + Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); + snapshot_r->getSatellites().emplace(j, sat_r); + + // Range r + Range range_r; + range_r.sat = j; + range_r.L_corrected = range + CLIGHT * clock_r; + range_r.L_var = 1; + range_r.L2_corrected = range_r.L_corrected; + range_r.L2_var = range_r.L_var; + + snapshot_r->getRanges().emplace(j, range_r); + + std::cout << "\tsat: " << j << "\n"; + std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n"; + std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n"; + + // Satellite k (random) + computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range); + ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3); + + Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); + snapshot_k->getSatellites().emplace(j, sat_k); + + // Range k + Range range_k; + range_k.sat = j; + range_k.L_corrected = range + CLIGHT * clock_k; + range_k.L_var = 1; + range_k.L2_corrected = range_k.L_corrected; + range_k.L2_var = range_k.L_var; + + snapshot_k->getRanges().emplace(j, range_k); + + std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n"; + std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n"; + } + + // TDCP + d = Vector4d::Zero(); // initialize + bool tdcp_ok = Tdcp(snapshot_r, + snapshot_k, + x_r_ECEF, + common_sats, + d, + cov_d, + residual, + raim_discarded_sats, + tdcp_params); + + // CHECKS + std::cout << "CHECKS iteration " << i << std::endl; + ASSERT_TRUE(tdcp_ok); + ASSERT_LE(residual, 1e-9); + ASSERT_MATRIX_APPROX(d, d_gt, 1e-6); + } +} + +TEST(Tdcp, Tdcp_raim) +{ + int N_sats = 20; + TdcpBatchParams tdcp_params; tdcp_params.max_iterations = 10; tdcp_params.min_common_sats = 6; @@ -166,7 +301,7 @@ TEST(Tdcp, Tdcp) std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n"; } - // Distort one range -> to be detected by RAIM + // Distort 2 ranges -> to be detected by RAIM int wrong_sat1 = i % N_sats; snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20; int wrong_sat2 = (i + N_sats / 2) % N_sats; @@ -197,6 +332,6 @@ TEST(Tdcp, Tdcp) int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); }