Extract code bias corrected PR, iono and tropo data from rtklib to VTL

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -39,10 +39,10 @@
 #include "rtklib_solution.h"
 #include <glog/logging.h>
 #include <matio.h>
+#include "iostream"
 #include <exception>
 #include <utility>
 #include <vector>
-#include "iostream"
 
 using namespace std;
 
@@ -1006,8 +1006,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                             d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data);
                         }
                 }
-
+            std::vector<double> tropo_vec;
-            result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data);
+            std::vector<double> iono_vec;
+            std::vector<double> pr_corrected_code_bias_vec;
+            result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_vec,
+                iono_vec, pr_corrected_code_bias_vec);
 
             if (result == 0)
                 {
@@ -1114,7 +1117,10 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     new_vtl_data.sat_health_flag(n) = svh.at(n);
                                     new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0];
                                     // TODO: first version of VTL works only with ONE frequency band (band #0 is L1)
-                                    new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0];
+                                    //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges
+                                    //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
+                                    //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts)
+                                    new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n];
                                     new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0];
                                     new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
                                 }
@@ -1138,9 +1144,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                             new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0];  //time
                             new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0];  //doppler
                             //receiver clock offset and receiver clock drift
-                            new_vtl_data.rx_dts(0)=rx_position_and_time[3];
+                            new_vtl_data.rx_dts(0) = rx_position_and_time[3];
-                            new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s]
+                            new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6;  // [ppm] to [s]
-                            
+
                             //Call the VTL engine loop: miguel: Should we wait until valid PVT solution?
                             vtl_engine.vtl_loop(new_vtl_data);
 

src/algorithms/libs/rtklib/rtklib_pntpos.cc

@@ -34,7 +34,7 @@
 #include "rtklib_ionex.h"
 #include "rtklib_sbas.h"
 #include <cstring>
-#include <vector>
+
 
 /* pseudorange measurement error variance ------------------------------------*/
 double varerr(const prcopt_t *opt, double el, int sys)
@@ -403,7 +403,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
     const double *dts, const double *vare, const int *svh,
     const nav_t *nav, const double *x, const prcopt_t *opt,
     double *v, double *H, double *var, double *azel, int *vsat,
-    double *resp, int *ns)
+    double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias)
 {
     double r;
     double dion;
@@ -500,6 +500,11 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
             /* pseudorange residual */
             v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
 
+            /* MOD ARRIBAS */
+            pr_corrected_code_bias[i] = P;
+            tropo_m[i] = dtrp;
+            iono_m[i] = dion;
+
             /* design matrix */
             for (j = 0; j < NX; j++)
                 {
@@ -603,7 +608,8 @@ int valsol(const double *azel, const int *vsat, int n,
 int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
     const double *vare, const int *svh, const nav_t *nav,
     const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
-    double *resp, char *msg)
+    double *resp, char *msg,
+    std::vector<double> &tropo_vec, std::vector<double> &iono_vec, std::vector<double> &pr_corrected_code_bias_vec)
 {
     double x[NX] = {0};
     double dx[NX];
@@ -621,6 +627,11 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
     int ns;
     char msg_aux[128];
 
+    double *tropo_m, *iono_m, *pr_corrected_code_bias;
+    tropo_m = mat(n + 4, 1);
+    iono_m = mat(n + 4, 1);
+    pr_corrected_code_bias = mat(n + 4, 1);
+
     trace(3, "estpos  : n=%d\n", n);
 
     v = mat(n + 4, 1);
@@ -635,7 +646,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
     for (i = 0; i < MAXITR; i++)
         {
             /* pseudorange residuals */
-            nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns);
+            nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp,
+                &ns, tropo_m, iono_m, pr_corrected_code_bias);
 
             if (nv < NX)
                 {
@@ -690,6 +702,13 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
                         {
                             sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
                         }
+
+                    for (k = 0; k < n; k++)
+                        {
+                            tropo_vec.push_back(tropo_m[k]);
+                            iono_vec.push_back(iono_m[k]);
+                            pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]);
+                        }
                     free(v);
                     free(H);
                     free(var);
@@ -766,9 +785,12 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
                     vare_e[k] = vare[j];
                     svh_e[k++] = svh[j];
                 }
+            std::vector<double> tropo_vec;
+            std::vector<double> iono_vec;
+            std::vector<double> pr_corrected_code_bias_vec;
             /* estimate receiver position without a satellite */
             if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e,
-                    vsat_e, resp_e, msg_e))
+                    vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec))
                 {
                     trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg);
                     continue;
@@ -978,7 +1000,9 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
  *-----------------------------------------------------------------------------*/
 int pntpos(const obsd_t *obs, int n, const nav_t *nav,
     const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
-    char *msg)
+    char *msg, std::vector<double> &tropo_vec,
+    std::vector<double> &iono_vec,
+    std::vector<double> &pr_corrected_code_bias_vec)
 {
     prcopt_t opt_ = *opt;
     double *rs;
@@ -1022,7 +1046,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
     satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh.data());
 
     /* estimate receiver position with pseudorange */
-    stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg);
+    stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg,
+        iono_vec, tropo_vec, pr_corrected_code_bias_vec);
 
     /* raim fde */
     if (!stat && n >= 6 && opt->posopt[4])

src/algorithms/libs/rtklib/rtklib_pntpos.h

@@ -35,6 +35,7 @@
 
 #include "rtklib.h"
 #include "rtklib_rtkcmn.h"
+#include <vector>
 
 /* constants -----------------------------------------------------------------*/
 const int NX = 4 + 3;         //!< # of estimated parameters
@@ -92,7 +93,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
     const double *dts, const double *vare, const int *svh,
     const nav_t *nav, const double *x, const prcopt_t *opt,
     double *v, double *H, double *var, double *azel, int *vsat,
-    double *resp, int *ns);
+    double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias);
 
 /* validate solution ---------------------------------------------------------*/
 int valsol(const double *azel, const int *vsat, int n,
@@ -103,7 +104,8 @@ int valsol(const double *azel, const int *vsat, int n,
 int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
     const double *vare, const int *svh, const nav_t *nav,
     const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
-    double *resp, char *msg);
+    double *resp, char *msg, std::vector<double> &tropo_vec,
+    std::vector<double> &iono_vec, std::vector<double> &pr_corrected_code_bias_vec);
 
 /* raim fde (failure detection and exclution) -------------------------------*/
 int raim_fde(const obsd_t *obs, int n, const double *rs,
@@ -140,6 +142,8 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
  */
 int pntpos(const obsd_t *obs, int n, const nav_t *nav,
     const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
-    char *msg);
+    char *msg, std::vector<double> &tropo_vec,
+    std::vector<double> &iono_vec,
+    std::vector<double> &pr_corrected_code_bias_vec);
 
 #endif  // GNSS_SDR_RTKLIB_PNTPOS_H

src/algorithms/libs/rtklib/rtklib_rtkpos.cc

@@ -38,7 +38,6 @@
 #include <cmath>
 #include <cstring>
 #include <string>
-#include <vector>
 
 static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
     const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)),
@@ -2762,7 +2761,10 @@ void rtkfree(rtk_t *rtk)
  * notes  : before calling function, base station position rtk->sol.rb[] should
  *          be properly set for relative mode except for moving-baseline
  *-----------------------------------------------------------------------------*/
-int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
+int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
+    std::vector<double> &tropo_vec,
+    std::vector<double> &iono_vec,
+    std::vector<double> &pr_corrected_code_bias_vec)
 {
     prcopt_t *opt = &rtk->opt;
     sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
@@ -2797,7 +2799,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
     time = rtk->sol.time; /* previous epoch */
 
     /* rover position by single point positioning */
-    if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg))
+    if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec,
+            iono_vec, pr_corrected_code_bias_vec))
         {
             errmsg(rtk, "point pos error (%s)\n", msg);
             if (!rtk->opt.dynamics)
@@ -2839,7 +2842,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
     if (opt->mode == PMODE_MOVEB)
         { /*  moving baseline */
             /* estimate position/velocity of base station */
-            if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg))
+            if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec,
+                    iono_vec, pr_corrected_code_bias_vec))
                 {
                     errmsg(rtk, "base station position error (%s)\n", msg);
                     return 0;

src/algorithms/libs/rtklib/rtklib_rtkpos.h

@@ -34,6 +34,7 @@
 
 #include "rtklib.h"
 #include "rtklib_rtkcmn.h"
+#include <vector>
 
 /** \addtogroup PVT
  * \{ */
@@ -174,7 +175,10 @@ void rtkinit(rtk_t *rtk, const prcopt_t *opt);
 
 void rtkfree(rtk_t *rtk);
 
-int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
+int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
+    std::vector<double> &tropo_vec,
+    std::vector<double> &iono_vec,
+    std::vector<double> &pr_corrected_code_bias_vec);
 
 
 /** \} */

src/algorithms/libs/rtklib/rtklib_rtksvr.cc

@@ -38,6 +38,7 @@
 #include "rtklib_solution.h"
 #include "rtklib_stream.h"
 #include <cstring>
+#include <vector>
 
 /* write solution header to output stream ------------------------------------*/
 void writesolhead(stream_t *stream, const solopt_t *solopt)
@@ -595,7 +596,11 @@ void *rtksvrthread(void *arg)
                         }
                     /* rtk positioning */
                     rtksvrlock(svr);
-                    rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
+                    std::vector<double> tropo_vec;
+                    std::vector<double> iono_vec;
+                    std::vector<double> pr_corrected_code_bias_vec;
+                    rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec,
+                        iono_vec, pr_corrected_code_bias_vec);
                     rtksvrunlock(svr);
 
                     if (svr->rtk.sol.stat != SOLQ_NONE)