[hamradio-commits] [gnss-sdr] 142/303: Use L2C observables for positioning
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:35:55 UTC 2017
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.
commit 1c975313b761e8a8ad9c23214a3d73f4ab3e8b5e
Author: Carles Fernandez <carlesfernandez at gmail.com>
Date: Thu Nov 3 14:33:20 2016 +0100
Use L2C observables for positioning
---
.../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 4 +-
src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 159 +++++++++++-------
src/algorithms/PVT/libs/hybrid_ls_pvt.h | 6 +-
src/core/system_parameters/gps_cnav_ephemeris.cc | 181 ++++++++++++++++++++-
src/core/system_parameters/gps_cnav_ephemeris.h | 19 ++-
5 files changed, 302 insertions(+), 67 deletions(-)
diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
index 35cef19..a8a7273 100644
--- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
+++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
@@ -877,10 +877,10 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
<< " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]";
- std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
+ /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = "
<< d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP
- << " GDOP = " << d_ls_pvt->d_GDOP << std::endl;
+ << " GDOP = " << d_ls_pvt->d_GDOP << std::endl; */
}
// MULTIPLEXED FILE RECORDING - Record results to file
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index d9c663d..9591dad 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -40,13 +40,9 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
- //d_Gal_ephemeris = new Galileo_Navigation_Message[nchannels];
- //d_GPS_ephemeris = new Gps_Navigation_Message[nchannels];
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_galileo_current_time = 0;
- d_valid_GPS_obs = 0;
- d_valid_GAL_obs = 0;
count_valid_position = 0;
d_flag_averaging = false;
// ############# ENABLE DATA FILE LOG #################
@@ -72,9 +68,6 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
hybrid_ls_pvt::~hybrid_ls_pvt()
{
d_dump_file.close();
- //delete[] d_Gal_ephemeris;
- //delete[] d_GPS_ephemeris;
- //delete[]
}
@@ -106,8 +99,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter = 0;
- int valid_obs_GPS_counter = 0;
- int valid_obs_GALILEO_counter = 0;
+
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
@@ -143,7 +135,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
- valid_obs_GALILEO_counter ++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
@@ -169,52 +160,103 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
{
//std::cout << "Satellite System: " << gnss_pseudoranges_iter->second.System <<std::endl;
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
- gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN);
- if (gps_ephemeris_iter != gps_ephemeris_map.end())
- {
- /*!
- * \todo Place here the satellite CN0 (power level, or weight factor)
- */
- W(obs_counter, obs_counter) = 1;
-
- // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
- // first estimate of transmit time
- double Rx_time = hybrid_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
- SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
-
- // 3- compute the current ECEF position for this SV using corrected TX time
- TX_time_corrected_s = Tx_time - SV_clock_bias_s;
- gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
-
- satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
- satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
- satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
-
- // 5- fill the observations vector with the corrected pseudorranges
- obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_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++;
- valid_obs_GPS_counter++;
- GPS_week = gps_ephemeris_iter->second.i_GPS_week;
-
- // SV ECEF DEBUG OUTPUT
- DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
- << " X=" << gps_ephemeris_iter->second.d_satpos_X
- << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
- << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
- << " [m] PR_obs=" << obs(obs_counter) << " [m]";
- }
- else // the ephemeris are not available for this SV
- {
- // no valid pseudorange for the current SV
- W(obs_counter, obs_counter) = 0; // SV de-activated
- obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
- DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->second.PRN;
- }
+ std::string sig_(gnss_pseudoranges_iter->second.Signal);
+ if(sig_.compare("1C") == 0)
+ {
+ gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN);
+ if (gps_ephemeris_iter != gps_ephemeris_map.end())
+ {
+ /*!
+ * \todo Place here the satellite CN0 (power level, or weight factor)
+ */
+ W(obs_counter, obs_counter) = 1;
+
+ // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
+ // first estimate of transmit time
+ double Rx_time = hybrid_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
+ SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
+
+ // 3- compute the current ECEF position for this SV using corrected TX time
+ TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+ gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+ satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
+ satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
+ satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
+
+ // 5- fill the observations vector with the corrected pseudoranges
+ obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_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++;
+ GPS_week = gps_ephemeris_iter->second.i_GPS_week;
+
+ // SV ECEF DEBUG OUTPUT
+ DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
+ << " X=" << gps_ephemeris_iter->second.d_satpos_X
+ << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
+ << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
+ << " [m] PR_obs=" << obs(obs_counter) << " [m]";
+ }
+ else // the ephemeris are not available for this SV
+ {
+ // no valid pseudorange for the current SV
+ W(obs_counter, obs_counter) = 0; // SV de-activated
+ obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
+ DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->second.PRN;
+ }
+ }
+ if(sig_.compare("2S") == 0)
+ {
+ gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN);
+ if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
+ {
+ /*!
+ * \todo Place here the satellite CN0 (power level, or weight factor)
+ */
+ W(obs_counter, obs_counter) = 1;
+
+ // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
+ // first estimate of transmit time
+ double Rx_time = hybrid_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
+ SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
+
+ // 3- compute the current ECEF position for this SV using corrected TX time
+ TX_time_corrected_s = Tx_time - SV_clock_bias_s;
+ gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
+
+ satpos(0, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_X;
+ satpos(1, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
+ satpos(2, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
+
+ // 5- fill the observations vector with the corrected pseudoranges
+ obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
+ d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
+ d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
+ valid_obs++;
+ GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
+
+ // SV ECEF DEBUG OUTPUT
+ DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
+ << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
+ << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
+ << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
+ << " [m] PR_obs=" << obs(obs_counter) << " [m]";
+ }
+ else // the ephemeris are not available for this SV
+ {
+ // no valid pseudorange for the current SV
+ W(obs_counter, obs_counter) = 0; // SV de-activated
+ obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
+ DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->second.PRN;
+ }
+ }
}
obs_counter++;
}
@@ -223,8 +265,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
d_valid_observations = valid_obs;
- d_valid_GPS_obs = valid_obs_GPS_counter;
- d_valid_GAL_obs = valid_obs_GALILEO_counter;
+
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if(valid_obs >= 4)
@@ -235,7 +276,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
- d_rx_dt_m = mypos(3)/GPS_C_m_s; // Convert RX time offset from meters to seconds
+ d_rx_dt_m = mypos(3) / GPS_C_m_s; // Convert RX time offset from meters to seconds
double secondsperweek = 604800.0;
// Compute GST and Gregorian time
if( GST != 0.0)
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h
index a7bd042..e078cf6 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h
@@ -58,15 +58,15 @@ public:
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging);
int d_nchannels; //!< Number of available channels for positioning
- int d_valid_GPS_obs; //!< Number of valid GPS pseudorange observations (valid GPS satellites) -- used for hybrid configuration
- int d_valid_GAL_obs; //!< Number of valid GALILEO pseudorange observations (valid GALILEO satellites) -- used for hybrid configuration
+ //int d_valid_GPS_obs; //!< Number of valid GPS pseudorange observations (valid GPS satellites) -- used for hybrid configuration
+ //int d_valid_GAL_obs; //!< Number of valid GALILEO pseudorange observations (valid GALILEO satellites) -- used for hybrid configuration
//Galileo_Navigation_Message* d_Gal_ephemeris;
//Gps_Navigation_Message* d_GPS_ephemeris;
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
- std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
+ std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
Galileo_Utc_Model galileo_utc_model;
Galileo_Iono galileo_iono;
Galileo_Almanac galileo_almanac;
diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc
index ac911a1..3109801 100644
--- a/src/core/system_parameters/gps_cnav_ephemeris.cc
+++ b/src/core/system_parameters/gps_cnav_ephemeris.cc
@@ -31,6 +31,8 @@
*/
#include "gps_cnav_ephemeris.h"
+#include <cmath>
+#include "GPS_L2C.h"
Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris()
{
@@ -46,8 +48,6 @@ Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris()
d_e_eccentricity = 0;
d_Cus = 0;
- d_Toe1 = 0;
- d_Toe2 = 0;
d_Toc = 0;
d_Cic = 0;
d_OMEGA0 = 0;
@@ -95,3 +95,180 @@ Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris()
d_ISCL5Q = 0.0;
b_l2c_phasing_flag = false;
}
+
+double Gps_CNAV_Ephemeris::check_t(double time)
+{
+ double corrTime;
+ double half_week = 302400.0; // seconds
+ corrTime = time;
+ if (time > half_week)
+ {
+ corrTime = time - 2.0 * half_week;
+ }
+ else if (time < -half_week)
+ {
+ corrTime = time + 2.0 * half_week;
+ }
+ return corrTime;
+}
+
+
+// 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
+double Gps_CNAV_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);
+ return d_satClkDrift;
+}
+
+
+// compute the relativistic correction term
+double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
+{
+ double tk;
+ double a;
+ double n;
+ double n0;
+ double E;
+ double E_old;
+ double dE;
+ double M;
+ const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
+ const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
+ const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 163
+ double d_sqrt_A = sqrt(A_REF + d_DELTA_A);
+
+ // Restore semi-major axis
+ a = d_sqrt_A * d_sqrt_A;
+
+ // Time from ephemeris reference epoch
+ tk = check_t(transmitTime - d_Toe1);
+
+ // Computed mean motion
+ n0 = sqrt(GM / (a * a * a));
+ // Corrected mean motion
+ n = n0 + d_Delta_n;
+ // Mean anomaly
+ M = d_M_0 + n * tk;
+
+ // Reduce mean anomaly to between 0 and 2pi
+ M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI));
+
+ // Initial guess of eccentric anomaly
+ E = M;
+
+ // --- Iteratively compute eccentric anomaly ----------------------------
+ for (int ii = 1; ii < 20; ii++)
+ {
+ E_old = E;
+ E = M + d_e_eccentricity * sin(E);
+ dE = fmod(E - E_old, 2.0 * GPS_L2_PI);
+ if (fabs(dE) < 1e-12)
+ {
+ //Necessary precision is reached, exit from the loop
+ break;
+ }
+ }
+
+ // Compute relativistic correction term
+ d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
+ return d_dtr;
+}
+
+
+void Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
+{
+ double tk;
+ double a;
+ double n;
+ double n0;
+ double M;
+ double E;
+ double E_old;
+ double dE;
+ double nu;
+ double phi;
+ double u;
+ double r;
+ double i;
+ double Omega;
+ const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 163
+ double d_sqrt_A = sqrt(A_REF + d_DELTA_A);
+ const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
+ const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164
+ double d_OMEGA_DOT = OMEGA_DOT_REF + d_DELTA_OMEGA_DOT;
+ const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s]
+ // Find satellite's position ----------------------------------------------
+
+ // Restore semi-major axis
+ a = d_sqrt_A*d_sqrt_A;
+
+ // Time from ephemeris reference epoch
+ tk = check_t(transmitTime - d_Toe1);
+
+ // Computed mean motion
+ n0 = sqrt(GM / (a*a*a));
+
+ // Corrected mean motion
+ n = n0 + d_Delta_n;
+
+ // Mean anomaly
+ M = d_M_0 + n * tk;
+
+ // Reduce mean anomaly to between 0 and 2pi
+ M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI));
+
+ // Initial guess of eccentric anomaly
+ E = M;
+
+ // --- Iteratively compute eccentric anomaly ----------------------------
+ for (int ii = 1; ii < 20; ii++)
+ {
+ E_old = E;
+ E = M + d_e_eccentricity * sin(E);
+ dE = fmod(E - E_old, 2 * GPS_L2_PI);
+ if (fabs(dE) < 1e-12)
+ {
+ //Necessary precision is reached, exit from the loop
+ break;
+ }
+ }
+
+ // Compute the true anomaly
+ double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E);
+ double tmp_X = cos(E) - d_e_eccentricity;
+ nu = atan2(tmp_Y, tmp_X);
+
+ // Compute angle phi (argument of Latitude)
+ phi = nu + d_OMEGA;
+
+ // Reduce phi to between 0 and 2*pi rad
+ phi = fmod((phi), (2*GPS_L2_PI));
+
+ // Correct argument of latitude
+ u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
+
+ // Correct radius
+ r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
+
+ // Correct inclination
+ i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi);
+
+ // Compute the angle between the ascending node and the Greenwich meridian
+ Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe1;
+
+ // Reduce to between 0 and 2*pi rad
+ Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI));
+
+ // --- Compute satellite coordinates in Earth-fixed coordinates
+ d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
+ d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
+ d_satpos_Z = sin(u) * r * sin(i);
+
+ // Satellite's velocity. Can be useful for Vector Tracking loops
+ double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT;
+ 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);
+}
diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h
index fabd211..46e94cb 100644
--- a/src/core/system_parameters/gps_cnav_ephemeris.h
+++ b/src/core/system_parameters/gps_cnav_ephemeris.h
@@ -44,7 +44,7 @@
class Gps_CNAV_Ephemeris
{
private:
-
+ double check_t(double time);
public:
unsigned int i_satellite_PRN; // SV PRN NUMBER
@@ -165,6 +165,23 @@ public:
}
/*!
+ * \brief Compute the ECEF SV coordinates and ECEF velocity
+ * Implementation of Table 20-IV (IS-GPS-200E)
+ */
+ void satellitePosition(double transmitTime);
+
+ /*!
+ * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
+ * (IS-GPS-200E, 20.3.3.3.3.1)
+ */
+ double sv_clock_drift(double transmitTime);
+
+ /*!
+ * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
+ * (IS-GPS-200E, 20.3.3.3.3.1)
+ */
+ double sv_clock_relativistic_term(double transmitTime);
+ /*!
* Default constructor
*/
Gps_CNAV_Ephemeris();
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list