mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-09-29 07:20:51 +00:00
Code Issues Releases Wiki Activity

PVT block enabled to find ephemeris and observables from both GPS and Galileo

Browse Source
This commit is contained in:
marabra 2014-06-19 17:07:27 +02:00
parent 25934c477e
commit e7c4b35238
3 changed files with 317 additions and 225 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

@ -152,7 +152,7 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
//d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges)
d_TOW_at_curr_symbol_constellation=in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) d_TOW_at_curr_symbol_constellation=in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug)
d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges) d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges)
std::cout<<"Ch PVT = "<< i << ", d_TOW = " << d_TOW_at_curr_symbol_constellation<<", rx_time_hybrid_PVT = " << d_rx_time << " same RX timestamp (common RX time pseudoranges)"<< std::endl; std::cout<<"CH PVT = "<< i << ", d_TOW = " << d_TOW_at_curr_symbol_constellation<<", rx_time_hybrid_PVT = " << d_rx_time << " same RX timestamp (common RX time pseudoranges)"<< std::endl;
} }
} }
@ -205,7 +205,7 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
{ {
bool pvt_result; bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging); pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging);
std::cout << "pvt_result = " << pvt_result << std::endl;
if (pvt_result == true) if (pvt_result == true)
{ {

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

@ -40,7 +40,8 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels; d_nchannels = nchannels;
d_ephemeris = new Galileo_Navigation_Message[nchannels]; d_Gal_ephemeris = new Galileo_Navigation_Message[nchannels];
d_GPS_ephemeris = new Gps_Navigation_Message[nchannels];
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0; d_averaging_depth = 0;
@ -75,7 +76,8 @@ void hybrid_ls_pvt::set_averaging_depth(int depth)
hybrid_ls_pvt::~hybrid_ls_pvt() hybrid_ls_pvt::~hybrid_ls_pvt()
{ {
d_dump_file.close(); d_dump_file.close();
delete[] d_ephemeris; delete[] d_Gal_ephemeris;
delete[] d_GPS_ephemeris;
} }
@ -215,10 +217,11 @@ arma::vec hybrid_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::m
} }
bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging) bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging)
{ {
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter; std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter; std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
int valid_pseudoranges = gnss_pseudoranges_map.size(); int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
@ -226,7 +229,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
int Galileo_week_number = 0; int Galileo_week_number = 0;
int GPS_week;
double utc = 0; double utc = 0;
double utc_tx_corrected = 0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s)
double SV_clock_drift_s = 0; double SV_clock_drift_s = 0;
double SV_relativistic_clock_corr_s = 0; double SV_relativistic_clock_corr_s = 0;
double TX_time_corrected_s; double TX_time_corrected_s;
@ -243,79 +248,165 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++) 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())
{
/*!
* \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) if (gnss_pseudoranges_iter->second.System == 'E')
// first estimate of transmit time {
//Galileo_week_number = galileo_ephemeris_iter->second.WN_5;//for GST std::cout << "Satellite System: " << gnss_pseudoranges_iter->second.System <<std::endl;
//double sec_in_day = 86400; // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
//double day_in_week = 7; galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
// t = WN*sec_in_day*day_in_week + TOW; // t is Galileo System Time to use to compute satellite positions 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;
//JAVIER VERSION: // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
double Rx_time = galileo_current_time; // first estimate of transmit time
//Galileo_week_number = galileo_ephemeris_iter->second.WN_5;//for GST
//double sec_in_day = 86400;
//double day_in_week = 7;
// t = WN*sec_in_day*day_in_week + TOW; // t is Galileo System Time to use to compute satellite positions
//to compute satellite position we need GST = WN+TOW (everything expressed in seconds) //JAVIER VERSION:
//double Rx_time = galileo_current_time + Galileo_week_number*sec_in_day*day_in_week; double Rx_time = hybrid_current_time;
std::cout<<"Gal_Rx_time = "<< Rx_time << std::endl;
//to compute satellite position we need GST = WN+TOW (everything expressed in seconds)
//double Rx_time = galileo_current_time + Galileo_week_number*sec_in_day*day_in_week;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GALILEO_C_m_s; double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GALILEO_C_m_s;
//std::cout<<"Gal_Tx_time = "<< Tx_time << std::endl;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_drift_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 2- compute the clock drift using the clock model (broadcast) for this SV // 3- compute the relativistic clock drift using the clock model (broadcast) for this SV
SV_clock_drift_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time); SV_relativistic_clock_corr_s = galileo_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time);
// 3- compute the relativistic clock drift using the clock model (broadcast) for this SV // 4- compute the current ECEF position for this SV using corrected TX time
SV_relativistic_clock_corr_s = galileo_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time); SV_clock_bias_s = SV_clock_drift_s + SV_relativistic_clock_corr_s;
// 4- compute the current ECEF position for this SV using corrected TX time TX_time_corrected_s = Tx_time - SV_clock_bias_s;
SV_clock_bias_s = SV_clock_drift_s + SV_relativistic_clock_corr_s; //std::cout<<"Gal_TX_time_corrected_s = "<< TX_time_corrected_s << std::endl;
TX_time_corrected_s = Tx_time - SV_clock_bias_s; galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X; satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y; satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z; satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected pseudoranges // 5- 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; 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_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++; valid_obs++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
//debug //debug
double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, galileo_current_time); double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); // this shoud be removed and it should be considered the utc_tx_corrected
// get time string gregorian calendar utc_tx_corrected = galileo_utc_model.GST_to_UTC_time(TX_time_corrected_s, Galileo_week_number);
boost::posix_time::time_duration t = boost::posix_time::seconds(utc); //std::cout<<"Gal UTC at TX_time_corrected_s = "<<utc_tx_corrected<< std::endl;
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2) //std::cout<<"Gal_week = "<<Galileo_week_number<< std::endl;
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); //std::cout << "Gal UTC = " << utc << std::endl;
d_position_UTC_time = p_time; // get time string gregorian calendar
LOG(INFO) << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time); boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
//end debug // 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;
LOG(INFO) << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time);
//end debug
// SV ECEF DEBUG OUTPUT
LOG(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]";
std::cout<<"E"<<galileo_ephemeris_iter->second.i_satellite_PRN<< " satellite position computed"<< std::endl;
}
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;
}
}
else if (gnss_pseudoranges_iter->second.System == 'G')
{
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->first);
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;
std::cout<<"Gps_Rx_time = "<< Rx_time << std::endl;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GPS_C_m_s;
//std::cout<<"Gps_Tx_time = "<< Tx_time << std::endl;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_drift_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the relativistic clock drift using the clock model (broadcast) for this SV
SV_relativistic_clock_corr_s = gps_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time);
// 4- compute the current ECEF position for this SV using corrected TX time
SV_clock_bias_s = SV_clock_drift_s + SV_relativistic_clock_corr_s - gps_ephemeris_iter->second.d_TGD;
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++;
// SV ECEF DEBUG OUTPUT
LOG(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]";
std::cout<<"G"<<gps_ephemeris_iter->second.i_satellite_PRN<< " satellite position computed"<< std::endl;
// compute the UTC time for this SV (just to print the asociated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
//std::cout<<"GPS_week = "<< GPS_week << std::endl;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
//std::cout << "GPS UTC at TX_time_corrected_s= " << utc << std::endl;
}
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++;
std::cout<<"Number of observations in PVT = " << obs_counter << std::endl;
// SV ECEF DEBUG OUTPUT
LOG(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++;
} }
// ******************************************************************************** // ********************************************************************************
// ****** SOLVE LEAST SQUARES****************************************************** // ****** SOLVE LEAST SQUARES******************************************************
@ -325,163 +416,164 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
if (valid_obs >= 4) if (valid_obs >= 4)
{ {
arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
// Compute GST and Gregorian time // arma::vec mypos;
double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, galileo_current_time); // DLOG(INFO) << "satpos=" << satpos;
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); // DLOG(INFO) << "obs="<< obs;
// get time string Gregorian calendar // DLOG(INFO) << "W=" << W;
boost::posix_time::time_duration t = boost::posix_time::seconds(utc); // mypos = leastSquarePos(satpos, obs, W);
// 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); // // Compute GST and Gregorian time
d_position_UTC_time = p_time; // double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
LOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos; // utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// // get time string Gregorian calendar
cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4); // boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) // // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
if (d_height_m > 50000) // boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
{ // d_position_UTC_time = p_time;
b_valid_position = false; // LOG(INFO) << "Galileo Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos;
return false; //
} // cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4);
LOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time) // //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d // if (d_height_m > 50000)
<< " [deg], Height= " << d_height_m << " [m]"; // {
// b_valid_position = false;
std::cout << "Galileo Position at " << boost::posix_time::to_simple_string(p_time) // return false;
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d // }
<< " [deg], Height= " << d_height_m << " [m]" << std::endl; // LOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
// << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
// ###### Compute DOPs ######## // << " [deg], Height= " << d_height_m << " [m]";
// 1- Rotation matrix from ECEF coordinates to ENU coordinates //
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates // std::cout << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
arma::mat F = arma::zeros(3,3); // << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0)); // << " [deg], Height= " << d_height_m << " [m]" << std::endl;
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); //
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); // // ###### Compute DOPs ########
// // 1- Rotation matrix from ECEF coordinates to ENU coordinates
F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0); // // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0); // arma::mat F = arma::zeros(3,3);
F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0); // F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0));
// F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(2,0) = 0; // F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0); //
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0)); // F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0);
// F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) // F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2); //
arma::mat DOP_ENU = arma::zeros(3, 3); // F(2,0) = 0;
// F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
try // F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
{ //
DOP_ENU = arma::htrans(F)*Q_ECEF*F; // // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP // arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP // arma::mat DOP_ENU = arma::zeros(3, 3);
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP //
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP // try
d_TDOP = sqrt(d_Q(3,3)); // TDOP // {
} // DOP_ENU = arma::htrans(F)*Q_ECEF*F;
catch(std::exception& ex) // d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
{ // d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
d_GDOP = -1; // Geometric DOP // d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_PDOP = -1; // PDOP // d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_HDOP = -1; // HDOP // d_TDOP = sqrt(d_Q(3,3)); // TDOP
d_VDOP = -1; // VDOP // }
d_TDOP = -1; // TDOP // catch(std::exception& ex)
} // {
// d_GDOP = -1; // Geometric DOP
// ######## LOG FILE ######### // d_PDOP = -1; // PDOP
if(d_flag_dump_enabled == true) // d_HDOP = -1; // HDOP
{ // d_VDOP = -1; // VDOP
// MULTIPLEXED FILE RECORDING - Record results to file // d_TDOP = -1; // TDOP
try // }
{ //
double tmp_double; // // ######## LOG FILE #########
// PVT GPS time // if(d_flag_dump_enabled == true)
tmp_double = galileo_current_time; // {
d_dump_file.write((char*)&tmp_double, sizeof(double)); // // MULTIPLEXED FILE RECORDING - Record results to file
// ECEF User Position East [m] // try
tmp_double = mypos(0); // {
d_dump_file.write((char*)&tmp_double, sizeof(double)); // double tmp_double;
// ECEF User Position North [m] // // PVT GPS time
tmp_double = mypos(1); // tmp_double = hybrid_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m] // // ECEF User Position East [m]
tmp_double = mypos(2); // tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s] // // ECEF User Position North [m]
tmp_double = mypos(3); // tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg] // // ECEF User Position Up [m]
tmp_double = d_latitude_d; // tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg] // // User clock offset [s]
tmp_double = d_longitude_d; // tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m] // // GEO user position Latitude [deg]
tmp_double = d_height_m; // tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double)); // d_dump_file.write((char*)&tmp_double, sizeof(double));
} // // GEO user position Longitude [deg]
catch (const std::ifstream::failure& e) // tmp_double = d_longitude_d;
{ // d_dump_file.write((char*)&tmp_double, sizeof(double));
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what(); // // GEO user position Height [m]
} // tmp_double = d_height_m;
} // d_dump_file.write((char*)&tmp_double, sizeof(double));
// }
// MOVING AVERAGE PVT // catch (const std::ifstream::failure& e)
if (flag_averaging == true) // {
{ // LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) // }
{ // }
// Pop oldest value //
d_hist_longitude_d.pop_back(); // // MOVING AVERAGE PVT
d_hist_latitude_d.pop_back(); // if (flag_averaging == true)
d_hist_height_m.pop_back(); // {
// Push new values // if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
d_hist_longitude_d.push_front(d_longitude_d); // {
d_hist_latitude_d.push_front(d_latitude_d); // // Pop oldest value
d_hist_height_m.push_front(d_height_m); // d_hist_longitude_d.pop_back();
// d_hist_latitude_d.pop_back();
d_avg_latitude_d = 0; // d_hist_height_m.pop_back();
d_avg_longitude_d = 0; // // Push new values
d_avg_height_m = 0; // d_hist_longitude_d.push_front(d_longitude_d);
for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++) // d_hist_latitude_d.push_front(d_latitude_d);
{ // d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); //
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); // d_avg_latitude_d = 0;
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); // d_avg_longitude_d = 0;
} // d_avg_height_m = 0;
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth; // for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth; // {
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth; // d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
b_valid_position = true; // d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
return true; //indicates that the returned position is valid // d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} // }
else // d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
{ // d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
// int current_depth=d_hist_longitude_d.size(); // d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
// Push new values // b_valid_position = true;
d_hist_longitude_d.push_front(d_longitude_d); // return true; //indicates that the returned position is valid
d_hist_latitude_d.push_front(d_latitude_d); // }
d_hist_height_m.push_front(d_height_m); // else
// {
d_avg_latitude_d = d_latitude_d; // // int current_depth=d_hist_longitude_d.size();
d_avg_longitude_d = d_longitude_d; // // Push new values
d_avg_height_m = d_height_m; // d_hist_longitude_d.push_front(d_longitude_d);
b_valid_position = false; // d_hist_latitude_d.push_front(d_latitude_d);
return false; //indicates that the returned position is not valid yet // d_hist_height_m.push_front(d_height_m);
} //
} // d_avg_latitude_d = d_latitude_d;
else // d_avg_longitude_d = d_longitude_d;
{ // d_avg_height_m = d_height_m;
b_valid_position = true; // b_valid_position = false;
return true; //indicates that the returned position is valid // return false; //indicates that the returned position is not valid yet
} // }
} // }
// else
// {
// b_valid_position = true;
// return true; //indicates that the returned position is valid
// }
}
else else
{ {
b_valid_position = false; b_valid_position = false;

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

@ -74,8 +74,8 @@ public:
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
Galileo_Navigation_Message* d_ephemeris; Galileo_Navigation_Message* d_Gal_ephemeris;
//Gps_Navigation_Message* d_ephemeris; Gps_Navigation_Message* d_GPS_ephemeris;
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_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 Galileo_Ephemeris std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new Galileo_Ephemeris
@ -127,7 +127,7 @@ public:
~hybrid_ls_pvt(); ~hybrid_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging); bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double hybrid_current_time, bool flag_averaging);
/*! /*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical