mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-25 22:43:14 +00:00
Code Issues Releases Wiki Activity

Use L2C observables for positioning

Browse Source
This commit is contained in:
Carles Fernandez 2016-11-03 14:33:20 +01:00
parent 866bb1537f
commit 1c975313b7
5 changed files with 296 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -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

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

@ -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;
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;
// 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);
// 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);
// 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;
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;
// 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;
}
// 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)

6
src/algorithms/PVT/libs/hybrid_ls_pvt.h
View File

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

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

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

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

@ -44,7 +44,7 @@
class Gps_CNAV_Ephemeris
{
private:
double check_t(double time);
public:
unsigned int i_satellite_PRN; // SV PRN NUMBER
@ -164,6 +164,23 @@ public:
archive & make_nvp("b_antispoofing_flag", b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV
}
/*!
* \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
*/