More improvements in the PVT algorithm for better observables estimations

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_ls_pvt->d_rx_dt_s*GPS_C_m_s<<std::endl;
                             if( first_fix == true)
                                 {

src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc

@@ -115,19 +115,22 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> 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++;

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

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