Resolve "TDCP batch implementation"

Closes #15 (closed)

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();
 }