single_differences.cpp

/*
 * single_differences.cpp
 *
 *  Created on: Dec 4, 2019
 *      Author: jvallve
 */

#include "gnss_utils/single_differences.h"

namespace GNSSUtils
{

bool singleDifferences(const Observations& obs_r, Navigation& nav_r,
                       const Observations& obs_k, const Navigation& nav_k,
                       Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
                       double& residual, std::set<int>& discarded_sats,
                       const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
    // COMPUTE SINGLE DIFFERENCES
    return singleDifferences(obs_r, nav_r, computePos(obs_r,nav_r,opt).pos,
                             obs_k, nav_k,
                             d, cov_d,
                             residual, discarded_sats,
                             sd_params, opt);
}

bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
                       const Observations& obs_k, const Navigation& nav_k,
                       Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
                       double& residual, std::set<int>& discarded_sats,
                       const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
    // FIND COMMON SATELLITES OBSERVATIONS
    Observations common_obs_r, common_obs_k;
    Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
    int n_common_sats = common_obs_r.getObservations().size();

    // COMPUTE COMMON SATELLITES POSITION
    std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
    computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
    computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);

    // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
    filterCommonSatellites(common_obs_r, common_sats_pos_r,
                           common_obs_k, common_sats_pos_k,
                           discarded_sats, x_r,
                           sd_params, opt);

    // COMPUTE SINGLE DIFFERENCES
    return singleDifferences(common_obs_r, nav_r, common_sats_pos_r, x_r,
                             common_obs_k, nav_k, common_sats_pos_k,
                             d, cov_d,
                             residual, discarded_sats,
                             sd_params);
}

bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<int,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
                       const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
                       Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
                       double& residual, std::set<int>& discarded_sats,
                       const SingleDifferencesParams& sd_params)
{
    assert(common_obs_r.size() == common_obs_k.size());
    assert(common_obs_r.size() == common_sats_pos_r.size());
    assert(common_obs_k.size() == common_sats_pos_k.size());

    double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
    double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);

    int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
    int n_common_sats = common_sats_pos_k.size();

    if(n_common_sats < required_n_sats)
    {
        #if GNSS_UTILS_SD_DEBUG == 1
            printf("SD: NOT ENOUGH COMMON SATS");
            printf("SD: %i common sats available, %i sats are REQUIRED. [ SKIPPING SD ]",
                   n_common_sats,
                   required_n_sats);
        #endif
        return false;
    }

    // MULTI-FREQUENCY
    std::map<int,std::pair<int,int>> row_2_sat_freq;
    int row = 0;
    for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
    {
        auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
        auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);

        const int& sat_number = obs_k.sat;

        if(sd_params.use_carrier_phase)
        {
            // L1/E1
            if (std::abs(obs_r.L[0]) > 1e-12 and
                std::abs(obs_k.L[0]) > 1e-12)
            {
                row_2_sat_freq[row] = std::make_pair(sat_number,0);
                row++;
            }
            if (!sd_params.use_multi_freq)
                continue;

            // L2 (GPS/GLO/QZS)
            int sys=satsys(sat_number,NULL);
            if (NFREQ >= 2 and (sys&(SYS_GPS|SYS_GLO|SYS_QZS)) and
                std::abs(obs_r.L[1]) > 1e-12 and
                std::abs(obs_k.L[1]) > 1e-12)
            {
                row_2_sat_freq[row] = std::make_pair(sat_number,1);
                row++;
            }
            // E5 (GAL)
            else if (NFREQ >= 3 and (sys&SYS_GAL) and
                std::abs(obs_r.L[2]) > 1e-12 and
                std::abs(obs_k.L[2]) > 1e-12)
            {
                row_2_sat_freq[row] = std::make_pair(sat_number,2);
                row++;
            }
        }
        else
            // L1/E1
            if (std::abs(obs_r.P[0]) > 1e-12 and
                std::abs(obs_k.P[0]) > 1e-12)
            {
                row_2_sat_freq[row] = std::make_pair(sat_number,0);
                row++;
            }
    }
    int n_differences = row_2_sat_freq.size();

    // Initial guess
    Eigen::Vector4d d_0(d);

    #if GNSS_UTILS_SD_DEBUG == 1
        std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
        std::cout << "d initial guess: " << d_0.transpose() << std::endl;
        std::cout << "common sats: ";
        for (auto row_sat_freq_it : row_2_sat_freq)
            std::cout << row_sat_freq_it.second.first << " ";
        std::cout << std::endl;
    #endif

    // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
    Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences,4));
    Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
    Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
    Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
    Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
    int sat_i = 0;

    for (auto row_sat_freq_it : row_2_sat_freq)
    {
        const int& row = row_sat_freq_it.first;
        const int& sat_number = row_sat_freq_it.second.first;
        const int& freq = row_sat_freq_it.second.second;

        auto obs_r = common_obs_r.getObservationBySat(sat_number);
        auto obs_k = common_obs_k.getObservationBySat(sat_number);

        // excluded satellite
        if (discarded_sats.count(sat_number) != 0)
        {
            #if GNSS_UTILS_SD_DEBUG == 1
            std::cout << "\tdiscarded sat" << std::endl;
            #endif
            continue;
        }

        // Satellite's positions
        s_r.col(row) = common_sats_pos_r.at(sat_number);
        s_k.col(row) = common_sats_pos_k.at(sat_number);

        if(sd_params.use_carrier_phase)
            drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq]) -
                          (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq]);

        else
            drho_m(row) = obs_k.P[freq] - obs_r.P[freq];

        #if GNSS_UTILS_SD_DEBUG == 1
            std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
            //std::cout << "\tpositions:\n\t\tt_r: " << s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl;
            //std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl;
            //std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl;
            //std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
            //std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
            //std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
            //std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
            //std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
        #endif

        if(!sd_params.relinearize_jacobian)
        {
            // Unit vectors from recerivers to satellites
            Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();

            // Fill Jacobian matrix
            A(row, 0) = u_k(0);
            A(row, 1) = u_k(1);
            A(row, 2) = u_k(2);
            A(row, 3) = -1.0;
        }
    }

    // LEAST SQUARES SOLUTION =======================================================================
    for (int j = 0; j < sd_params.max_iterations; j++)
    {
        // fill A and r
        for (int row = 0; row < A.rows(); row++)
        {
            // Evaluate r
            r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);

            // Evaluate A
            if (sd_params.relinearize_jacobian)
            {
                // Unit vectors from rcvrs to sats
                Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();

                // Fill Jacobian matrix
                A(row, 0) = u_k(0);
                A(row, 1) = u_k(1);
                A(row, 2) = u_k(2);
                A(row, 3) = -1.0;
            }
        }

        // Solve
        cov_d = (A.transpose()*A).inverse();
        d -= cov_d*A.transpose()*r;

        // wrong solution
        if(d.array().isNaN().any())
        {
            std::cout << "SD: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
            return false;
        }

        // residual
        //residual = sqrt((r - A * d).squaredNorm() / A.rows());
        residual = (r + A * d).norm();
        //std::cout << "residual = " << residual << std::endl;
        //std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << std::endl;

#if GNSS_UTILS_SD_DEBUG == 1
        std::cout << "Displacement vector = %s" << d.head<3>().transpose() << std::endl;
        printf("Ref distance      = %7.3f m\n", d_0.norm());
        printf("Computed distance = %7.3f m\n", d.head<3>().norm());
        printf("SD: residual      = %13.10f\n", residual);
        printf("SD: row           = %i\n", r.rows());
        std::cout << "SD: drho          = " << r.transpose() << std::endl;
        std::cout << "SD: drho_m        = " << drho_m.transpose() << std::endl;
        std::cout << "SD: H             = \n" << A << std::endl;
        printf("SD: dT            = %8.3f secs\n", d(3));
#endif


        // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
        int input_discarded_sats = discarded_sats.size();
        if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
        {
            int worst_sat_row = -1;
            double best_residual = 1e12;
            Eigen::Vector4d best_d;
            int n_removed_rows = 1;

            // remove some satellites
            while(discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
            {
                auto A_raim = A;
                auto r_raim = r;

                // solve removing each satellite
                for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
                {
                    // Multi-freq: some rows for the same satellite
                    n_removed_rows = 1;
                    if (sd_params.use_multi_freq)
                        while (row_removed + n_removed_rows < A_raim.rows() and row_2_sat_freq.at(row_removed+n_removed_rows) == row_2_sat_freq.at(row_removed))
                            n_removed_rows++;

                    // remove satellite rows
                    for (auto r = 0; r < n_removed_rows; r++)
                    {
                        A_raim.row(row_removed+r) = Eigen::Vector4d::Zero().transpose();
                        r_raim(row_removed+r) = 0; // not necessary
                    }

                    // solve
                    Eigen::Vector4d d_raim = d_0-(A_raim.transpose()*A_raim).inverse()*A_raim.transpose()*r_raim;

                    // no NaN
                    if (!d_raim.array().isNaN().any())
                    {
                        // residual
                        residual = (r - A * d_raim).norm() / A.rows(); //FIXME: evaluate with XX_raim or not? I think next line is the good one
                        //residual = sqrt((r_raim - A_raim * d_raim).squaredNorm() / A_raim.rows());
    
                        // store if best residual
                        if (residual < best_residual)
                        {
                            worst_sat_row = row_removed;
                            best_residual = residual;
                            best_d = d_raim;
                        }
                    }
                    // restore initial A and r
                    for (auto row = 0; row < n_removed_rows; row++)
                    {
                        A_raim.row(row_removed+row) = A.row(row_removed+row);
                        r_raim(row_removed+row) = r(row_removed+row);
                    }
                    row_removed += (n_removed_rows-1);
                }

                // No successful RAIM solution
                if(worst_sat_row == -1)
                {
                    printf("SD: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
                    return false;
                }

                // store removed sat
                discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);

                // decrease n_common_sats
                n_differences=-n_removed_rows;
                n_common_sats--;

                // Remove selected satellite from problem
                auto A_aux = A;
                auto r_aux = r;
                auto drho_m_aux = drho_m;
                auto s_r_aux = s_r;
                auto s_k_aux = s_k;
                A.conservativeResize(n_differences, Eigen::NoChange);
                r.conservativeResize(n_differences);
                drho_m.conservativeResize(n_differences);
                s_r.conservativeResize(Eigen::NoChange, n_differences);
                s_k.conservativeResize(Eigen::NoChange, n_differences);

                int back_elements_changing = A.rows() - worst_sat_row - n_removed_rows;
                if (back_elements_changing != 0) //if last satelite removed, conservative resize did the job
                {
                    A       .bottomRows(back_elements_changing)  = A_aux     .bottomRows(back_elements_changing);
                    s_r     .rightCols(back_elements_changing)   = s_r_aux   .rightCols(back_elements_changing);
                    s_k     .rightCols(back_elements_changing)   = s_k_aux   .rightCols(back_elements_changing);
                    r       .tail(back_elements_changing)        = r_aux     .tail(back_elements_changing);
                    drho_m  .tail(back_elements_changing)        = drho_m_aux.tail(back_elements_changing);
                }

                // apply the RAIM solution
                d = best_d;
                cov_d = (A.transpose()*A).inverse();

                #if GNSS_UTILS_SD_DEBUG == 1
                    std::cout << "SD After RAIM iteration" << std::endl;
                    std::cout << "\tExcluded sats : ";
                    for (auto dsat : discarded_sats)
                        std::cout << (int)dsat << " ";
                    std::cout << std::endl;
                    std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
                    std::cout << "\tRows          : " << n_differences << std::endl;
                    std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
                    std::cout << "\tSolution      = " << d.transpose()   << " (" << d.head<3>().norm()   << "m)" << std::endl;
                    std::cout << "\tClock offset  = " << d(3) << std::endl;
                    std::cout << "\tResidual      = " << residual << std::endl;
                #endif
            }

            #if GNSS_UTILS_SD_DEBUG == 1
                std::cout << "SD After RAIM " << std::endl;
                std::cout << "\tExcluded sats : ";
                for (auto dsat : discarded_sats)
                    std::cout << (int)dsat << " ";
                std::cout << std::endl;
                std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
                std::cout << "\tRows          : " << n_differences << std::endl;
                std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
                std::cout << "\tSolution      = " << d.transpose()   << " (" << d.head<3>().norm()   << "m)" << std::endl;
                std::cout << "\tClock offset  = " << d(3) << std::endl;
                std::cout << "\tResidual      = " << residual << std::endl;
            #endif
        }

        #if GNSS_UTILS_SD_DEBUG == 1
            std::cout << "SD iteration " << j << std::endl;
            std::cout << "\tExcluded sats : ";
            for (auto dsat : discarded_sats)
                std::cout << (int)dsat << " ";
            std::cout << std::endl;
            std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
            std::cout << "\tRows          : " << n_differences << std::endl;
            std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
            std::cout << "\tSolution      = " << d.transpose()   << " (" << d.head<3>().norm()   << "m)" << std::endl;
            std::cout << "\tClock offset  = " << d(3) << std::endl;
            std::cout << "\tResidual      = " << residual << std::endl;
        #endif
    }

    // weight covariance with the measurement noise (proportional to time)
    double sq_sigma_SD = (tk-tr) * sd_params.sigma_atm * sd_params.sigma_atm +
                          2      * (sd_params.use_carrier_phase ?
                                          sd_params.sigma_carrier * sd_params.sigma_carrier :
                                          sd_params.sigma_code    * sd_params.sigma_code);
    cov_d *= sq_sigma_SD;
    //residual = (r - A * d).norm() / A.rows();

    // Computing error on measurement space
    for(int row = 0; row < n_differences; row++)
    {
        r(row) = drho_m(row)
                 - (s_k.col(row) - x_r - d.head(3)).norm()
                 + (s_r.col(row) - x_r).norm()
                 - d(3);
    }
    residual = sqrt(r.squaredNorm() / r.size());

    return true;
}

void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vector3d>& common_sats_pos_r,
                            Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
                            std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
                            const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{
    assert(&common_obs_r != &common_obs_k);
    assert(&common_sats_pos_r != &common_sats_pos_k);
    assert(common_obs_r.size() == common_obs_k.size());
    assert(common_obs_r.size() == common_sats_pos_r.size());
    assert(common_obs_r.size() == common_sats_pos_k.size());

    //std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;

    std::set<int> remove_sats;

    for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
    {
        auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
        auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
        assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
        const int& sat_number = obs_r.sat;

        // already discarded sats
        if (discarded_sats.count(sat_number) != 0)
        {
            remove_sats.insert(sat_number);
            continue;
        }

        // wrong data (satellite is not included in the discarded list)
        if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
        {
            // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " << obs_k.L[0] << std::endl;
            remove_sats.insert(sat_number);
            continue;
        }
        if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
        {
            // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " << obs_k.P[0] << std::endl;
            remove_sats.insert(sat_number);
            continue;
        }

        // bad satellite position (satellite is not included in the discarded list)
        if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
            common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
        {
            // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" << common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
            remove_sats.insert(sat_number);
            continue;
        }

        // constellation
        int sys=satsys(obs_r.sat,NULL);
        if (!(sys & opt.navsys))
        {
            // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
            discarded_sats.insert(sat_number);
            remove_sats.insert(sat_number);
            continue;
        }

        // check both elevations
        double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
                                    computeSatElevation(x_r, common_sats_pos_k[sat_number]));
        if (elevation < opt.elmin)
        {
            // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin << std::endl;
            discarded_sats.insert(sat_number);
            remove_sats.insert(sat_number);
            continue;
        }

        // snr TODO: multifrequency (2nd param and 3rd idx)
        if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
        {
            // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
            discarded_sats.insert(sat_number);
            remove_sats.insert(sat_number);
            continue;
        }
    }

    // remove sats
    //std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
    for (auto sat : remove_sats)
    {
        assert(common_obs_r.hasSatellite(sat));
        assert(common_obs_k.hasSatellite(sat));
        assert(common_sats_pos_r.count(sat));
        assert(common_sats_pos_k.count(sat));
        common_obs_r.removeObservationBySat(sat);
        common_obs_k.removeObservationBySat(sat);
        common_sats_pos_r.erase(sat);
        common_sats_pos_k.erase(sat);
    }

    assert(common_obs_r.size() == common_obs_k.size());
    assert(common_obs_r.size() == common_sats_pos_r.size());
    assert(common_obs_r.size() == common_sats_pos_k.size());

    //std::cout << "final size: " << common_obs_k.size() << std::endl;
}

}