mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-03-03 18:25:23 +00:00
Code Issues Releases Wiki Activity

GPS CNAV satellite positioning bug fixes.

Browse Source
This commit is contained in:
Javier Arribas 2017-03-29 18:32:17 +02:00
parent 5b83d828da
commit b745ebf0a8
6 changed files with 47 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b.conf
View File

@ -267,13 +267,13 @@ Resampler2.implementation=Pass_Through
;######### CHANNELS GLOBAL CONFIG ############ ;######### CHANNELS GLOBAL CONFIG ############
;#count: Number of available GPS satellite channels. ;#count: Number of available GPS satellite channels.
Channels_1C.count=8 Channels_1C.count=0
Channels_2S.count=8 Channels_2S.count=13
;#GPS.prns=7,8 ;#GPS.prns=7,8
;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver
Channels.in_acquisition=8 Channels.in_acquisition=12
;# signal: ;# signal:
;# "1C" GPS L1 C/A ;# "1C" GPS L1 C/A
@ -283,19 +283,19 @@ Channels.in_acquisition=8
;# CHANNEL NUMBERING ORDER: GPS L1 C/A, GPS L2 L2C (M), GALILEO E1 B, GALILEO E5a ;# CHANNEL NUMBERING ORDER: GPS L1 C/A, GPS L2 L2C (M), GALILEO E1 B, GALILEO E5a
;# CHANNEL CONNECTION ;# CHANNEL CONNECTION
Channel0.RF_channel_ID=0 Channel0.RF_channel_ID=1
Channel1.RF_channel_ID=0 Channel1.RF_channel_ID=1
Channel2.RF_channel_ID=0 Channel2.RF_channel_ID=1
Channel3.RF_channel_ID=0 Channel3.RF_channel_ID=1
Channel4.RF_channel_ID=0 Channel4.RF_channel_ID=1
Channel5.RF_channel_ID=0 Channel5.RF_channel_ID=1
Channel6.RF_channel_ID=0 Channel6.RF_channel_ID=1
Channel7.RF_channel_ID=0 Channel7.RF_channel_ID=1
Channel8.RF_channel_ID=0 Channel8.RF_channel_ID=1
Channel9.RF_channel_ID=1 Channel9.RF_channel_ID=1
Channel10.RF_channel_ID=1 Channel10.RF_channel_ID=1
Channel11.RF_channel_ID=1 Channel11.RF_channel_ID=1
Channel12.RF_channel_ID=1 Channel12.RF_channel_ID=0
Channel13.RF_channel_ID=1 Channel13.RF_channel_ID=1
Channel14.RF_channel_ID=1 Channel14.RF_channel_ID=1
Channel15.RF_channel_ID=1 Channel15.RF_channel_ID=1
@ -323,7 +323,7 @@ Acquisition_2S.dump_filename=./acq_dump.dat
Acquisition_2S.item_type=gr_complex Acquisition_2S.item_type=gr_complex
Acquisition_2S.if=0 Acquisition_2S.if=0
Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition
Acquisition_2S.threshold=0.0015 Acquisition_2S.threshold=0.00074
;Acquisition_2S.pfa=0.001 ;Acquisition_2S.pfa=0.001
Acquisition_2S.doppler_max=5000 Acquisition_2S.doppler_max=5000
Acquisition_2S.doppler_min=-5000 Acquisition_2S.doppler_min=-5000
@ -349,7 +349,7 @@ Tracking_1C.early_late_space_chips=0.5;
Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking
Tracking_2S.item_type=gr_complex Tracking_2S.item_type=gr_complex
Tracking_2S.if=0 Tracking_2S.if=0
Tracking_2S.dump=true Tracking_2S.dump=false
Tracking_2S.dump_filename=./tracking_ch_ Tracking_2S.dump_filename=./tracking_ch_
Tracking_2S.pll_bw_hz=2.0; Tracking_2S.pll_bw_hz=2.0;
Tracking_2S.dll_bw_hz=0.25; Tracking_2S.dll_bw_hz=0.25;

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

@ -177,12 +177,12 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not 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; 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 and obtain clock bias including relativistic effect // 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; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//std::cout<<"L1 Tx_time: "<<Tx_time<<" SV_clock_bias_s: "<<SV_clock_bias_s<<" dtr: "<<dtr<<std::endl;
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
@ -229,12 +229,11 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 2- compute the clock drift using the clock model (broadcast) for this SV // 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); 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 // 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
//std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl; //std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl;
double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//std::cout<<"L2 Tx_time: "<<Tx_time<<" SV_clock_bias_s: "<<SV_clock_bias_s<<" dtr: "<<dtr<<std::endl;
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
@ -243,7 +242,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 4- fill the observations vector with the corrected observables // 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s; obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;

6
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -150,7 +150,11 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
//update TOW at the preamble instant //update TOW at the preamble instant
d_TOW_at_Preamble=(int)msg.tow; d_TOW_at_Preamble=(int)msg.tow;
std::cout<<"["<<(int)msg.prn<<"] deco delay: "<<delay<<"[symbols]"<<std::endl; std::cout<<"["<<(int)msg.prn<<"] deco delay: "<<delay<<"[symbols]"<<std::endl;
d_TOW_at_current_symbol=(double)msg.tow * 6.0 + (double)delay * GPS_L2_M_PERIOD + GPS_L2_M_PERIOD; //* The time of the last input symbol can be computed from the message ToW and
//* delay by the formulae:
//* \code
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 20
d_TOW_at_current_symbol=((double)msg.tow) * 6.0 + ((double)delay) * GPS_L2_M_PERIOD +GPS_L2_M_PERIOD;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0; current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0;
d_flag_valid_word=true; d_flag_valid_word=true;

24
src/core/system_parameters/gps_cnav_ephemeris.cc
View File

@ -33,6 +33,7 @@
#include "gps_cnav_ephemeris.h" #include "gps_cnav_ephemeris.h"
#include <cmath> #include <cmath>
#include "GPS_L2C.h" #include "GPS_L2C.h"
#include <iostream>
Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris() Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris()
{ {
@ -153,7 +154,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI)); //M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
@ -193,11 +194,13 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
double r; double r;
double i; double i;
double Omega; double Omega;
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 163
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170
double d_sqrt_A = sqrt(A_REF + d_DELTA_A); 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 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 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] const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s]
// Find satellite's position ---------------------------------------------- // Find satellite's position ----------------------------------------------
@ -210,14 +213,19 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
// Computed mean motion // Computed mean motion
n0 = sqrt(GM / (a*a*a)); n0 = sqrt(GM / (a*a*a));
// Mean motion difference from computed value
double delta_n_a=d_Delta_n+0.5*d_DELTA_DOT_N*tk;
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; n = n0 + delta_n_a;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI)); //M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
@ -244,7 +252,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA; phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GPS_L2_PI)); //phi = fmod((phi), (2*GPS_L2_PI));
// Correct argument of latitude // Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi); u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
@ -256,10 +264,11 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi); 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 // Compute the angle between the ascending node and the Greenwich meridian
double d_OMEGA_DOT = OMEGA_DOT_REF*GPS_L2_PI + d_DELTA_OMEGA_DOT;
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe1; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe1;
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI)); //Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -281,5 +290,4 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s); dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s);
return dtr_s; return dtr_s;
} }

6
src/core/system_parameters/gps_cnav_ephemeris.h
View File

@ -153,9 +153,9 @@ public:
archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s]
archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week]
archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
archive & make_nvp("d_DELTA_A", d_DELTA_A); archive & make_nvp("d_DELTA_A", d_DELTA_A); //!< Semi-major axis difference at reference time [m]
archive & make_nvp("d_A_DOT", d_A_DOT); archive & make_nvp("d_A_DOT", d_A_DOT); //!< Change rate in semi-major axis [m/s]
archive & make_nvp("d_DELTA_OMEGA_DOT", d_DELTA_OMEGA_DOT); //!< Rate of Right Ascension difference [semi-circles/s]
archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s] archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s]
archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s] archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s]
archive & make_nvp("d_A_f2", d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2] archive & make_nvp("d_A_f2", d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2]

8
src/core/system_parameters/gps_ephemeris.cc
View File

@ -156,7 +156,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); //M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
@ -215,7 +215,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); //M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
@ -242,7 +242,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA; phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2.0 * GPS_PI)); //phi = fmod((phi), (2.0 * GPS_PI));
// Correct argument of latitude // Correct argument of latitude
u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi);
@ -257,7 +257,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI)); //Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);