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;

int N_sats = 20;
int N_tests = 100;
double distortion1 = -30;
double distortion2 = -20;

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 computeVisibleSatellite(const Vector3d& receiver_latlonalt,
                             const int& n_sat,
                             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);

    // equidistributed azimuth at elevation 45º
    if (n_sat < 6)
    {
        sat_azel(0) = -M_PI + n_sat * 2*M_PI/6 + 0.1;   // in [-pi, pi]
        sat_azel(1) = M_PI / 4;                 // in [0, pi/2]
    }
    // random elevation and azimuth
    else
    {
        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]
    }

    // random range
    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)
{
    TdcpBatchParams tdcp_params;
    tdcp_params.max_iterations = 5;
    tdcp_params.min_common_sats = 6;
    tdcp_params.raim_n = 0;
    tdcp_params.residual_opt = 0;
    tdcp_params.max_residual = 1e20;
    tdcp_params.relinearize_jacobian = true;
    tdcp_params.tdcp.multi_freq = false;
    tdcp_params.tdcp.sigma_atm = 1;
    tdcp_params.tdcp.sigma_carrier = 1;

    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<N_tests; 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 << "Test " << 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
            computeVisibleSatellite(x_r_LLA, j, 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 = range + CLIGHT * clock_r;
            range_r.L_corrected = range_r.L;
            range_r.L_var = 1;
            range_r.L2 = range_r.L;
            range_r.L2_corrected = range_r.L;
            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)
            computeVisibleSatellite(x_k_LLA, j, 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 = range + CLIGHT * clock_k;
            range_k.L_corrected = range_k.L;
            range_k.L_var = 1;
            range_k.L2 = range_k.L;
            range_k.L2_corrected = range_k.L;
            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 Test " << i << std::endl;
        ASSERT_TRUE(tdcp_out.success);
        ASSERT_LE(tdcp_out.residual, 1e-3);
        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
    }
}

TEST(Tdcp, Tdcp_raim_residual_rmse)
{
    int N_sats = 20;

    TdcpBatchParams tdcp_params;
    tdcp_params.max_iterations = 5;
    tdcp_params.min_common_sats = 6;
    tdcp_params.raim_n = 2;
    tdcp_params.relinearize_jacobian = true;
    tdcp_params.residual_opt = 0; // Normalized RMSE
    tdcp_params.max_residual = 0.1; // low threshold to detect outliers...
    tdcp_params.tdcp.multi_freq = false;
    tdcp_params.tdcp.sigma_atm = 1;
    tdcp_params.tdcp.sigma_carrier = 1;


    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<N_tests; 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;

        // distorted sats
        int wrong_sat1 = i % N_sats;
        int wrong_sat2 = (i + N_sats / 2) % N_sats;

        std::cout << "Test " << 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::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;

        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)
            computeVisibleSatellite(x_r_LLA, j, 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 = range + CLIGHT * clock_r;
            range_r.L_corrected = range_r.L;
            range_r.L_var = 1;
            range_r.L2 = range_r.L;
            range_r.L2_corrected = range_r.L;
            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)
            computeVisibleSatellite(x_k_LLA, j, 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 = range + CLIGHT * clock_k;
            range_k.L_corrected = range_k.L;
            range_k.L_var = 1;
            range_k.L2 = range_k.L;
            range_k.L2_corrected = range_k.L;
            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
        snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
        snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
        snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
        snapshot_r->getRanges().at(wrong_sat2).L += distortion2;


        // 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-3);
        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
    }
}

TEST(Tdcp, Tdcp_raim_residual_max_mah)
{
    int N_sats = 20;

    TdcpBatchParams tdcp_params;
    tdcp_params.max_iterations = 5;
    tdcp_params.min_common_sats = 6;
    tdcp_params.raim_n = 2;
    tdcp_params.relinearize_jacobian = true;
    tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance
    tdcp_params.max_residual = 3.84; // 95% of confidence
    tdcp_params.tdcp.multi_freq = false;
    tdcp_params.tdcp.sigma_atm = 1;
    tdcp_params.tdcp.sigma_carrier = 1;


    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<N_tests; 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;

        // distorted sats
        int wrong_sat1 = i % N_sats;
        int wrong_sat2 = (i + N_sats / 2) % N_sats;

        std::cout << "\nTest " << 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::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;

        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)
            computeVisibleSatellite(x_r_LLA, j, 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 = range + CLIGHT * clock_r;
            range_r.L_corrected = range_r.L;
            range_r.L_var = 1;
            range_r.L2 = range_r.L;
            range_r.L2_corrected = range_r.L;
            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)
            computeVisibleSatellite(x_k_LLA, j, 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 = range + CLIGHT * clock_k;
            range_k.L_corrected = range_k.L;
            range_k.L_var = 1;
            range_k.L2 = range_k.L;
            range_k.L2_corrected = range_k.L;
            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
        snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
        snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
        snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
        snapshot_r->getRanges().at(wrong_sat2).L += distortion2;

        // 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-3);
        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
    }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}