From 888bc17dbe66f30eb6e12c94495deb978b85a789 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 30 Jan 2017 19:03:18 +0100 Subject: [PATCH] More improvements in the PVT algorithm for better observables estimations --- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 4 ---- src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 11 ++++++---- src/core/system_parameters/gps_ephemeris.cc | 21 ++++++++++++++++--- src/core/system_parameters/gps_ephemeris.h | 3 ++- 4 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 8c774fa87..a59d339bb 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -379,10 +379,6 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g { it->second.Pseudorange_m=it->second.Pseudorange_m-d_ls_pvt->d_rx_dt_s*GPS_C_m_s; } - // send asynchronous message to observables block - // time offset is expressed as the equivalent travel distance [m] - //pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s); - //this->message_port_pub(pmt::mp("rx_dt_s"), value); //std::cout<<"d_rx_dt_s*GPS_C_m_s="<d_rx_dt_s*GPS_C_m_s< gnss_pseudoranges_map, double Rx_time = GPS_current_time; double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s; - // 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect + // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; - // 3- compute the current ECEF position for this SV using corrected TX time + // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect TX_time_corrected_s = Tx_time - SV_clock_bias_s; - gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); + double dtr=gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); + + //store satellite positions in a matrix satpos.resize(3,valid_obs+1); satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; + // 4- fill the observations vector with the corrected pseudoranges obs.resize(valid_obs+1,1); - obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s; + obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; valid_obs++; diff --git a/src/core/system_parameters/gps_ephemeris.cc b/src/core/system_parameters/gps_ephemeris.cc index 392fb57f4..ea803150a 100644 --- a/src/core/system_parameters/gps_ephemeris.cc +++ b/src/core/system_parameters/gps_ephemeris.cc @@ -119,7 +119,12 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime) { double dt; dt = check_t(transmitTime - d_Toc); - d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime); + + for (int i=0;i<2;i++) { + dt-=d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + } + d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + return d_satClkDrift; } @@ -174,7 +179,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime) } -void Gps_Ephemeris::satellitePosition(double transmitTime) +double Gps_Ephemeris::satellitePosition(double transmitTime) { double tk; double a; @@ -197,7 +202,7 @@ void Gps_Ephemeris::satellitePosition(double transmitTime) a = d_sqrt_A*d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toc); + tk = check_t(transmitTime - d_Toe); // Computed mean motion n0 = sqrt(GM / (a*a*a)); @@ -263,4 +268,14 @@ void Gps_Ephemeris::satellitePosition(double transmitTime) d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); d_satvel_Z = d_satpos_Y * sin(i); + + // Time from ephemeris reference clock + tk = check_t(transmitTime - d_Toc); + + double dtr_s=d_A_f0+d_A_f1*tk+d_A_f2*tk*tk; + + /* relativity correction */ + dtr_s-=2.0*sqrt(GM*a)*d_e_eccentricity*sin(E)/(GPS_C_m_s*GPS_C_m_s); + + return dtr_s; } diff --git a/src/core/system_parameters/gps_ephemeris.h b/src/core/system_parameters/gps_ephemeris.h index 78fc3cfc5..c71fde0b6 100644 --- a/src/core/system_parameters/gps_ephemeris.h +++ b/src/core/system_parameters/gps_ephemeris.h @@ -183,8 +183,9 @@ public: /*! * \brief Compute the ECEF SV coordinates and ECEF velocity * Implementation of Table 20-IV (IS-GPS-200E) + * and compute the clock bias term including relativistic effect (return value) */ - void satellitePosition(double transmitTime); + double satellitePosition(double transmitTime); /*! * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction