mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 14:25:00 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Javier Arribas 2022-10-13 22:26:11 +02:00
parent 46ba287091
commit b699173208
6 changed files with 71 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -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);
}
}
result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data);
std::vector<double> tropo_vec;
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(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s]
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]
//Call the VTL engine loop: miguel: Should we wait until valid PVT solution?
vtl_engine.vtl_loop(new_vtl_data);

39
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -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])

10
src/algorithms/libs/rtklib/rtklib_pntpos.h
View File

@ -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

12
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

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

6
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@ -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);
/** \} */

7
src/algorithms/libs/rtklib/rtklib_rtksvr.cc
View File

@ -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)