mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-10 17:30:33 +00:00
Code Issues Releases Wiki Activity

add: pseudorange residual

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-13 19:15:12 +00:00
parent 1155895f70
commit 8bfdb1343e
9 changed files with 43 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1009,9 +1009,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
std::vector<double> tropo_vec; std::vector<double> tropo_vec;
std::vector<double> iono_vec; std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec; std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual_vec;
std::vector<double> doppler_residual_vec;
result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_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); iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec);
if (result == 0) if (result == 0)
{ {
LOG(INFO) << "RTKLIB rtkpos error: " << d_rtk.errbuf; LOG(INFO) << "RTKLIB rtkpos error: " << d_rtk.errbuf;
@ -1124,6 +1126,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2];
new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; 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]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
new_vtl_data.pr_res(n) = pr_residual_vec[n];
} }
//VTL input data extraction from rtklib structures //VTL input data extraction from rtklib structures
/* Receiver position, velocity and clock */ /* Receiver position, velocity and clock */

1
src/algorithms/PVT/libs/vtl_data.cc
View File

@ -37,6 +37,7 @@ void Vtl_Data::init_storage(int n_sats)
pr_m = arma::vec(n_sats); pr_m = arma::vec(n_sats);
doppler_hz = arma::vec(n_sats); doppler_hz = arma::vec(n_sats);
carrier_phase_rads = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats);
pr_res = arma::vec(n_sats);
rx_p = arma::mat(1, 3); rx_p = arma::mat(1, 3);
rx_v = arma::mat(1, 3); rx_v = arma::mat(1, 3);

3
src/algorithms/PVT/libs/vtl_data.h
View File

@ -47,7 +47,8 @@ public:
arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec pr_m; // Satellite Code pseudoranges [m]
arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz]
arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads]
arma::colvec pr_res; // pseudorange residual
arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_p; // Receiver ENU Position [m]
arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_v; // Receiver Velocity [m/s]
arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s]

9
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -76,7 +76,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// Kalman state prediction (time update) // Kalman state prediction (time update)
//kf_x.print(" KF RTKlib STATE"); //kf_x.print(" KF RTKlib STATE");
new_data.kf_state=kf_x; new_data.kf_state=kf_x;
kf_x = kf_F * kf_x; // state prediction //kf_x = kf_F * kf_x; // state prediction
kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
//from error state variables to variables //from error state variables to variables
// From state variables definition // From state variables definition
@ -166,9 +166,10 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
new_data.kf_state.print(" KF RTKlib STATE"); new_data.pr_res.print(" pr RESIDUALS");
cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; //new_data.kf_state.print(" KF RTKlib STATE");
cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100; //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
//cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################

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

@ -609,7 +609,11 @@ 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 double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, 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) std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{ {
double x[NX] = {0}; double x[NX] = {0};
double dx[NX]; double dx[NX];
@ -630,6 +634,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
double *tropo_m, *iono_m, *pr_corrected_code_bias; double *tropo_m, *iono_m, *pr_corrected_code_bias;
tropo_m = mat(n + 4, 1); tropo_m = mat(n + 4, 1);
iono_m = mat(n + 4, 1); iono_m = mat(n + 4, 1);
resp = mat(n + 4, 1);
pr_corrected_code_bias = mat(n + 4, 1); pr_corrected_code_bias = mat(n + 4, 1);
trace(3, "estpos : n=%d\n", n); trace(3, "estpos : n=%d\n", n);
@ -708,6 +713,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
tropo_vec.push_back(tropo_m[k]); tropo_vec.push_back(tropo_m[k]);
iono_vec.push_back(iono_m[k]); iono_vec.push_back(iono_m[k]);
pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]); pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]);
pr_residual_vec.push_back(resp[k]);
} }
free(v); free(v);
free(H); free(H);
@ -788,9 +794,11 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
std::vector<double> tropo_vec; std::vector<double> tropo_vec;
std::vector<double> iono_vec; std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec; std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual_vec;
std::vector<double> doppler_residual_vec;
/* estimate receiver position without a satellite */ /* 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, 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, iono_vec, tropo_vec, pr_corrected_code_bias_vec)) vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec))
{ {
trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg);
continue; continue;
@ -1002,7 +1010,9 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg, std::vector<double> &tropo_vec, char *msg, std::vector<double> &tropo_vec,
std::vector<double> &iono_vec, std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec) std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{ {
prcopt_t opt_ = *opt; prcopt_t opt_ = *opt;
double *rs; double *rs;
@ -1047,8 +1057,7 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
/* estimate receiver position with pseudorange */ /* 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); iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec);
/* raim fde */ /* raim fde */
if (!stat && n >= 6 && opt->posopt[4]) if (!stat && n >= 6 && opt->posopt[4])
{ {

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

@ -144,6 +144,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg, std::vector<double> &tropo_vec, char *msg, std::vector<double> &tropo_vec,
std::vector<double> &iono_vec, std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec); std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual,
std::vector<double> &doppler_residual);
#endif // GNSS_SDR_RTKLIB_PNTPOS_H #endif // GNSS_SDR_RTKLIB_PNTPOS_H

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

@ -2764,7 +2764,9 @@ 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> &tropo_vec,
std::vector<double> &iono_vec, std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec) std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{ {
prcopt_t *opt = &rtk->opt; prcopt_t *opt = &rtk->opt;
sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
@ -2800,7 +2802,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
/* rover position by single point positioning */ /* rover position by single point positioning */
if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec,
iono_vec, pr_corrected_code_bias_vec)) iono_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec))
{ {
errmsg(rtk, "point pos error (%s)\n", msg); errmsg(rtk, "point pos error (%s)\n", msg);
if (!rtk->opt.dynamics) if (!rtk->opt.dynamics)
@ -2842,8 +2844,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
if (opt->mode == PMODE_MOVEB) if (opt->mode == PMODE_MOVEB)
{ /* moving baseline */ { /* moving baseline */
/* estimate position/velocity of base station */ /* estimate position/velocity of base station */
if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec,
iono_vec, pr_corrected_code_bias_vec)) iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec))
{ {
errmsg(rtk, "base station position error (%s)\n", msg); errmsg(rtk, "base station position error (%s)\n", msg);
return 0; return 0;

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

@ -178,7 +178,9 @@ 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> &tropo_vec,
std::vector<double> &iono_vec, std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec); std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec);
/** \} */ /** \} */

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

@ -599,8 +599,10 @@ void *rtksvrthread(void *arg)
std::vector<double> tropo_vec; std::vector<double> tropo_vec;
std::vector<double> iono_vec; std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec; std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual;
std::vector<double> doppler_residual;
rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec,
iono_vec, pr_corrected_code_bias_vec); iono_vec, pr_corrected_code_bias_vec,pr_residual,doppler_residual);
rtksvrunlock(svr); rtksvrunlock(svr);
if (svr->rtk.sol.stat != SOLQ_NONE) if (svr->rtk.sol.stat != SOLQ_NONE)