[hamradio-commits] [gnss-sdr] 295/303: Code cleaning
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:09 UTC 2017
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.
commit dbe08ade8c73d55f992f0411b3729c1333914ad5
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Mon Feb 6 19:24:17 2017 +0100
Code cleaning
---
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc | 270 +++++++-------
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 274 +++++++-------
src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 520 +++++++++++++--------------
src/algorithms/PVT/libs/ls_pvt.cc | 272 +++++++-------
4 files changed, 669 insertions(+), 667 deletions(-)
diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
index 4b08cd9..7041b83 100644
--- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
@@ -99,58 +99,58 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> 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
- 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.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;
-
- // 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);
-
- //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.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(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;
+ // 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.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;
+
+ // 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);
+
+ //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.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(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******************************************************
@@ -159,97 +159,99 @@ 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;
- try{
- // check if this is the initial position computation
- if (d_rx_dt_s == 0)
+ {
+ arma::vec rx_position_and_time;
+ DLOG(INFO) << "satpos=" << satpos;
+ DLOG(INFO) << "obs="<< obs;
+ DLOG(INFO) << "W=" << W;
+ try
{
- // 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]
+ // 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_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);
}
- // 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)
+ 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);
- }catch(const std::exception & e)
+ }
+ else
{
- 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/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
index 72d87d8..a0d197d 100644
--- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
@@ -99,58 +99,58 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> 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())
- {
- /*!
- * \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;
-
- // 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_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]";
-
- 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;
+ // 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;
+
+ // 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;
+
+ // 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_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]";
+
+ 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,99 +159,101 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> 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;
-
- 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 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) << "(new)Position at TOW=" << GPS_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]";
-
- 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);
-
- // Compute UTC time and print PVT solution
- double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
- boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast<double>(GPS_week));
- // 22 August 1999 last GPS time roll over
- 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]";
+ {
+ arma::vec rx_position_and_time;
+ DLOG(INFO) << "satpos=" << satpos;
+ DLOG(INFO) << "obs=" << obs;
+ DLOG(INFO) << "W=" << W;
- // ###### Compute DOPs ########
- compute_DOP();
+ 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 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) << "(new)Position at TOW=" << GPS_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]";
+
+ 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);
+
+ // Compute UTC time and print PVT solution
+ double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
+ boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast<double>(GPS_week));
+ // 22 August 1999 last GPS time roll over
+ 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]";
+
+ // ###### 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 = 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);
- // ######## LOG FILE #########
- if(d_flag_dump_enabled == true)
+ }
+ catch(const std::exception & e)
{
- // 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();
- }
+ 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
- pos_averaging(flag_averaging);
-
- }catch(const std::exception & e)
+ }
+ else
{
- 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 c55d055..09fc2b6 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -100,180 +100,176 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
for(gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++)
- {
- switch(gnss_observables_iter->second.System)
- {
- 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;
-
- // 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);
-
- //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 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;
-
-
-
- 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);
-
- // 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_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)
+ switch(gnss_observables_iter->second.System)
{
- gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
- if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
+ case 'E':
{
- /*!
- * \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++;
+ // 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;
+
+ // 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);
+
+ //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 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;
+
+ 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);
+
+ // 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_observables_iter->second.PRN;
+ }
+ break;
}
- else // the ephemeris are not available for this SV
+ case 'G':
{
- DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
+ // 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;
}
- break;
- }
- default :
- DLOG(INFO) << "Hybrid observables: Unknown GNSS";
- break;
}
- }
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
@@ -283,107 +279,109 @@ 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;
- 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 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 rx_position_and_time;
+ DLOG(INFO) << "satpos=" << satpos;
+ DLOG(INFO) << "obs=" << obs;
+ DLOG(INFO) << "W=" << W;
+ try
{
- utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
+ // 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)
+ {
+ 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
+ catch(const std::exception & e)
{
- utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
+ d_rx_dt_s = 0; //reset rx time estimation
+ LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
+ b_valid_position = false;
}
-
- // 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);
- }catch(const std::exception & e)
+ }
+ else
{
- 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/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
index ec97328..e4bdfdd 100644
--- a/src/algorithms/PVT/libs/ls_pvt.cc
+++ b/src/algorithms/PVT/libs/ls_pvt.cc
@@ -82,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++)
{
- 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();
- }
- 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);
- }
+ 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
- arma::vec abs_omc = arma::zeros(2,1);
- for (int j = 0; j < m; j++)
- {
+ 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++)
- {
- 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
+ {
+ possible_pos.col(i) = r(i) * BBBe + BBBalpha;
+ possible_pos(3,i) = -possible_pos(3,i);
+ }
- //discrimination between roots
- if (abs_omc(0) > abs_omc(1))
- {
- pos = possible_pos.col(1);
- }
- else
- {
- pos = possible_pos.col(0);
- }
- }// % iter 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
return pos;
}
@@ -164,7 +164,7 @@ double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y)
{
// LORENTZ Calculates the Lorentz inner product of the two
// 4 by 1 vectors x and y
- // Based ob code by:
+ // Based on code by:
// Kai Borre 04-22-95
// Copyright (c) by Kai Borre
// $Revision: 1.0 $ $Date: 1997/09/26 $
@@ -182,7 +182,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
- * w - weigths vector
+ * w - weight vector
*
* Returns:
* pos - receiver position and receiver clock error
@@ -218,82 +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++)
{
- 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)
+ for (int i = 0; i < nmbOfSatellites; i++)
{
- //--- 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;
- }
+ if (iter == 0)
+ {
+ //--- Initialize variables at the first iteration --------------
+ Rot_X = X.col(i); //Armadillo
+ 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
+ {
+ //--- 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;
- //--- 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;
- }
+ //--- Correct satellite position (do to earth rotation) --------
+ Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
- //--- Find position update ---------------------------------------------
- x = arma::solve(w*A, w*omc); // 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
- //--- 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)
+ //--- 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)
+ {
+ 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]:"<<pos(3)/ GPS_C_m_s;
- throw std::runtime_error("Receiver time offset out of range!");
- }
+ 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]:" << pos(3) / GPS_C_m_s;
+ throw std::runtime_error("Receiver time offset out of range!");
+ }
return pos;
}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list