mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-04-05 02:07:02 +00:00
Upgrading Galileo PVT and Hybrid PVT chains with the latest bug fixes for GPS
This commit is contained in:
Javier Arribas
parent
b8ba57b9cf
commit
fc7ff7ba0b
@ -287,6 +287,11 @@ int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)),
|
||||
|
||||
if (pvt_result == true)
|
||||
{
|
||||
// correct the observable to account for the receiver clock offset
|
||||
for (std::map<int,Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
|
||||
{
|
||||
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s;
|
||||
}
|
||||
if( first_fix == true)
|
||||
{
|
||||
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
|
||||
|
||||
@ -455,6 +455,22 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
|
||||
|
||||
if (pvt_result == true)
|
||||
{
|
||||
// correct the observable to account for the receiver clock offset
|
||||
for (std::map<int,Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
|
||||
{
|
||||
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s;
|
||||
}
|
||||
if(first_fix == true)
|
||||
{
|
||||
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
|
||||
<< " UTC 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::endl;
|
||||
ttff_msgbuf ttff;
|
||||
ttff.mtype = 1;
|
||||
ttff.ttff = d_sample_counter;
|
||||
send_sys_v_ttff_msg(ttff);
|
||||
first_fix = false;
|
||||
}
|
||||
d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
|
||||
d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
|
||||
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
|
||||
|
||||
@ -48,21 +48,21 @@ galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, b
|
||||
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -78,11 +78,10 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
||||
{
|
||||
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
|
||||
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
|
||||
int valid_pseudoranges = gnss_pseudoranges_map.size();
|
||||
|
||||
arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); // channels weights matrix
|
||||
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
|
||||
arma::mat satpos = arma::zeros(3, valid_pseudoranges); // satellite positions matrix
|
||||
arma::vec W; // channels weight vector
|
||||
arma::vec obs; // pseudoranges observation vector
|
||||
arma::mat satpos; // satellite positions matrix
|
||||
|
||||
int Galileo_week_number = 0;
|
||||
double utc = 0.0;
|
||||
@ -96,60 +95,62 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
||||
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
|
||||
// ********************************************************************************
|
||||
int valid_obs = 0; //valid observations counter
|
||||
int obs_counter = 0;
|
||||
|
||||
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
|
||||
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
|
||||
gnss_pseudoranges_iter++)
|
||||
{
|
||||
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W(obs_counter, obs_counter) = 1.0;
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 1;
|
||||
|
||||
// COMMON RX TIME PVT ALGORITHM
|
||||
double Rx_time = galileo_current_time;
|
||||
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
|
||||
// COMMON RX TIME PVT ALGORITHM
|
||||
double Rx_time = galileo_current_time;
|
||||
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
|
||||
|
||||
// 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
|
||||
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
|
||||
// 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
|
||||
SV_clock_bias_s = galileo_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;
|
||||
galileo_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;
|
||||
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
|
||||
|
||||
satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
|
||||
//store satellite positions in a matrix
|
||||
satpos.resize(3, valid_obs + 1);
|
||||
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
|
||||
|
||||
// 4- fill the observations vector with the corrected pseudoranges
|
||||
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s;
|
||||
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++;
|
||||
// 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 * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
|
||||
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;
|
||||
|
||||
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
|
||||
GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << galileo_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->first;
|
||||
}
|
||||
obs_counter++;
|
||||
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
|
||||
GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
|
||||
}
|
||||
}
|
||||
|
||||
// ********************************************************************************
|
||||
// ****** SOLVE LEAST SQUARES******************************************************
|
||||
@ -158,78 +159,91 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
||||
LOG(INFO) << "Galileo PVT: valid observations=" << valid_obs;
|
||||
|
||||
if (valid_obs >= 4)
|
||||
{
|
||||
arma::vec rx_position_and_time;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs="<< obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
arma::vec mypos;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs="<< obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
mypos = leastSquarePos(satpos, obs, W);
|
||||
|
||||
// Compute Gregorian time
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos;
|
||||
|
||||
cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
|
||||
d_rx_dt_s = mypos(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
|
||||
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = galileo_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = mypos(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = mypos(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = mypos(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = mypos(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GALILEO_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
|
||||
// Compute Gregorian time
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time;
|
||||
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
|
||||
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = galileo_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
||||
}
|
||||
else
|
||||
{
|
||||
b_valid_position = false;
|
||||
}
|
||||
{
|
||||
b_valid_position = false;
|
||||
}
|
||||
return b_valid_position;
|
||||
}
|
||||
|
||||
|
||||
@ -133,7 +133,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
||||
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++;
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
@ -142,6 +141,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
||||
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
|
||||
|
||||
@ -47,21 +47,21 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
|
||||
d_flag_averaging = false;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -78,11 +78,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
|
||||
std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
|
||||
|
||||
int valid_observables = gnss_observables_map.size();
|
||||
|
||||
arma::mat W = arma::eye(valid_observables, valid_observables); // channels weights matrix
|
||||
arma::vec obs = arma::zeros(valid_observables); // observables observation vector
|
||||
arma::mat satpos = arma::zeros(3, valid_observables); // satellite positions matrix
|
||||
arma::vec W; // channels weight vector
|
||||
arma::vec obs; // pseudoranges observation vector
|
||||
arma::mat satpos; // satellite positions matrix
|
||||
|
||||
int Galileo_week_number = 0;
|
||||
int GPS_week = 0;
|
||||
@ -98,168 +96,184 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
|
||||
// ********************************************************************************
|
||||
int valid_obs = 0; //valid observations counter
|
||||
int obs_counter = 0;
|
||||
|
||||
for(gnss_observables_iter = gnss_observables_map.begin();
|
||||
gnss_observables_iter != gnss_observables_map.end();
|
||||
gnss_observables_iter++)
|
||||
{
|
||||
switch(gnss_observables_iter->second.System)
|
||||
{
|
||||
if(gnss_observables_iter->second.System == 'E')
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W(obs_counter, obs_counter) = 1;
|
||||
case 'E':
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 1;
|
||||
|
||||
// COMMON RX TIME PVT ALGORITHM
|
||||
double Rx_time = hybrid_current_time;
|
||||
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
|
||||
// COMMON RX TIME PVT ALGORITHM
|
||||
double Rx_time = hybrid_current_time;
|
||||
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
|
||||
|
||||
// 2- compute the clock drift using the clock model (broadcast) for this SV
|
||||
SV_clock_bias_s = galileo_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 = galileo_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;
|
||||
galileo_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;
|
||||
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
|
||||
|
||||
satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
|
||||
//store satellite positions in a matrix
|
||||
satpos.resize(3, valid_obs + 1);
|
||||
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
|
||||
|
||||
// 5- fill the observations vector with the corrected observables
|
||||
obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s;
|
||||
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
||||
valid_obs++;
|
||||
|
||||
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);
|
||||
// 4- fill the observations vector with the corrected observables
|
||||
obs.resize(valid_obs + 1, 1);
|
||||
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
|
||||
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << galileo_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_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
|
||||
else if(gnss_observables_iter->second.System == 'G')
|
||||
{
|
||||
//std::cout << "Satellite System: " << gnss_observables_iter->second.System <<std::endl;
|
||||
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
if(sig_.compare("1C") == 0)
|
||||
{
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_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;
|
||||
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);
|
||||
|
||||
// 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_observables_iter->second.Pseudorange_m / GPS_C_m_s;
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
|
||||
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
|
||||
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
|
||||
|
||||
// 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);
|
||||
valid_obs++;
|
||||
}
|
||||
|
||||
// 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 observables
|
||||
obs(obs_counter) = gnss_observables_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_observables_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_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
if(sig_.compare("2S") == 0)
|
||||
{
|
||||
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_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_observables_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 observables
|
||||
obs(obs_counter) = gnss_observables_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_observables_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_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
}
|
||||
obs_counter++;
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'G':
|
||||
{
|
||||
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
if(sig_.compare("1C") == 0)
|
||||
{
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 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_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
|
||||
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 and obtain clock bias including relativistic effect
|
||||
TX_time_corrected_s = Tx_time - SV_clock_bias_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_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_ephemeris_iter->second.i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
||||
|
||||
// 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(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
|
||||
}
|
||||
}
|
||||
if(sig_.compare("2S") == 0)
|
||||
{
|
||||
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_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.resize(valid_obs + 1, 1);
|
||||
W(valid_obs) = 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_observables_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);
|
||||
|
||||
//store satellite positions in a matrix
|
||||
satpos.resize(3, valid_obs + 1);
|
||||
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
|
||||
satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
|
||||
satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
|
||||
|
||||
// 4- fill the observations vector with the corrected observables
|
||||
obs.resize(valid_obs + 1, 1);
|
||||
obs(valid_obs) = gnss_observables_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_observables_iter->second.CN0_dB_hz;
|
||||
|
||||
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(valid_obs) << " [m]";
|
||||
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default :
|
||||
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// ********************************************************************************
|
||||
// ****** SOLVE LEAST SQUARES******************************************************
|
||||
@ -269,86 +283,101 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
|
||||
|
||||
if(valid_obs >= 4)
|
||||
{
|
||||
arma::vec rx_position_and_time;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs=" << obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
// check if this is the initial position computation
|
||||
if (d_rx_dt_s == 0)
|
||||
{
|
||||
// execute Bancroft's algorithm to estimate initial receiver position and time
|
||||
DLOG(INFO) << " Executing Bancroft algorithm...";
|
||||
rx_position_and_time = bancroftPos(satpos.t(), obs);
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
|
||||
}
|
||||
|
||||
// Execute WLS using previous position as the initialization point
|
||||
rx_position_and_time = leastSquarePos(satpos, obs, W);
|
||||
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
|
||||
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
|
||||
|
||||
double secondsperweek = 604800.0;
|
||||
// Compute GST and Gregorian time
|
||||
if( GST != 0.0)
|
||||
{
|
||||
arma::vec mypos;
|
||||
DLOG(INFO) << "satpos=" << satpos;
|
||||
DLOG(INFO) << "obs=" << obs;
|
||||
DLOG(INFO) << "W=" << W;
|
||||
|
||||
mypos = leastSquarePos(satpos, obs, W);
|
||||
d_rx_dt_s = 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)
|
||||
{
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
}
|
||||
else
|
||||
{
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
|
||||
}
|
||||
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
DLOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos;
|
||||
|
||||
cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
hybrid_ls_pvt::compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = hybrid_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = mypos(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = mypos(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = mypos(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = mypos(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
pos_averaging(flag_averaging);
|
||||
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
|
||||
}
|
||||
else
|
||||
{
|
||||
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
|
||||
}
|
||||
|
||||
// get time string Gregorian calendar
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
|
||||
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
d_position_UTC_time = p_time;
|
||||
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
|
||||
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ###### Compute DOPs ########
|
||||
hybrid_ls_pvt::compute_DOP();
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = hybrid_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVING AVERAGE PVT
|
||||
pos_averaging(flag_averaging);
|
||||
}
|
||||
else
|
||||
{
|
||||
b_valid_position = false;
|
||||
}
|
||||
{
|
||||
b_valid_position = false;
|
||||
}
|
||||
return b_valid_position;
|
||||
}
|
||||
|
||||
@ -196,7 +196,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
|
||||
DLOG(INFO) << "d_TOW_hybrid_reference [ms] = " << d_TOW_reference * 1000;
|
||||
double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
|
||||
DLOG(INFO) << "ref_PRN_rx_time_ms [ms] = " << d_ref_PRN_rx_time_ms;
|
||||
//int reference_channel= gnss_synchro_iter->second.Channel_ID;
|
||||
|
||||
// Now compute RX time differences due to the PRN alignment in the correlators
|
||||
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
|
||||
@ -235,8 +234,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
|
||||
dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
|
||||
|
||||
desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0;
|
||||
// arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
|
||||
// arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
|
||||
|
||||
// Curve fitting to cuadratic function
|
||||
arma::mat A = arma::ones<arma::mat> (history_deep, 2);
|
||||
@ -249,10 +246,8 @@ int hybrid_observables_cc::general_work (int noutput_items,
|
||||
coef_doppler = pinv_A * dopper_vec_hz;
|
||||
arma::vec acc_phase_lin;
|
||||
arma::vec carrier_doppler_lin;
|
||||
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; // +coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
|
||||
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; // +coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
|
||||
//std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
|
||||
//std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
|
||||
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];
|
||||
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];
|
||||
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0];
|
||||
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0];
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user