Resolve "TDCP batch implementation"

Closes #15 (closed)

test/gtest_tdcp.cpp

+#include "gtest/utils_gtest.h"
+#include "gnss_utils/tdcp.h"
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GnssUtils;
+using namespace Eigen;
+
+Vector3d computeRandomReceiverLatLonAlt()
+{
+    Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
+    receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
+    receiver_LLA(1) *= M_PI;     // in [-PI, PI]
+    receiver_LLA(2) += 1;        // ([0, 2])
+    receiver_LLA(2) *= 5e2;      // in [0, 1e3]
+
+    return receiver_LLA;
+}
+
+void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
+                                   Vector3d& sat_ENU,
+                                   Vector3d& sat_ECEF,
+                                   Vector2d& sat_azel,
+                                   double& range)
+{
+    Vector3d t_ECEF_ENU, t_ENU_ECEF;
+    Matrix3d R_ENU_ECEF;
+
+    t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
+
+    computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+    // random elevation and azimuth
+    sat_azel = Vector2d::Random();               // in [-1, 1]
+    sat_azel(0) *= M_PI;                                // in [-pi, pi]
+    sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2;     // in [0, pi/2]
+    range = VectorXd::Random(1)(0) * 5e2 + 1e3;  // in [500, 1500]
+
+    // ENU
+    sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
+               cos(sat_azel(0)) * cos(sat_azel(1)) * range,
+               sin(sat_azel(1)) * range;
+
+    // ECEF
+    sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+
+    // Range
+    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_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
+        auto tdcp_out = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r_ECEF,
+                             common_sats,
+                             Vector4d::Zero(),
+                             tdcp_params);
+
+        // CHECKS
+        std::cout << "CHECKS iteration " << i << std::endl;
+        ASSERT_TRUE(tdcp_out.success);
+        ASSERT_LE(tdcp_out.residual, 1e-9);
+        ASSERT_MATRIX_APPROX(tdcp_out.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;
+    tdcp_params.raim_n = 2;
+    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";
+        }
+
+        // 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;
+        snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
+
+
+        // TDCP
+        auto tdcp_out = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r_ECEF,
+                             common_sats,
+                             Vector4d::Zero(),
+                             tdcp_params);
+
+        // CHECKS
+        std::cout << "CHECKS iteration " << i << std::endl;
+        ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+        ASSERT_TRUE(tdcp_out.success);
+        ASSERT_LE(tdcp_out.residual, 1e-9);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}