From 8c2f1f992f34af2fd41d75ecf0b1fa653292ed51 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 6 Feb 2017 17:51:11 +0100 Subject: [PATCH] Adding consistency checks to the PVT solutions --- src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 136 ++++----- src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 218 ++++++++------- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 156 ++++++----- src/algorithms/PVT/libs/ls_pvt.cc | 280 +++++++++---------- 4 files changed, 405 insertions(+), 385 deletions(-) diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc index 8f60917a2..4b08cd9e7 100644 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc +++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc @@ -164,81 +164,87 @@ bool galileo_e1_ls_pvt::get_PVT(std::map gnss_pseudoranges_map DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "obs="<< obs; DLOG(INFO) << "W=" << W; + try{ + // 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) / 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); - // 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) / 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_dt_s += rx_position_and_time(3) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] - 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; - // 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; - DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time; + cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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]"; - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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(); - // ###### 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(); + } + } - // ######## LOG FILE ######### - if(d_flag_dump_enabled == true) + // MOVING AVERAGE PVT + galileo_e1_ls_pvt::pos_averaging(flag_averaging); + }catch(const std::exception & e) { - // 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(); - } + d_rx_dt_s=0;//reset rx time estimation + LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what(); + b_valid_position = false; } - - // MOVING AVERAGE PVT - galileo_e1_ls_pvt::pos_averaging(flag_averaging); } else { diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc index 1633f6f42..72d87d851 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -49,21 +49,21 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, boo // ############# 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(); + } } + } } @@ -99,58 +99,58 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, 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 + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first); + if (gps_ephemeris_iter != gps_ephemeris_map.end()) { - // 1- find the ephemeris for the current 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.resize(valid_obs + 1, 1); - W(valid_obs) = 1; + /*! + * \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 = GPS_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 = GPS_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, not including relativistic effect - SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; + // 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); + // 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; + //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_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; + // 4- fill the observations vector with the corrected pseudoranges + obs.resize(valid_obs + 1, 1); + 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; - // 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]"; + // 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_pseudoranges_iter->first; - } + 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_pseudoranges_iter->first; + } + } // ******************************************************************************** // ****** SOLVE LEAST SQUARES****************************************************** @@ -159,21 +159,22 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, LOG(INFO) << "(new)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; + { + arma::vec rx_position_and_time; + DLOG(INFO) << "satpos=" << satpos; + DLOG(INFO) << "obs=" << obs; + DLOG(INFO) << "W=" << W; + try{ // 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 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); @@ -193,57 +194,64 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); d_position_UTC_time = p_time; DLOG(INFO) << "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]"; + << " 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 { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - // PVT GPS time - tmp_double = GPS_current_time; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position East [m] - tmp_double = d_rx_pos(0); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position North [m] - tmp_double = d_rx_pos(1); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position Up [m] - tmp_double = d_rx_pos(2); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // User clock offset [s] - tmp_double = d_rx_dt_s; - 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(); - } + double tmp_double; + // PVT GPS time + tmp_double = GPS_current_time; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position East [m] + tmp_double = d_rx_pos(0); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position North [m] + tmp_double = d_rx_pos(1); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position Up [m] + tmp_double = d_rx_pos(2); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // User clock offset [s] + tmp_double = d_rx_dt_s; + 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 + + }catch(const std::exception & e) { + d_rx_dt_s=0;//reset rx time estimation + LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what(); b_valid_position = false; } + } + else + { + b_valid_position = false; + } return b_valid_position; } diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 87dae62bd..c55d055ef 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -201,17 +201,17 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // 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]"; + << " 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 + else // the ephemeris are not available for this SV { DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; } @@ -288,9 +288,9 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou 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) + try{ + // 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..."; @@ -299,81 +299,87 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou 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); + // 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] + 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]"; + 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) - { - 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(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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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 secondsperweek = 604800.0; + // Compute GST and Gregorian time + if( GST != 0.0) { - 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)); + utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); } - catch (const std::ifstream::failure& e) + else { - LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); + utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast(GPS_week); } - } - // MOVING AVERAGE PVT - pos_averaging(flag_averaging); + // 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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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); + }catch(const std::exception & e) + { + d_rx_dt_s=0;//reset rx time estimation + LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what(); + b_valid_position = false; + } } else { diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index ead1e107d..ec9732876 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -31,6 +31,7 @@ #include "ls_pvt.h" #include +#include #include "GPS_L1_CA.h" #include #include @@ -81,80 +82,80 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) arma::mat BBB; double traveltime = 0; for (int iter = 0; iter < 2; iter++) + { + B = B_pass; + int m = arma::size(B,0); + for (int i = 0; i < m; i++) { - B = B_pass; - int m = arma::size(B,0); - for (int i = 0; i < m; i++) - { - int x = B(i,0); - int y = B(i,1); - if (iter == 0) - { - traveltime = 0.072; - } - else - { - int z = B(i,2); - double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2)); - traveltime = sqrt(rho) / GPS_C_m_s; - } - double angle = traveltime * 7.292115147e-5; - double cosa = cos(angle); - double sina = sin(angle); - B(i,0) = cosa * x + sina * y; - B(i,1) = -sina * x + cosa * y; - }// % i-loop - - if (m > 3) - { - BBB = arma::inv(B.t() * B) * B.t(); - } + int x = B(i,0); + int y = B(i,1); + if (iter == 0) + { + traveltime = 0.072; + } else - { - BBB = arma::inv(B); - } - arma::vec e = arma::ones(m,1); - arma::vec alpha = arma::zeros(m,1); - for (int i = 0; i < m; i++) - { - alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0; - } - arma::mat BBBe = BBB * e; - arma::mat BBBalpha = BBB * alpha; - double a = lorentz(BBBe, BBBe); - double b = lorentz(BBBe, BBBalpha) - 1; - double c = lorentz(BBBalpha, BBBalpha); - double root = sqrt(b * b - a * c); - arma::vec r = {(-b - root) / a, (-b + root) / a}; - arma::mat possible_pos = arma::zeros(4,2); + { + int z = B(i,2); + double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2)); + traveltime = sqrt(rho) / GPS_C_m_s; + } + double angle = traveltime * 7.292115147e-5; + double cosa = cos(angle); + double sina = sin(angle); + B(i,0) = cosa * x + sina * y; + B(i,1) = -sina * x + cosa * y; + }// % i-loop + + if (m > 3) + { + BBB = arma::inv(B.t() * B) * B.t(); + } + else + { + BBB = arma::inv(B); + } + arma::vec e = arma::ones(m,1); + arma::vec alpha = arma::zeros(m,1); + for (int i = 0; i < m; i++) + { + alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0; + } + arma::mat BBBe = BBB * e; + arma::mat BBBalpha = BBB * alpha; + double a = lorentz(BBBe, BBBe); + double b = lorentz(BBBe, BBBalpha) - 1; + double c = lorentz(BBBalpha, BBBalpha); + double root = sqrt(b * b - a * c); + arma::vec r = {(-b - root) / a, (-b + root) / a}; + arma::mat possible_pos = arma::zeros(4,2); + for (int i = 0; i < 2; i++) + { + possible_pos.col(i) = r(i) * BBBe + BBBalpha; + possible_pos(3,i) = -possible_pos(3,i); + } + + arma::vec abs_omc = arma::zeros(2,1); + for (int j = 0; j < m; j++) + { for (int i = 0; i < 2; i++) - { - possible_pos.col(i) = r(i) * BBBe + BBBalpha; - possible_pos(3,i) = -possible_pos(3,i); - } + { + double c_dt = possible_pos(3,i); + double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt; + double omc = obs(j) - calc; + abs_omc(i) = std::abs(omc); + } + }// % j-loop - arma::vec abs_omc = arma::zeros(2,1); - for (int j = 0; j < m; j++) - { - for (int i = 0; i < 2; i++) - { - double c_dt = possible_pos(3,i); - double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt; - double omc = obs(j) - calc; - abs_omc(i) = std::abs(omc); - } - }// % j-loop - - //discrimination between roots - if (abs_omc(0) > abs_omc(1)) - { - pos = possible_pos.col(1); - } - else - { - pos = possible_pos.col(0); - } - }// % iter loop + //discrimination between roots + if (abs_omc(0) > abs_omc(1)) + { + pos = possible_pos.col(1); + } + else + { + pos = possible_pos.col(0); + } + }// % iter loop return pos; } @@ -217,83 +218,82 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs //=== Iteratively find receiver position =================================== for (int iter = 0; iter < nmbOfIterations; iter++) + { + for (int i = 0; i < nmbOfSatellites; i++) { - for (int i = 0; i < nmbOfSatellites; i++) - { - if (iter == 0) - { - //--- Initialize variables at the first iteration -------------- - Rot_X = X.col(i); //Armadillo - trop = 0.0; - } - else - { - //--- Update equations ----------------------------------------- - rho2 = (X(0, i) - pos(0)) * - (X(0, i) - pos(0)) + (X(1, i) - pos(1)) * - (X(1, i) - pos(1)) + (X(2, i) - pos(2)) * - (X(2, i) - pos(2)); - traveltime = sqrt(rho2) / GPS_C_m_s; - - //--- Correct satellite position (do to earth rotation) -------- - Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo - - //--- Find DOA and range of satellites - Ls_Pvt::topocent(&d_visible_satellites_Az[i], - &d_visible_satellites_El[i], - &d_visible_satellites_Distance[i], - pos.subvec(0,2), - Rot_X - pos.subvec(0, 2)); - if(traveltime < 0.1 && nmbOfSatellites > 3) - { - //--- Find receiver's height - Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); - // Add troposphere correction if the receiver is below the troposphere - if (h > 15000) - { - //receiver is above the troposphere - trop = 0.0; - } - else - { - //--- Find delay due to troposphere (in meters) - Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); - if(trop > 5.0 ) trop = 0.0; //check for erratic values - } - } - } - //--- Apply the corrections ---------------------------------------- - omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo - - //--- Construct the A matrix --------------------------------------- - //Armadillo - A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i); - A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i); - A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i); - A(i,3) = 1.0; - } - - //--- Find position update --------------------------------------------- - x = arma::solve(w*A, w*omc); // Armadillo - - //--- Apply position update -------------------------------------------- - pos = pos + x; - if (arma::norm(x,2) < 1e-4) + if (iter == 0) { - break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) + //--- Initialize variables at the first iteration -------------- + Rot_X = X.col(i); //Armadillo + trop = 0.0; } + else + { + //--- Update equations ----------------------------------------- + rho2 = (X(0, i) - pos(0)) * + (X(0, i) - pos(0)) + (X(1, i) - pos(1)) * + (X(1, i) - pos(1)) + (X(2, i) - pos(2)) * + (X(2, i) - pos(2)); + traveltime = sqrt(rho2) / GPS_C_m_s; + + //--- Correct satellite position (do to earth rotation) -------- + Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo + + //--- Find DOA and range of satellites + Ls_Pvt::topocent(&d_visible_satellites_Az[i], + &d_visible_satellites_El[i], + &d_visible_satellites_Distance[i], + pos.subvec(0,2), + Rot_X - pos.subvec(0, 2)); + if(traveltime < 0.1 && nmbOfSatellites > 3) + { + //--- Find receiver's height + Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); + // Add troposphere correction if the receiver is below the troposphere + if (h > 15000) + { + //receiver is above the troposphere + trop = 0.0; + } + else + { + //--- Find delay due to troposphere (in meters) + Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + if(trop > 5.0 ) trop = 0.0; //check for erratic values + } + } + } + //--- Apply the corrections ---------------------------------------- + omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo + + //--- Construct the A matrix --------------------------------------- + //Armadillo + A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i); + A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i); + A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i); + A(i,3) = 1.0; } - try - { - //-- compute the Dilution Of Precision values - d_Q = arma::inv(arma::htrans(A) * A); - } - catch(std::exception& e) - { - d_Q = arma::zeros(4,4); + //--- Find position update --------------------------------------------- + x = arma::solve(w*A, w*omc); // Armadillo + + //--- Apply position update -------------------------------------------- + pos = pos + x; + if (arma::norm(x,2) < 1e-4) + { + break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) + } } + //-- compute the Dilution Of Precision values + d_Q = arma::inv(arma::htrans(A) * A); + + // check the consistency of the PVT solution + if (((fabs(pos(3))*1000.0)/ GPS_C_m_s)>GPS_STARTOFFSET_ms*2) + { + LOG(WARNING)<<"Receiver time offset out of range! Estimated RX Time error [s]:"<