mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-27 01:14:51 +00:00
Cleaning code and comments
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
parent
cc02888dad
commit
10a4ac60df
@ -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_display_rate_ms=display_rate_ms;
|
||||
d_display_rate_ms = display_rate_ms;
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_nchannels = nchannels;
|
||||
d_dump_filename = dump_filename;
|
||||
std::string dump_ls_pvt_filename;
|
||||
dump_ls_pvt_filename=dump_filename;
|
||||
std::string dump_ls_pvt_filename = dump_filename;
|
||||
|
||||
//initialize kml_printer
|
||||
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);
|
||||
|
||||
//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");
|
||||
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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
@ -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)
|
||||
{
|
||||
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;
|
||||
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.satelliteBlock[nav_msg.i_satellite_PRN]
|
||||
<< ")" << std::endl;
|
||||
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;
|
||||
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 ################################
|
||||
// write the pseudoranges to RINEX OBS file
|
||||
// 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 satellite_tx_time_using_timestamps;
|
||||
//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
|
||||
d_tx_time=d_ephemeris_clock_s + gnss_pseudoranges_iter->second.Pseudorange_symbol_shift/(double)GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND+GPS_SUBFRAME_SECONDS;
|
||||
//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 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;
|
||||
//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)
|
||||
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
|
||||
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.
|
||||
if (clock_error<1){
|
||||
clock_error = std::abs(d_tx_time - satellite_tx_time_using_timestamps);
|
||||
// -> 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)
|
||||
{
|
||||
// compute on the fly PVT solution
|
||||
//mod 8/4/2012 Set the PVT computation rate in this block
|
||||
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)
|
||||
<< " 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;
|
||||
|
||||
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;
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
consume_each(1); //one by one
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -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.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)
|
||||
{
|
||||
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
|
||||
@ -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)
|
||||
*/
|
||||
|
||||
//const double Omegae_dot = 7.292115147e-5; // rad/sec
|
||||
|
||||
//--- Find rotation angle --------------------------------------------------
|
||||
double omegatau;
|
||||
omegatau = OMEGA_EARTH_DOT * traveltime;
|
||||
|
||||
//--- Make a rotation matrix -----------------------------------------------
|
||||
//--- Build a rotation matrix ----------------------------------------------
|
||||
arma::mat R3 = arma::zeros(3,3);
|
||||
R3(0, 0) = cos(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, 1) = 0.0;
|
||||
R3(2, 2) = 1;
|
||||
|
||||
//--- Do the rotation ------------------------------------------------------
|
||||
arma::vec X_sat_rot;
|
||||
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)
|
||||
{
|
||||
/*Function calculates the Least Square Solution.
|
||||
/* Computes the Least Squares Solution.
|
||||
* Inputs:
|
||||
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
|
||||
* obs - Observations - the pseudorange measurements to each
|
||||
* satellite
|
||||
* obs - Observations - the pseudorange measurements to each satellite
|
||||
* w - weigths vector
|
||||
*
|
||||
* Returns:
|
||||
@ -136,11 +134,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
|
||||
//=== Initialization =======================================================
|
||||
int nmbOfIterations = 10; // TODO: include in config
|
||||
|
||||
//double dtr = GPS_PI / 180.0;
|
||||
|
||||
int nmbOfSatellites;
|
||||
|
||||
nmbOfSatellites = satpos.n_cols; //Armadillo
|
||||
arma::vec pos = "0.0 0.0 0.0 0.0";
|
||||
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;
|
||||
}
|
||||
el = az;
|
||||
|
||||
arma::mat X = satpos;
|
||||
arma::vec Rot_X;
|
||||
double rho2;
|
||||
@ -169,6 +162,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
double trop;
|
||||
arma::mat mat_tmp;
|
||||
arma::vec x;
|
||||
|
||||
//=== Iteratively find receiver position ===================================
|
||||
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
|
||||
{
|
||||
//--- 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;
|
||||
|
||||
//--- 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 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));
|
||||
|
||||
//--- Find DOA and range of satellites
|
||||
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, :));
|
||||
|
||||
}
|
||||
@ -206,7 +203,6 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
A(i,3) = 1.0;
|
||||
}
|
||||
|
||||
|
||||
// These lines allow the code to exit gracefully in case of any errors
|
||||
//if (rank(A) != 4) {
|
||||
// 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
|
||||
arma::mat Q;
|
||||
Q = arma::inv(arma::htrans(A)*A);
|
||||
//std::cout<<Q<<std::endl;
|
||||
|
||||
d_GDOP = sqrt(arma::trace(Q)); // GDOP
|
||||
d_GDOP = sqrt(arma::trace(Q)); // Geometric DOP
|
||||
d_PDOP = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // PDOP
|
||||
d_HDOP = sqrt(Q(0,0) + Q(1,1)); // HDOP
|
||||
d_VDOP = sqrt(Q(2,2)); // VDOP
|
||||
d_TDOP = sqrt(Q(3,3)); // TDOP
|
||||
}catch(std::exception e)
|
||||
}
|
||||
catch(std::exception e)
|
||||
{
|
||||
d_GDOP = -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;
|
||||
|
||||
@ -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 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++)
|
||||
{
|
||||
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(1,i) = d_ephemeris[i].d_satpos_Y;
|
||||
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
|
||||
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
|
||||
DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN
|
||||
<< " 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;
|
||||
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_IDs[valid_obs] = d_ephemeris[i].i_satellite_PRN;
|
||||
d_visible_satellites_CN0_dB[valid_obs] =gnss_pseudoranges_iter->second.CN0_dB_hz;
|
||||
valid_obs++;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
mypos=leastSquarePos(satpos,obs,W);
|
||||
DLOG(INFO) << "Position at TOW="<< GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
|
||||
cart2geo(mypos(0), mypos(1), mypos(2), 4);
|
||||
DLOG(INFO) << "Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
|
||||
gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
|
||||
|
||||
// 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);
|
||||
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
|
||||
<< " [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)
|
||||
{
|
||||
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_longitude_d = d_avg_longitude_d / (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
|
||||
}
|
||||
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_longitude_d = d_longitude_d;
|
||||
d_avg_height_m = d_height_m;
|
||||
b_valid_position=false;
|
||||
return false;//indicates that the returned position is not valid yet
|
||||
b_valid_position = false;
|
||||
return false; //indicates that the returned position is not valid yet
|
||||
// output the average, although it will not have the full historic available
|
||||
// d_avg_latitude_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
|
||||
{
|
||||
b_valid_position=true;
|
||||
return true;//indicates that the returned position is valid
|
||||
b_valid_position = true;
|
||||
return true; //indicates that the returned position is valid
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b_valid_position=false;
|
||||
b_valid_position = 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)
|
||||
{
|
||||
// Conversion of Cartesian coordinates (X,Y,Z) to geographical
|
||||
// coordinates (latitude, longitude, h) on a selected reference ellipsoid.
|
||||
//
|
||||
//
|
||||
// Choices of Reference Ellipsoid for Geographical Coordinates
|
||||
// 0. International Ellipsoid 1924
|
||||
// 1. International Ellipsoid 1967
|
||||
// 2. World Geodetic System 1972
|
||||
// 3. Geodetic Reference System 1980
|
||||
// 4. World Geodetic System 1984
|
||||
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
|
||||
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
|
||||
|
||||
Choices of Reference Ellipsoid for Geographical Coordinates
|
||||
0. International Ellipsoid 1924
|
||||
1. International Ellipsoid 1967
|
||||
2. World Geodetic System 1972
|
||||
3. Geodetic Reference System 1980
|
||||
4. World Geodetic System 1984
|
||||
*/
|
||||
|
||||
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};
|
||||
|
||||
double lambda;
|
||||
lambda = atan2(Y,X);
|
||||
double ex2;
|
||||
ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -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 lambda = atan2(Y,X);
|
||||
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection]));
|
||||
double c = a[elipsoid_selection] * sqrt(1+ex2);
|
||||
double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
|
||||
|
||||
double h = 0.1;
|
||||
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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
@ -484,93 +478,74 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
|
||||
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)
|
||||
{
|
||||
//function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z)
|
||||
//%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
|
||||
//% height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
||||
//% values semi-major axis (a) and the inverse of flattening (finv).
|
||||
//%
|
||||
//%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
|
||||
//%
|
||||
//% 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
|
||||
//% (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.
|
||||
//%
|
||||
//% Inputs:
|
||||
//% a - semi-major axis of the reference ellipsoid
|
||||
//% finv - inverse of flattening of the reference ellipsoid
|
||||
//% X,Y,Z - Cartesian coordinates
|
||||
//%
|
||||
//% Outputs:
|
||||
//% dphi - latitude
|
||||
//% 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 $
|
||||
//%==========================================================================
|
||||
//
|
||||
/* Subroutine to calculate geodetic coordinates latitude, longitude,
|
||||
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
||||
values semi-major axis (a) and the inverse of flattening (finv).
|
||||
|
||||
The output units of angular quantities will be in decimal degrees
|
||||
(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.
|
||||
|
||||
Inputs:
|
||||
a - semi-major axis of the reference ellipsoid
|
||||
finv - inverse of flattening of the reference ellipsoid
|
||||
X,Y,Z - Cartesian coordinates
|
||||
|
||||
Outputs:
|
||||
dphi - latitude
|
||||
dlambda - longitude
|
||||
h - height above reference ellipsoid
|
||||
|
||||
Based in a Matlab function by Kai Borre
|
||||
*/
|
||||
|
||||
*h = 0;
|
||||
double tolsq = 1.e-10;
|
||||
int maxit=10;
|
||||
// compute radians-to-degree factor
|
||||
double rtd;
|
||||
rtd = 180/GPS_PI;
|
||||
double tolsq = 1.e-10; // tolerance to accept convergence
|
||||
int maxit = 10; // max number of iterations
|
||||
double rtd = 180/GPS_PI;
|
||||
|
||||
// compute square of eccentricity
|
||||
double esq;
|
||||
if (finv < 1.0E-20)
|
||||
{
|
||||
esq = 0;
|
||||
}else
|
||||
}
|
||||
else
|
||||
{
|
||||
esq = (2 - 1/finv) / finv;
|
||||
}
|
||||
|
||||
double oneesq;
|
||||
|
||||
oneesq = 1 - esq;
|
||||
|
||||
// first guess
|
||||
// P is distance from spin axis
|
||||
double P;
|
||||
P = sqrt(X*X+Y*Y);
|
||||
|
||||
double P = sqrt(X*X + Y*Y); // P is distance from spin axis
|
||||
//direct calculation of longitude
|
||||
//
|
||||
if (P > 1.0E-20)
|
||||
{
|
||||
*dlambda = atan2(Y,X) * rtd;
|
||||
}else
|
||||
}
|
||||
else
|
||||
{
|
||||
*dlambda = 0;
|
||||
}
|
||||
|
||||
// correct longitude bound
|
||||
if (*dlambda < 0)
|
||||
{
|
||||
*dlambda = *dlambda + 360.0;
|
||||
}
|
||||
|
||||
// r is distance from origin (0,0,0)
|
||||
double r;
|
||||
r = sqrt(P*P + Z*Z);
|
||||
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0)
|
||||
|
||||
double sinphi;
|
||||
|
||||
if (r > 1.0E-20)
|
||||
{
|
||||
sinphi = Z/r;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
sinphi = 0;
|
||||
}
|
||||
|
||||
*dphi = asin(sinphi);
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
// iterate
|
||||
double cosphi;
|
||||
double N_phi;
|
||||
double dP;
|
||||
double dZ;
|
||||
double oneesq = 1 - esq;
|
||||
|
||||
for (int i=0; i<maxit; i++)
|
||||
{
|
||||
sinphi = sin(*dphi);
|
||||
cosphi = cos(*dphi);
|
||||
|
||||
// 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
|
||||
dP = P - (N_phi + (*h)) * cosphi;
|
||||
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
|
||||
//
|
||||
|
||||
// update height and latitude
|
||||
*h = *h + (sinphi*dZ + cosphi*dP);
|
||||
*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;
|
||||
}
|
||||
|
||||
// Not Converged--Warn user
|
||||
// if (i == (maxit-1))
|
||||
// fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
|
||||
// ' iterations\n'], i);
|
||||
// end
|
||||
if (i == (maxit-1))
|
||||
{
|
||||
DLOG(INFO) << "The computation of geodetic coordinates did not converged" << std::endl;
|
||||
}
|
||||
}
|
||||
//
|
||||
*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)
|
||||
//%TOPOCENT Transformation of vector dx into topocentric coordinate
|
||||
//% system with origin at X.
|
||||
//% Both parameters are 3 by 1 vectors.
|
||||
//%
|
||||
//%[Az, El, D] = topocent(X, dx);
|
||||
//%
|
||||
//% 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
|
||||
Outputs:
|
||||
D - vector length. Units like the input
|
||||
Az - azimuth from north positive clockwise, degrees
|
||||
El - elevation angle, degrees
|
||||
|
||||
Based on a Matlab function by Kai Borre
|
||||
*/
|
||||
|
||||
double dtr;
|
||||
double lambda;
|
||||
double phi;
|
||||
double cl;
|
||||
double sl;
|
||||
double cb;
|
||||
double sb;
|
||||
|
||||
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);
|
||||
sl = sin(lambda * dtr);
|
||||
cb = cos(phi * dtr);
|
||||
sb = sin(phi * dtr);
|
||||
F(0,0) = -sl;
|
||||
F(0,1) = -sb*cl;
|
||||
F(0,2) = cb*cl;
|
||||
|
||||
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(0,1)=-sb*cl;
|
||||
F(0,2)=cb*cl;
|
||||
|
||||
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;
|
||||
F(2,0) = 0;
|
||||
F(2,1) = cb;
|
||||
F(2,2) = sb;
|
||||
|
||||
arma::vec local_vector;
|
||||
|
||||
local_vector = arma::htrans(F) * x_sat;
|
||||
local_vector = arma::htrans(F) * dx;
|
||||
|
||||
double E;
|
||||
double N;
|
||||
double U;
|
||||
|
||||
E = local_vector(0);
|
||||
N = local_vector(1);
|
||||
U = local_vector(2);
|
||||
double E = local_vector(0);
|
||||
double N = local_vector(1);
|
||||
double U = local_vector(2);
|
||||
|
||||
double hor_dis;
|
||||
|
||||
hor_dis = sqrt(E*E + N*N);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
*D = sqrt(x_sat(0)*x_sat(0) + x_sat(1)*x_sat(1) + x_sat(2)*x_sat(2));
|
||||
|
||||
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2));
|
||||
}
|
||||
|
||||
@ -57,12 +57,12 @@ class gps_l1_ca_ls_pvt
|
||||
{
|
||||
private:
|
||||
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
|
||||
arma::vec e_r_corr(double traveltime, arma::vec X_sat);
|
||||
void topocent(double *Az, double *El, double *D, arma::vec x, 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 dx);
|
||||
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
|
||||
public:
|
||||
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
|
||||
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
|
||||
@ -121,7 +121,7 @@ public:
|
||||
* \param[in] X [m] Cartesian coordinate
|
||||
* \param[in] Y [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.
|
||||
* 1 - International Ellipsoid 1967.
|
||||
* 2 - World Geodetic System 1972.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user