mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-28 01:44:52 +00:00
Code Issues Releases Wiki Activity

Cleaning code and comments

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@238 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez 2012-09-02 16:53:59 +00:00
parent cc02888dad
commit 10a4ac60df
3 changed files with 604 additions and 646 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@ -59,13 +59,12 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
{ {
d_output_rate_ms = output_rate_ms; d_output_rate_ms = output_rate_ms;
d_display_rate_ms=display_rate_ms; d_display_rate_ms = display_rate_ms;
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
d_nchannels = nchannels; d_nchannels = nchannels;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
std::string dump_ls_pvt_filename; std::string dump_ls_pvt_filename = dump_filename;
dump_ls_pvt_filename=dump_filename;
//initialize kml_printer //initialize kml_printer
std::string kml_dump_filename; std::string kml_dump_filename;
@ -74,7 +73,7 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
d_kml_dump.set_headers(kml_dump_filename); d_kml_dump.set_headers(kml_dump_filename);
//initialize nmea_printer //initialize nmea_printer
d_nmea_printer=new Nmea_Printer(nmea_dump_filename,flag_nmea_tty_port,nmea_dump_devname); d_nmea_printer = new Nmea_Printer(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
d_dump_filename.append("_raw.dat"); d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat"); dump_ls_pvt_filename.append("_ls_pvt.dat");
@ -108,7 +107,8 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout << "PVT dump enabled Log file: " << d_dump_filename.c_str() << std::endl; std::cout << "PVT dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e)
{
std::cout << "Exception opening PVT dump file " << e.what() << std::endl; std::cout << "Exception opening PVT dump file " << e.what() << std::endl;
} }
} }
@ -150,7 +150,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{ {
if (in[i][0].Flag_valid_pseudorange == true) if (in[i][0].Flag_valid_pseudorange == true)
{ {
gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); //record the valid pseudorange in a map gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map
} }
} }
@ -169,12 +169,12 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
Gps_Navigation_Message nav_msg; Gps_Navigation_Message nav_msg;
while (d_nav_queue->try_pop(nav_msg) == true) while (d_nav_queue->try_pop(nav_msg) == true)
{ {
std::cout<<"New ephemeris record has arrived from SAT ID " std::cout << "New ephemeris record has arrived from SAT ID "
<< nav_msg.i_satellite_PRN << " (Block " << nav_msg.i_satellite_PRN << " (Block "
<< nav_msg.satelliteBlock[nav_msg.i_satellite_PRN] << nav_msg.satelliteBlock[nav_msg.i_satellite_PRN]
<< ")" << std::endl; << ")" << std::endl;
d_last_nav_msg = nav_msg; d_last_nav_msg = nav_msg;
if (nav_msg.b_valid_ephemeris_set_flag==true) if (nav_msg.b_valid_ephemeris_set_flag == true)
{ {
d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg; d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg;
nav_data_map[nav_msg.i_channel_ID] = nav_msg; nav_data_map[nav_msg.i_channel_ID] = nav_msg;
@ -190,20 +190,21 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
// ############ 2. COMPUTE THE PVT ################################ // ############ 2. COMPUTE THE PVT ################################
// write the pseudoranges to RINEX OBS file // write the pseudoranges to RINEX OBS file
// 1- need a valid clock // 1- need a valid clock
if (d_ephemeris_clock_s > 0 and d_last_nav_msg.i_satellite_PRN > 0 and d_last_nav_msg.b_valid_ephemeris_set_flag==true) if (d_ephemeris_clock_s > 0 and d_last_nav_msg.i_satellite_PRN > 0 and d_last_nav_msg.b_valid_ephemeris_set_flag == true)
{ {
double clock_error; double clock_error;
double satellite_tx_time_using_timestamps; double satellite_tx_time_using_timestamps;
//for GPS L1 C/A: t_tx=TOW+N_symbols_from_TOW*T_symbol //for GPS L1 C/A: t_tx = TOW + N_symbols_from_TOW*T_symbol
//Notice that the TOW is decoded AFTER processing the subframe -> we ned to add ONE subframe duration to t_tx //Notice that the TOW is decoded AFTER processing the subframe -> we need to add ONE subframe duration to t_tx
d_tx_time=d_ephemeris_clock_s + gnss_pseudoranges_iter->second.Pseudorange_symbol_shift/(double)GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND+GPS_SUBFRAME_SECONDS; d_tx_time = d_ephemeris_clock_s + gnss_pseudoranges_iter->second.Pseudorange_symbol_shift/(double)GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND + GPS_SUBFRAME_SECONDS;
//Perform an extra check to verify the TOW update (the ephemeris queue is ASYNCHRONOUS to the GNU Radio Gnss_Synchro sample stream) //Perform an extra check to verify the TOW update (the ephemeris queue is ASYNCHRONOUS to the GNU Radio Gnss_Synchro sample stream)
//-> compute the t_tx_timestamps using the symbols timestamp (it is affected by code Doppler, but it is not wrapped like N_symbols_from_TOW) //-> compute the t_tx_timestamps using the symbols timestamp (it is affected by code Doppler, but it is not wrapped like N_symbols_from_TOW)
satellite_tx_time_using_timestamps=d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0; satellite_tx_time_using_timestamps = d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms - d_ephemeris_timestamp_ms)/1000.0;
//->compute the absolute error between both T_tx //->compute the absolute error between both T_tx
clock_error=std::abs(d_tx_time-satellite_tx_time_using_timestamps); clock_error = std::abs(d_tx_time - satellite_tx_time_using_timestamps);
// -> The symbol conter N_symbols_from_TOW will be resetted every new received telemetry word, if the TOW is not uptated, both t_tx and t_tx_timestamps times will difer by more than 1 seconds. // -> The symbol counter N_symbols_from_TOW will be reset every new received telemetry word, if the TOW is not updated, both t_tx and t_tx_timestamps times will difer by more than 1 seconds.
if (clock_error<1){ if (clock_error < 1)
{
// compute on the fly PVT solution // compute on the fly PVT solution
//mod 8/4/2012 Set the PVT computation rate in this block //mod 8/4/2012 Set the PVT computation rate in this block
if ((d_sample_counter % d_output_rate_ms) == 0) if ((d_sample_counter % d_output_rate_ms) == 0)
@ -227,11 +228,12 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
} }
} }
if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position==true) if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
{ {
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d << " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is HDOP = " << d_ls_pvt->d_HDOP << " and VDOP = " << d_ls_pvt->d_VDOP << std::endl; << " is HDOP = " << d_ls_pvt->d_HDOP << " and VDOP = " << d_ls_pvt->d_VDOP << std::endl;
} }
@ -256,10 +258,8 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
std::cout << "Exception writing observables dump file " << e.what() << std::endl; std::cout << "Exception writing observables dump file " << e.what() << std::endl;
} }
} }
} }
} }
consume_each(1); //one by one consume_each(1); //one by one
return 0; return 0;
} }

326
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -58,11 +58,11 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool
{ {
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); 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); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout << "PVT lib dump enabled Log file: "<< d_dump_filename.c_str() << std::endl; std::cout << "PVT lib dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {
std::cout << "Exception opening PVT lib dump file "<< e.what() << std::endl; std::cout << "Exception opening PVT lib dump file " << e.what() << std::endl;
} }
} }
} }
@ -82,7 +82,7 @@ gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
} }
arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat) arma::vec gps_l1_ca_ls_pvt::rotateSatellite(double traveltime, arma::vec X_sat)
{ {
/* /*
* Returns rotated satellite ECEF coordinates due to Earth * Returns rotated satellite ECEF coordinates due to Earth
@ -96,13 +96,11 @@ arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat)
* X_sat_rot - rotated satellite's coordinates (ECEF) * X_sat_rot - rotated satellite's coordinates (ECEF)
*/ */
//const double Omegae_dot = 7.292115147e-5; // rad/sec
//--- Find rotation angle -------------------------------------------------- //--- Find rotation angle --------------------------------------------------
double omegatau; double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime; omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Make a rotation matrix ----------------------------------------------- //--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3); arma::mat R3 = arma::zeros(3,3);
R3(0, 0) = cos(omegatau); R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau); R3(0, 1) = sin(omegatau);
@ -113,6 +111,7 @@ arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat)
R3(2, 0) = 0.0; R3(2, 0) = 0.0;
R3(2, 1) = 0.0; R3(2, 1) = 0.0;
R3(2, 2) = 1; R3(2, 2) = 1;
//--- Do the rotation ------------------------------------------------------ //--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot; arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat; X_sat_rot = R3 * X_sat;
@ -122,11 +121,10 @@ arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat)
arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w) arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w)
{ {
/*Function calculates the Least Square Solution. /* Computes the Least Squares Solution.
* Inputs: * Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;] * satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each * obs - Observations - the pseudorange measurements to each satellite
* satellite
* w - weigths vector * w - weigths vector
* *
* Returns: * Returns:
@ -136,11 +134,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//=== Initialization ======================================================= //=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config int nmbOfIterations = 10; // TODO: include in config
//double dtr = GPS_PI / 180.0;
int nmbOfSatellites; int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; //Armadillo nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0"; arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A; arma::mat A;
@ -161,7 +155,6 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
az(0, i)= 0.0; az(0, i)= 0.0;
} }
el = az; el = az;
arma::mat X = satpos; arma::mat X = satpos;
arma::vec Rot_X; arma::vec Rot_X;
double rho2; double rho2;
@ -169,6 +162,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
double trop; double trop;
arma::mat mat_tmp; arma::mat mat_tmp;
arma::vec x; arma::vec x;
//=== Iteratively find receiver position =================================== //=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++) for (int iter = 0; iter < nmbOfIterations; iter++)
{ {
@ -183,15 +177,18 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
else else
{ {
//--- Update equations ----------------------------------------- //--- 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)); 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; traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) -------- //--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
Rot_X = e_r_corr(traveltime, X.col(i)); //armadillo //--- Find DOA and range of satellites
//--- Find the elevation angle of the satellite ---------------- 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));
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));
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); //[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
} }
@ -206,7 +203,6 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
A(i,3) = 1.0; A(i,3) = 1.0;
} }
// These lines allow the code to exit gracefully in case of any errors // These lines allow the code to exit gracefully in case of any errors
//if (rank(A) != 4) { //if (rank(A) != 4) {
// pos.clear(); // pos.clear();
@ -224,14 +220,13 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//-- compute the Dilution Of Precision values //-- compute the Dilution Of Precision values
arma::mat Q; arma::mat Q;
Q = arma::inv(arma::htrans(A)*A); Q = arma::inv(arma::htrans(A)*A);
//std::cout<<Q<<std::endl; d_GDOP = sqrt(arma::trace(Q)); // Geometric DOP
d_GDOP = sqrt(arma::trace(Q)); // GDOP
d_PDOP = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // PDOP d_PDOP = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // PDOP
d_HDOP = sqrt(Q(0,0) + Q(1,1)); // HDOP d_HDOP = sqrt(Q(0,0) + Q(1,1)); // HDOP
d_VDOP = sqrt(Q(2,2)); // VDOP d_VDOP = sqrt(Q(2,2)); // VDOP
d_TDOP = sqrt(Q(3,3)); // TDOP d_TDOP = sqrt(Q(3,3)); // TDOP
}catch(std::exception e) }
catch(std::exception e)
{ {
d_GDOP = -1; d_GDOP = -1;
d_PDOP = -1; d_PDOP = -1;
@ -243,7 +238,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
} }
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,double GPS_current_time,bool flag_averaging) bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging)
{ {
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter; std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
@ -255,9 +250,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
double GPS_corrected_time = 0; double GPS_corrected_time = 0;
double utc = 0; double utc = 0;
d_flag_averaging=flag_averaging; d_flag_averaging = flag_averaging;
int valid_obs=0; //valid observations counter int valid_obs = 0; //valid observations counter
for (int i=0; i<d_nchannels; i++) for (int i=0; i<d_nchannels; i++)
{ {
if (d_ephemeris[i].satellite_validation() == true) if (d_ephemeris[i].satellite_validation() == true)
@ -284,11 +279,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
satpos(0,i) = d_ephemeris[i].d_satpos_X; satpos(0,i) = d_ephemeris[i].d_satpos_X;
satpos(1,i) = d_ephemeris[i].d_satpos_Y; satpos(1,i) = d_ephemeris[i].d_satpos_Y;
satpos(2,i) = d_ephemeris[i].d_satpos_Z; satpos(2,i) = d_ephemeris[i].d_satpos_Z;
DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl; << " X=" << d_ephemeris[i].d_satpos_X
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y
<< " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s; obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
d_visible_satellites_IDs[valid_obs]=d_ephemeris[i].i_satellite_PRN; d_visible_satellites_IDs[valid_obs] = d_ephemeris[i].i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs]=gnss_pseudoranges_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] =gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++; valid_obs++;
} }
else else
@ -307,23 +304,24 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
} }
d_valid_observations = valid_obs; d_valid_observations = valid_obs;
DLOG(INFO) << "PVT: valid observations=" << valid_obs << std::endl;
DLOG(INFO) <<"PVT: valid observations="<<valid_obs<<std::endl; if (valid_obs >= 4)
if (valid_obs>=4)
{ {
arma::vec mypos; arma::vec mypos;
mypos=leastSquarePos(satpos,obs,W); mypos=leastSquarePos(satpos,obs,W);
DLOG(INFO) << "Position at TOW="<< GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl; DLOG(INFO) << "Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4); gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
// Compute UTC time and print PVT solution // Compute UTC time and print PVT solution
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800.0*(double)GPS_week); 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*(double)GPS_week);
// 22 August 1999 last GPS time roll over
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
GPS_current_time=GPS_corrected_time; GPS_current_time = GPS_corrected_time;
DLOG(INFO)<< "Position at " << boost::posix_time::to_simple_string(p_time) DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl; << " [deg], Height= " << d_height_m << " [m]" << std::endl;
@ -361,7 +359,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {
std::cout << "Exception writing PVT LS dump file "<<e.what()<<"\r\n"; std::cout << "Exception writing PVT LS dump file "<< e.what() << std::endl;
} }
} }
@ -391,7 +389,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth; d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth; d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth; d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
b_valid_position=true; b_valid_position = true;
return true; //indicates that the returned position is valid return true; //indicates that the returned position is valid
} }
else else
@ -405,8 +403,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_latitude_d = d_latitude_d; d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d; d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m; d_avg_height_m = d_height_m;
b_valid_position=false; b_valid_position = false;
return false;//indicates that the returned position is not valid yet return false; //indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available // output the average, although it will not have the full historic available
// d_avg_latitude_d=0; // d_avg_latitude_d=0;
// d_avg_longitude_d=0; // d_avg_longitude_d=0;
@ -424,13 +422,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
} }
else else
{ {
b_valid_position=true; b_valid_position = true;
return true;//indicates that the returned position is valid return true; //indicates that the returned position is valid
} }
} }
else else
{ {
b_valid_position=false; b_valid_position = false;
return false; return false;
} }
} }
@ -438,28 +436,24 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection) void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{ {
// Conversion of Cartesian coordinates (X,Y,Z) to geographical /* Conversion of Cartesian coordinates (X,Y,Z) to geographical
// coordinates (latitude, longitude, h) on a selected reference ellipsoid. coordinates (latitude, longitude, h) on a selected reference ellipsoid.
//
// Choices of Reference Ellipsoid for Geographical Coordinates
// Choices of Reference Ellipsoid for Geographical Coordinates 0. International Ellipsoid 1924
// 0. International Ellipsoid 1924 1. International Ellipsoid 1967
// 1. International Ellipsoid 1967 2. World Geodetic System 1972
// 2. World Geodetic System 1972 3. Geodetic Reference System 1980
// 3. Geodetic Reference System 1980 4. World Geodetic System 1984
// 4. World Geodetic System 1984 */
const double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137}; const double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563}; const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda; double lambda = atan2(Y,X);
lambda = atan2(Y,X); double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection]));
double ex2; double c = a[elipsoid_selection] * sqrt(1+ex2);
ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection])); double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
double c;
c = a[elipsoid_selection] * sqrt(1+ex2);
double phi;
phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1; double h = 0.1;
double oldh = 0; double oldh = 0;
@ -474,7 +468,7 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
{ {
std::cout << "Failed to approximate h with desired precision. h-oldh= " << h-oldh << std::endl; std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh << std::endl;
break; break;
} }
} }
@ -484,93 +478,74 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
d_height_m = h; d_height_m = h;
} }
void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{ {
//function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) /* Subroutine to calculate geodetic coordinates latitude, longitude,
//%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, height given Cartesian coordinates X,Y,Z, and reference ellipsoid
//% height given Cartesian coordinates X,Y,Z, and reference ellipsoid values semi-major axis (a) and the inverse of flattening (finv).
//% values semi-major axis (a) and the inverse of flattening (finv).
//% The output units of angular quantities will be in decimal degrees
//%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z); (15.5 degrees not 15 deg 30 min). The output units of h will be the
//% same as the units of X,Y,Z,a.
//% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
//% The output units of angular quantities will be in decimal degrees Inputs:
//% (15.5 degrees not 15 deg 30 min). The output units of h will be the a - semi-major axis of the reference ellipsoid
//% same as the units of X,Y,Z,a. finv - inverse of flattening of the reference ellipsoid
//% X,Y,Z - Cartesian coordinates
//% Inputs:
//% a - semi-major axis of the reference ellipsoid Outputs:
//% finv - inverse of flattening of the reference ellipsoid dphi - latitude
//% X,Y,Z - Cartesian coordinates dlambda - longitude
//% h - height above reference ellipsoid
//% Outputs:
//% dphi - latitude Based in a Matlab function by Kai Borre
//% dlambda - longitude */
//% h - height above reference ellipsoid
//
//% Copyright (C) 1987 C. Goad, Columbus, Ohio
//% Reprinted with permission of author, 1996
//% Fortran code translated into MATLAB
//% Kai Borre 03-30-96
//%
//% CVS record:
//% $Id: togeod.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
//%==========================================================================
//
*h = 0; *h = 0;
double tolsq = 1.e-10; double tolsq = 1.e-10; // tolerance to accept convergence
int maxit=10; int maxit = 10; // max number of iterations
// compute radians-to-degree factor double rtd = 180/GPS_PI;
double rtd;
rtd = 180/GPS_PI;
// compute square of eccentricity // compute square of eccentricity
double esq; double esq;
if (finv < 1.0E-20) if (finv < 1.0E-20)
{ {
esq = 0; esq = 0;
}else }
else
{ {
esq = (2 - 1/finv) / finv; esq = (2 - 1/finv) / finv;
} }
double oneesq;
oneesq = 1 - esq;
// first guess // first guess
// P is distance from spin axis
double P; double P = sqrt(X*X + Y*Y); // P is distance from spin axis
P = sqrt(X*X+Y*Y);
//direct calculation of longitude //direct calculation of longitude
//
if (P > 1.0E-20) if (P > 1.0E-20)
{ {
*dlambda = atan2(Y,X) * rtd; *dlambda = atan2(Y,X) * rtd;
}else }
else
{ {
*dlambda = 0; *dlambda = 0;
} }
// correct longitude bound
if (*dlambda < 0) if (*dlambda < 0)
{ {
*dlambda = *dlambda + 360.0; *dlambda = *dlambda + 360.0;
} }
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0)
// r is distance from origin (0,0,0)
double r;
r = sqrt(P*P + Z*Z);
double sinphi; double sinphi;
if (r > 1.0E-20) if (r > 1.0E-20)
{ {
sinphi = Z/r; sinphi = Z/r;
} else }
else
{ {
sinphi = 0; sinphi = 0;
} }
*dphi = asin(sinphi); *dphi = asin(sinphi);
// initial value of height = distance from origin minus // initial value of height = distance from origin minus
@ -583,24 +558,25 @@ void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a
*h = r - a*(1-sinphi*sinphi/finv); *h = r - a*(1-sinphi*sinphi/finv);
// iterate // iterate
double cosphi; double cosphi;
double N_phi; double N_phi;
double dP; double dP;
double dZ; double dZ;
double oneesq = 1 - esq;
for (int i=0; i<maxit; i++) for (int i=0; i<maxit; i++)
{ {
sinphi = sin(*dphi); sinphi = sin(*dphi);
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // compute radius of curvature in prime vertical direction
N_phi = a/sqrt(1-esq*sinphi*sinphi); N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute residuals in P and Z // compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi; dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi; dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
//
// update height and latitude // update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP); *h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h)); *dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
@ -610,85 +586,69 @@ void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a
{ {
break; break;
} }
if (i == (maxit-1))
// Not Converged--Warn user {
// if (i == (maxit-1)) DLOG(INFO) << "The computation of geodetic coordinates did not converged" << std::endl;
// fprintf([' Problem in TOGEOD, did not converge in %2.0f',... }
// ' iterations\n'], i);
// end
} }
//
*dphi = (*dphi) * rtd; *dphi = (*dphi) * rtd;
} }
void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x, arma::vec x_sat)
void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx)
{ {
/* Transformation of vector dx into topocentric coordinate
system with origin at x
Inputs:
x - vector origin coordinates (in ECEF system [X; Y; Z;])
dx - vector ([dX; dY; dZ;]).
//%function [Az, El, D] = topocent(X, dx) Outputs:
//%TOPOCENT Transformation of vector dx into topocentric coordinate D - vector length. Units like the input
//% system with origin at X. Az - azimuth from north positive clockwise, degrees
//% Both parameters are 3 by 1 vectors. El - elevation angle, degrees
//%
//%[Az, El, D] = topocent(X, dx); Based on a Matlab function by Kai Borre
//% */
//% Inputs:
//% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
//% dx - vector ([dX; dY; dZ;]).
//%
//% Outputs:
//% D - vector length. Units like units of the input
//% Az - azimuth from north positive clockwise, degrees
//% El - elevation angle, degrees
double dtr;
double lambda; double lambda;
double phi; double phi;
double cl;
double sl;
double cb;
double sb;
double h; double h;
double dtr = GPS_PI/180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
dtr = GPS_PI/180.0; // Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
//[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3)); double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
togeod(&phi, &lambda, &h,6378137.0, 298.257223563, x(0), x(1), x(2)); arma::mat F = arma::zeros(3,3);
cl = cos(lambda * dtr); F(0,0) = -sl;
sl = sin(lambda * dtr); F(0,1) = -sb*cl;
cb = cos(phi * dtr); F(0,2) = cb*cl;
sb = sin(phi * dtr);
arma::mat F=arma::zeros(3,3); F(1,0) = cl;
F(1,1) = -sb*sl;
F(1,2) = cb*sl;
F(0,0)=-sl; F(2,0) = 0;
F(0,1)=-sb*cl; F(2,1) = cb;
F(0,2)=cb*cl; F(2,2) = sb;
F(1,0)=cl;
F(1,1)=-sb*sl;
F(1,2)=cb*sl;
F(2,0)=0;
F(2,1)=cb;
F(2,2)=sb;
arma::vec local_vector; arma::vec local_vector;
local_vector = arma::htrans(F) * x_sat; local_vector = arma::htrans(F) * dx;
double E; double E = local_vector(0);
double N; double N = local_vector(1);
double U; double U = local_vector(2);
E = local_vector(0);
N = local_vector(1);
U = local_vector(2);
double hor_dis; double hor_dis;
hor_dis = sqrt(E*E + N*N); hor_dis = sqrt(E*E + N*N);
if (hor_dis < 1.0E-20) if (hor_dis < 1.0E-20)
@ -707,7 +667,5 @@ void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
*Az = *Az + 360.0; *Az = *Az + 360.0;
} }
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2));
*D = sqrt(x_sat(0)*x_sat(0) + x_sat(1)*x_sat(1) + x_sat(2)*x_sat(2));
} }

8
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@ -57,12 +57,12 @@ class gps_l1_ca_ls_pvt
{ {
private: private:
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w); arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
arma::vec e_r_corr(double traveltime, arma::vec X_sat); arma::vec rotateSatellite(double traveltime, arma::vec X_sat);
void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec x_sat); void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public: public:
int d_nchannels; //! Number of available channels for positioning int d_nchannels; //! Number of available channels for positioning
int d_valid_observations; //! Number of valid pseudorrange observations (valid satellites) int d_valid_observations; //! Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites
@ -121,7 +121,7 @@ public:
* \param[in] X [m] Cartesian coordinate * \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate * \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate * \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection Choices of Reference Ellipsoid for Geographical Coordinates: * \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates:
* 0 - International Ellipsoid 1924. * 0 - International Ellipsoid 1924.
* 1 - International Ellipsoid 1967. * 1 - International Ellipsoid 1967.
* 2 - World Geodetic System 1972. * 2 - World Geodetic System 1972.