mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-13 19:50:34 +00:00
Code Issues Releases Wiki Activity

Added GNSS-SDR statistics over PVT and gnuplot scripts

Browse Source
This commit is contained in:
marabra 2014-07-22 09:30:27 +02:00
parent cefd2de5af
commit 0303ac536a
18 changed files with 3637 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

264
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -91,6 +91,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_q
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_sample_counter = 0;
valid_solution_counter = 0;
d_last_sample_nav_output = 0;
d_rx_time = 0.0;
d_TOW_at_curr_symbol_constellation = 0.0;
@ -211,8 +212,264 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
if (d_ls_pvt->b_valid_position == true)
{
valid_solution_counter ++;
d_kml_dump.print_position_hybrid(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
/* *********************************** COMPUTE STATISTICS OVER THE FIRST 1000 SOLUTOINS **********************************/
if (valid_solution_counter<=1000)
//if (d_ls_pvt->d_valid_observations >= 14)
//if (d_ls_pvt->d_valid_GPS_obs >= 7 or d_ls_pvt->d_valid_GAL_obs >=7)
{
GDOP_vect.push_back (d_ls_pvt->d_GDOP);
//PDOP_vect.push_back (d_ls_pvt->d_PDOP);
X_vect.push_back (d_ls_pvt->X); // add the last X value to X_vect
Y_vect.push_back (d_ls_pvt->Y); // add the last X value to X_vect
Z_vect.push_back (d_ls_pvt->Z);
longitude_vect.push_back (d_ls_pvt->d_longitude_d);
latitude_vect.push_back (d_ls_pvt->d_latitude_d);
h_vect.push_back (d_ls_pvt->d_height_m);
tot_obs_vect.push_back (d_ls_pvt->d_valid_observations);
Gal_obs_vect.push_back (d_ls_pvt->d_valid_GAL_obs);
GPS_obs_vect.push_back (d_ls_pvt->d_valid_GPS_obs);
time_vect.push_back (d_ls_pvt->d_position_UTC_time);
//valid_solution_16_sat_counter ++;
}
//if(valid_solution_16_sat_counter ==1000)
if (valid_solution_counter==1000)
{
//compute ECEF average and standard deviation over the first 1000 solutions
GDOP_sum=std::accumulate( GDOP_vect.begin(), GDOP_vect.end(), 0.0);
GDOP_mean = GDOP_sum / GDOP_vect.size();
longitude_vect_sum = std::accumulate( longitude_vect.begin(), longitude_vect.end(), 0.0);
longitude_mean = longitude_vect_sum / longitude_vect.size();
//IFEN true solutions
// double ref_longitude= 11.80800563;
// double ref_latitude= 48.17149767;
//
// double ref_X=4171691.011;
// double ref_Y=872120.003;
// double ref_Z=4730005.761;
//REAL CAPTURE reference solutions (obtained with 4 Galileo)
double ref_longitude= 1.987686994;
double ref_latitude= 41.274786935;
double ref_X=4797680.560;
double ref_Y= 166506.414;
double ref_Z=4185453.947;
//compute mean value for precision
latitude_vect_sum = std::accumulate( latitude_vect.begin(), latitude_vect.end(), 0.0);
latitude_mean = latitude_vect_sum / latitude_vect.size();
X_vect_sum = std::accumulate(X_vect.begin(), X_vect.end(), 0.0);
X_vect_mean = X_vect_sum / X_vect.size();
X_vect_sq_sum = std::inner_product(X_vect.begin(), X_vect.end(), X_vect.begin(), 0.0);
X_vect_stdev = std::sqrt(X_vect_sq_sum / X_vect.size() - X_vect_mean * X_vect_mean);
Y_vect_sum = std::accumulate(Y_vect.begin(), Y_vect.end(), 0.0);
Y_vect_mean = Y_vect_sum / Y_vect.size();
Y_vect_sq_sum = std::inner_product(Y_vect.begin(), Y_vect.end(), Y_vect.begin(), 0.0);
Y_vect_stdev = std::sqrt(Y_vect_sq_sum / Y_vect.size() - Y_vect_mean * Y_vect_mean);
Z_vect_sum = std::accumulate(Z_vect.begin(), Z_vect.end(), 0.0);
Z_vect_mean = Z_vect_sum / Z_vect.size();
Z_vect_sq_sum = std::inner_product(Z_vect.begin(), Z_vect.end(), Z_vect.begin(), 0.0);
Z_vect_stdev = std::sqrt(Z_vect_sq_sum / Z_vect.size() - Z_vect_mean * Z_vect_mean);
std::fstream file_solutions;
std::ofstream file_statitics;
//std::fstream file_RES_X;
file_solutions.open ("GNSS_SDR_solutions.txt", std::fstream::out);
//file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out | std::ofstream::app);
file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out);
file_solutions.setf(file_solutions.fixed, file_solutions.floatfield);
file_statitics.setf(file_statitics.fixed, file_statitics.floatfield);
file_statitics << std::setprecision(9);
file_solutions << std::setw(13)<< "time" << std::setw(26)<<"X [m]" << std::setw(20)<< "Y [m]"<< std::setw(20)<< "Z [m]" << // X Y Z solutions
std::setw(20)<<"Long [deg]" << std::setw(15)<< "Lat [deg]"<< std::setw(15)<< "h [m]"<< // long, lat, h solutions
std::setw(18)<<"E(Acc) [m]" << std::setw(18)<< "N(Acc) [m]"<<std::setw(18)<<"Up(Acc) [m]"<< //ENU residual for accuracy columns 7:8:9
std::setw(18)<<"E(Pre) [m]" << std::setw(18)<< "N(Pre) [m]"<<std::setw(18)<<"Up(Pre) [m]"<< //ENU residual for precision columns 10:11:12
std::setw(18)<<"Tot Sat" << std::setw(18)<< "Gal"<<std::setw(18)<<"GPS"<< //number of satellites
std::setw(18)<<"GDOP" <<std::endl; //GDOP
std::cout << "X mean over 1000 solutions [m]= " << X_vect_mean << ", X dev st over 1000 solutins= " << X_vect_stdev << std::endl;
std::cout << "Y mean over 1000 solutions [m]= " << Y_vect_mean << ", Y dev st over 1000 solutins= " << Y_vect_stdev << std::endl;
std::cout << "Z mean over 1000 solutions [m]= " << Z_vect_mean << ", Z dev st over 1000 solutins= " << Z_vect_stdev << std::endl;
//file_statitics << "X mean over 1000 solutions [m]= " << X_vect_mean << ", X dev st over 1000 solutins= " << X_vect_stdev << std::endl;
//file_statitics << "Y mean over 1000 solutions [m]= " << Y_vect_mean << ", Y dev st over 1000 solutins= " << Y_vect_stdev << std::endl;
//file_statitics << "Z mean over 1000 solutions [m]= " << Z_vect_mean << ", Z dev st over 1000 solutins= " << Z_vect_stdev << std::endl << std::endl;
file_statitics << "Num of GPS observation " << d_ls_pvt->d_valid_GPS_obs << std::endl;
file_statitics << "Num of GALILEO observation " << d_ls_pvt->d_valid_GAL_obs << std::endl;
file_statitics << "GDOP mean= " << GDOP_mean << std::endl << std::endl;
file_statitics << "ENU computed at (IFEN true coordinates): ref Longitude = " << ref_longitude << ", Ref Latitude = " << ref_latitude << " for Accuracy"<<std::endl;
file_statitics << "ENU computed at (average coordinates) mean Longitude = " << longitude_mean << ", mean Latitude = " << latitude_mean << " for Precision"<<std::endl;
//file_statitics << "Residual computed with respect (IFEN true coordinates) X Y Z= " << ref_X << '\t'<< ref_Y << '\t'<< ref_Z << std::endl;
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat R = arma::zeros(3,3);//matrix with REFERENCE position
arma::mat M = arma::zeros(3,3);//matrix with MEAN position
//matrix with MEAN position
M(0,0) = -sin(GPS_TWO_PI*(longitude_mean/360.0));
M(0,1) = -sin(GPS_TWO_PI*(latitude_mean/360.0))*cos(GPS_TWO_PI*(longitude_mean/360.0));
M(0,2) = cos(GPS_TWO_PI*(latitude_mean/360.0))*cos(GPS_TWO_PI*(longitude_mean/360.0));
M(1,0) = cos((GPS_TWO_PI*longitude_mean)/360.0);
M(1,1) = -sin((GPS_TWO_PI*latitude_mean)/360.0)*sin((GPS_TWO_PI*longitude_mean)/360.0);
M(1,2) = cos((GPS_TWO_PI*latitude_mean/360.0))*sin((GPS_TWO_PI*longitude_mean)/360.0);
M(2,0) = 0;
M(2,1) = cos((GPS_TWO_PI*latitude_mean)/360.0);
M(2,2) = sin((GPS_TWO_PI*latitude_mean/360.0));
//matrix with REFERENCE position
R(0,0) = -sin(GPS_TWO_PI*(ref_longitude/360.0));
R(0,1) = -sin(GPS_TWO_PI*(ref_latitude/360.0))*cos(GPS_TWO_PI*(ref_longitude/360.0));
R(0,2) = cos(GPS_TWO_PI*(ref_latitude/360.0))*cos(GPS_TWO_PI*(ref_longitude/360.0));
R(1,0) = cos((GPS_TWO_PI*ref_longitude)/360.0);
R(1,1) = -sin((GPS_TWO_PI*ref_latitude)/360.0)*sin((GPS_TWO_PI*ref_longitude)/360.0);
R(1,2) = cos((GPS_TWO_PI*ref_latitude/360.0))*sin((GPS_TWO_PI*ref_longitude)/360.0);
R(2,0) = 0;
R(2,1) = cos((GPS_TWO_PI*ref_latitude)/360.0);
R(2,2) = sin((GPS_TWO_PI*ref_latitude/360.0));
for(unsigned i=0; i<X_vect.size(); i++ )
{
//compute residual with respect to the REFERENCE SOLUTIONS (true IFEN coordinates)
X_vect_res.push_back (X_vect[i] - ref_X); //accuracy
Y_vect_res.push_back (Y_vect[i] - ref_Y); //accuracy
Z_vect_res.push_back (Z_vect[i] - ref_Z); //accuracy
//compute residual with respect to the MEAN SOLUTIONS
X_vect_res_precision.push_back (X_vect[i] - X_vect_mean); //precision
Y_vect_res_precision.push_back (Y_vect[i] - Y_vect_mean); //precision
Z_vect_res_precision.push_back (Z_vect[i] - Z_vect_mean); //precision
//apply to residual value the rotation from ECEF to ENU IN REFERENCE POSITION (matrix R)
E_res.push_back (R(0,0)*X_vect_res[i] + R(1,0)*Y_vect_res[i] + R(2,0) * Z_vect_res[i]);
N_res.push_back (R(0,1)*X_vect_res[i] + R(1,1)*Y_vect_res[i] + R(2,1) * Z_vect_res[i]);
Up_res.push_back (R(0,2)*X_vect_res[i] + R(1,2)*Y_vect_res[i] + R(2,2) * Z_vect_res[i]);
//apply to residual value the rotation from ECEF to ENU MEAN POSITOIN (matrix R)
E_res_precision.push_back (M(0,0)*X_vect_res_precision[i] + M(1,0)*Y_vect_res_precision[i] + M(2,0) * Z_vect_res_precision[i]);
N_res_precision.push_back (M(0,1)*X_vect_res_precision[i] + M(1,1)*Y_vect_res_precision[i] + M(2,1) * Z_vect_res_precision[i]);
Up_res_precision.push_back (M(0,2)*X_vect_res_precision[i] + M(1,2)*Y_vect_res_precision[i] + M(2,2) * Z_vect_res_precision[i]);
file_solutions << std::setprecision(5);
file_solutions << std::setw(13)<< time_vect[i]<<
std::setw(22)<< X_vect[i]<< std::setw(20)<< Y_vect[i]<< std::setw(20)<< Z_vect[i] << // X Y Z solutions
std::setw(15)<< longitude_vect [i]<<std::setw(15)<< latitude_vect [i]<< std::setw(15)<< h_vect [i]<< // long, lat, h solutions
std::setw(18)<< E_res[i]<< std::setw(18)<< N_res[i]<< std::setw(18)<< Up_res[i]<< //ENU residual for accuracy columns 7:8:9
std::setw(18)<< E_res_precision[i]<< std::setw(18)<< N_res_precision[i]<< std::setw(18)<<Up_res_precision[i]<< //ENU residual for precision columns 10:11:12
std::setw(18)<< std::setprecision(0)<<tot_obs_vect[i] << std::setw(18)<< std::setprecision(0)<<Gal_obs_vect[i] << std::setw(18)<<std::setprecision(0)<< GPS_obs_vect[i] << // number of satellites
std::setw(18)<<std::setprecision(2)<<GDOP_vect[i] <<std::endl;
}
//compute ENU average and standard deviation over the first 1000 residual solutions ACCURACY
E_res_sum = std::accumulate(E_res.begin(), E_res.end(), 0.0);
E_res_mean = E_res_sum / E_res.size();
E_res_sq_sum = std::inner_product(E_res.begin(), E_res.end(), E_res.begin(), 0.0);
E_res_stdev = std::sqrt(E_res_sq_sum / E_res.size() - E_res_mean * E_res_mean);
N_res_sum = std::accumulate(N_res.begin(), N_res.end(), 0.0);
N_res_mean = N_res_sum / N_res.size();
N_res_sq_sum = std::inner_product(N_res.begin(), N_res.end(), N_res.begin(), 0.0);
N_res_stdev = std::sqrt(N_res_sq_sum / N_res.size() - N_res_mean * N_res_mean);
Up_res_sum = std::accumulate(Up_res.begin(), Up_res.end(), 0.0);
Up_res_mean = Up_res_sum / Up_res.size();
Up_res_sq_sum = std::inner_product(Up_res.begin(), Up_res.end(), Up_res.begin(), 0.0);
Up_res_stdev = std::sqrt(Up_res_sq_sum / Up_res.size() - Up_res_mean * Up_res_mean);
//compute ENU average and standard deviation over the first 1000 residual solutions PRECISION
E_res_sum_precision = std::accumulate(E_res_precision.begin(), E_res_precision.end(), 0.0);
E_res_mean_precision = E_res_sum_precision / E_res_precision.size();
E_res_sq_sum_precision = std::inner_product(E_res_precision.begin(), E_res_precision.end(), E_res_precision.begin(), 0.0);
E_res_stdev_precision = std::sqrt(E_res_sq_sum_precision / E_res_precision.size() - E_res_mean_precision * E_res_mean_precision);
N_res_sum_precision = std::accumulate(N_res_precision.begin(), N_res_precision.end(), 0.0);
N_res_mean_precision = N_res_sum_precision / N_res_precision.size();
N_res_sq_sum_precision = std::inner_product(N_res_precision.begin(), N_res_precision.end(), N_res_precision.begin(), 0.0);
N_res_stdev_precision = std::sqrt(N_res_sq_sum_precision / N_res_precision.size() - N_res_mean_precision * N_res_mean_precision);
Up_res_sum_precision = std::accumulate(Up_res_precision.begin(), Up_res_precision.end(), 0.0);
Up_res_mean_precision = Up_res_sum_precision / Up_res_precision.size();
Up_res_sq_sum_precision = std::inner_product(Up_res_precision.begin(), Up_res_precision.end(), Up_res_precision.begin(), 0.0);
Up_res_stdev_precision = std::sqrt(Up_res_sq_sum_precision / Up_res_precision.size() - Up_res_mean_precision * Up_res_mean_precision);
DRMS = std::sqrt( E_res_stdev * E_res_stdev + N_res_stdev*N_res_stdev);
DUE_DRMS = 2*DRMS;
CEP = 0.62 * N_res_stdev + 0.56 * E_res_stdev;
MRSE = std::sqrt( E_res_stdev * E_res_stdev + N_res_stdev*N_res_stdev + Up_res_stdev*Up_res_stdev);
SEP = 0.51* (E_res_stdev * E_res_stdev + N_res_stdev*N_res_stdev + Up_res_stdev*Up_res_stdev);
file_statitics << std::endl;
file_statitics << "ACCURACY (respect true position)" << std::endl;
file_statitics << "East offset [m] = " << E_res_mean << ", East st. dev = " << E_res_stdev << std::endl;
file_statitics << "Nord offset [m] = " << N_res_mean << ",Noth st. dev = " << N_res_stdev << std::endl;
file_statitics << "Up offset [m] = " << Up_res_mean << ", Up st. dev = " << Up_res_stdev << std::endl;
file_statitics << std::endl;
file_statitics << "DRMS= " << DRMS << std::endl;
file_statitics << "DUE_DRMS= " << DUE_DRMS << std::endl;
file_statitics << "CEP= " << CEP << std::endl;
file_statitics << "MRSE= " << MRSE << std::endl;
file_statitics << "SEP= " << SEP << std::endl;
file_statitics << std::endl;
file_statitics << "PRECISION (respect average solution)" << std::endl;
file_statitics << "East offset [m] = " << E_res_mean_precision << ", East st. dev = " << E_res_stdev_precision << std::endl;
file_statitics << "Nord offset [m] = " << N_res_mean_precision << ", ,Noth st. dev = " << N_res_stdev_precision << std::endl;
file_statitics << "Up offset [m]= " << Up_res_mean_precision << ", Up st. dev = " << Up_res_stdev_precision << std::endl;
file_statitics << "----------------------------------------------------------------------------------------------" << std::endl;
file_solutions.close();
file_statitics.close();
}/* END *********************************** COMPUTE STATISTICS OVER THE FIRST 1000 SOLUTOINS **********************************/
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
//
// if (!b_rinex_header_writen) // & we have utc data in nav message!
@ -260,6 +517,11 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
<< " using "<<d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = "
<< d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP
<< " GDOP = " << d_ls_pvt->d_GDOP;
}
// MULTIPLEXED FILE RECORDING - Record results to file

95
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h
View File

@ -109,11 +109,106 @@ private:
int d_output_rate_ms;
int d_display_rate_ms;
long unsigned int d_sample_counter;
long unsigned int valid_solution_counter;
long unsigned int valid_solution_16_sat_counter;
long unsigned int d_last_sample_nav_output;
Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer;
double d_rx_time;
double d_TOW_at_curr_symbol_constellation;
/* *********variable used for final statistics****/
std::vector<double> GDOP_vect;
std::vector<double> PDOP_vect;
double PDOP_sum;
double PDOP_mean;
double GDOP_sum;
double GDOP_mean;
std::vector<double> tot_obs_vect;
std::vector<double> Gal_obs_vect;
std::vector<double> GPS_obs_vect;
std::vector<boost::posix_time::ptime> time_vect;
std::vector<double> longitude_vect;
std::vector<double> latitude_vect;
std::vector<double> h_vect;
std::vector<double> X_vect;
std::vector<double> X_vect_res;
std::vector<double> X_vect_res_precision;
std::vector<double> Y_vect;
std::vector<double> Y_vect_res;
std::vector<double> Y_vect_res_precision;
std::vector<double> Z_vect;
std::vector<double> Z_vect_res;
std::vector<double> Z_vect_res_precision;
std::vector<double> E_res;
std::vector<double> N_res;
std::vector<double> Up_res;
std::vector<double> E_res_precision;
std::vector<double> N_res_precision;
std::vector<double> Up_res_precision;
double longitude_vect_sum;
double longitude_mean;
double latitude_vect_sum;
double latitude_mean;
double X_vect_sum;
double X_vect_mean;
double X_vect_sq_sum;
double X_vect_stdev;
double Y_vect_sum;
double Y_vect_mean;
double Y_vect_sq_sum;
double Y_vect_stdev;
double Z_vect_sum;
double Z_vect_mean;
double Z_vect_sq_sum;
double Z_vect_stdev;
//precision
double E_res_sum_precision;
double E_res_mean_precision;
double E_res_sq_sum_precision;
double E_res_stdev_precision;
double N_res_sum_precision;
double N_res_mean_precision;
double N_res_sq_sum_precision;
double N_res_stdev_precision;
double Up_res_sum_precision;
double Up_res_mean_precision;
double Up_res_sq_sum_precision;
double Up_res_stdev_precision;
//accuracy
double E_res_sum;
double E_res_mean;
double E_res_sq_sum;
double E_res_stdev;
double N_res_sum;
double N_res_mean;
double N_res_sq_sum;
double N_res_stdev;
double Up_res_sum;
double Up_res_mean;
double Up_res_sq_sum;
double Up_res_stdev;
double DRMS; //2D-65%
double DUE_DRMS; //2D-95%
double CEP; //2D-50%
double MRSE; //3D-61%
double SEP; //3D-50%
/* END variable used for final statistics */
hybrid_ls_pvt *d_ls_pvt;
bool pseudoranges_pairCompare_min(std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b);

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

@ -244,6 +244,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter = 0;
int valid_obs_GPS_counter = 0;
int valid_obs_GALILEO_counter = 0;
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
@ -298,7 +300,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
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;
valid_obs++;
valid_obs_GALILEO_counter ++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
//debug
@ -334,6 +336,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
}
}
else if (gnss_pseudoranges_iter->second.System == 'G')
{
@ -373,7 +377,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
valid_obs_GPS_counter++;
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
@ -398,6 +402,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
}
}
@ -412,6 +417,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
d_valid_observations = valid_obs;
d_valid_GPS_obs = valid_obs_GPS_counter;
d_valid_GAL_obs = valid_obs_GALILEO_counter;
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if (valid_obs >= 4)
@ -454,6 +461,14 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
LOG(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]";
//std::cout <<"X = " << mypos(0) << "Y = " << mypos(1) << "Z = " << mypos(2) << std::endl;
X = (double)mypos(0);
Y = (double)mypos(1);
Z = (double)mypos(2);
int count_solutions;
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
@ -470,7 +485,20 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
// std::fstream fs;
// fs.open ("append_all_solutions.txt", std::fstream::out | std::fstream::app); //file printed for debug purposes
// fs.setf(fs.fixed, fs.floatfield);
// fs << std::setprecision(5);
// fs << boost::posix_time::to_simple_string(p_time) <<'\t'<< '\t'<< X << '\t'<< '\t'<< Y << '\t'<< '\t'<< Z << '\t'<< '\t' <<
// d_latitude_d << '\t'<< '\t'<< d_longitude_d << '\t'<< '\t'<< d_height_m << '\t'<< '\t' <<
// valid_obs << '\t'<< '\t'<< valid_obs_GALILEO_counter <<'\t'<< '\t'<< valid_obs_GPS_counter << std::endl;
// fs.close();
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);

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

@ -68,6 +68,8 @@ private:
public:
int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_valid_GPS_obs; //!< Number of valid GPS pseudorange observations (valid GPS satellites) -- used for hybrid configuration
int d_valid_GAL_obs; //!< Number of valid GALILEO pseudorange observations (valid GALILEO satellites) -- used for hybrid configuration
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
@ -87,13 +89,14 @@ public:
double d_galileo_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
int count_valid_position;
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
double X;
double Y;
double Z;
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;

41
src/utils/gnuplot/4_GPS_3_GAL.plt Normal file
View File

@ -0,0 +1,41 @@
#set terminal pdf color font "Bold,14"
#set output "IFEN_accuracy.pdf"
set terminal jpeg font "Helvetica, 14"
set output "4_GPS_3_GAL_accuracy_precision.jpeg"
set grid
set xrange [-10:10]
set yrange [-5:15]
set ylabel "North [m]"
set xlabel "East [m]"
set key Left left
set title "IFEN simulated data, 4 GPS, 8 Gal - Accuracy and Precision"
#file1="8_GPS_GNSS_SDR_solutions.txt"
#file2="8_GAL_GNSS_SDR_solutions.txt"
file3="4_GPS_3_GAL_GNSS_SDR_solutions.txt"
#values to copy from statistic file
DRMS= 3.077806456
DUE_DRMS= 6.155612912
CEP= 2.565164055
#difference with respect to the reference position
#values to copy from statistic file
delta_E= -1.812 # combined
delta_N= 3.596 # combined
set parametric
#dummy variable is t for curves, u/v for surfaces
set size square
set angle degree
set trange [0:360]
#radius_6_GPS=6
plot file3 u 9:10 with points pointsize 0.3 lc rgb "green" notitle,\
DRMS*sin(t)+delta_E,DRMS*cos(t)+delta_N lw 3 lc rgb "black" title "DRMS",\
DUE_DRMS*sin(t)+delta_E,DUE_DRMS*cos(t)+delta_N lw 2 lc rgb "gray" title "2DRMS",\
CEP*sin(t)+delta_E,CEP*cos(t)+delta_N lw 1 lc rgb "black" title "CEP"

1001
src/utils/gnuplot/4_GPS_3_GAL_GNSS_SDR_solutions.txt Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
src/utils/gnuplot/4_GPS_3_GAL_accuracy_precision.jpeg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 47 KiB

BIN
src/utils/gnuplot/8_GALILEO_accuracy_precision.jpeg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 52 KiB

1001
src/utils/gnuplot/8_GAL_GNSS_SDR_solutions.txt Normal file
View File

File diff suppressed because it is too large Load Diff

40
src/utils/gnuplot/8_GPS.plt Normal file
View File

@ -0,0 +1,40 @@
#set terminal pdf color font "Bold,14"
#set output "IFEN_accuracy.pdf"
set terminal jpeg font "Helvetica, 14"
set output "8_GPS_accuracy_precision.jpeg"
set grid
set xrange [-8:8]
set yrange [-8:8]
set ylabel "North [m]"
set xlabel "East [m]"
set key Left left
set title "IFEN simulated data, 8 GPS - Accuracy and Precision"
file1="8_GPS_GNSS_SDR_solutions.txt"
#file2="8_GAL_GNSS_SDR_solutions.txt"
#file3="8_GPS_GNSS_SDR_solutions.txt"
#values to copy from statistic file
DRMS= 2.034509899
DUE_DRMS= 4.069019799
CEP= 1.678044871
#difference with respect to the reference position
#values to copy from statistic file
delta_E=-0.560 #gps
delta_N=1.323 #gps
set parametric
#dummy variable is t for curves, u/v for surfaces
set size square
set angle degree
set trange [0:360]
#radius_6_GPS=6
plot file1 u 9:10 with points pointsize 0.3 lc rgb "red" notitle,\
DRMS*sin(t)+delta_E,DRMS*cos(t)+delta_N lw 3 lc rgb "black" title "DRMS",\
DUE_DRMS*sin(t)+delta_E,DUE_DRMS*cos(t)+delta_N lw 2 lc rgb "gray" title "2DRMS",\
CEP*sin(t)+delta_E,CEP*cos(t)+delta_N lw 1 lc rgb "black" title "CEP"

1001
src/utils/gnuplot/8_GPS_GNSS_SDR_solutions.txt Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
src/utils/gnuplot/8_GPS_accuracy_precision.jpeg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

40
src/utils/gnuplot/8_Galileo.plt Normal file
View File

@ -0,0 +1,40 @@
#set terminal pdf color font "Bold,14"
#set output "IFEN_accuracy.pdf"
set terminal jpeg font "Helvetica, 14"
set output "8_GALILEO_accuracy_precision.jpeg"
set grid
set xrange [-8:8]
set yrange [-8:8]
set ylabel "North [m]"
set xlabel "East [m]"
set key Left left
set title "IFEN simulated data, 8 Galileo - Accuracy and Precision"
#file1="8_GPS_GNSS_SDR_solutions.txt"
file2="8_GAL_GNSS_SDR_solutions.txt"
#file3="8_GPS_GNSS_SDR_solutions.txt"
#values to copy from statistic file
DRMS= 1.870121081
DUE_DRMS= 3.740242162
CEP= 1.556390643
#difference with respect to the reference position
#values to copy from statistic file
delta_E=1.191 #galileo
delta_N=1.923 #galileo
set parametric
#dummy variable is t for curves, u/v for surfaces
set size square
set angle degree
set trange [0:360]
#radius_6_GPS=6
plot file2 u 9:10 with points pointsize 0.3 lc rgb "blue" notitle,\
DRMS*sin(t)+delta_E,DRMS*cos(t)+delta_N lw 3 lc rgb "black" title "DRMS",\
DUE_DRMS*sin(t)+delta_E,DUE_DRMS*cos(t)+delta_N lw 2 lc rgb "gray" title "2DRMS",\
CEP*sin(t)+delta_E,CEP*cos(t)+delta_N lw 1 lc rgb "black" title "CEP"

BIN
src/utils/gnuplot/8_sat_accuracy_precision.jpeg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 49 KiB

50
src/utils/gnuplot/8_sat_accuracy_precision.plt Normal file
View File

@ -0,0 +1,50 @@
#to load the file digit from terminal:
#> gnuplot 8_sat_IFEN_accuracy_precision.plt
#set terminal pdf color font "Bold,14"
#set output "IFEN_solutions_pdf"
set terminal jpeg font "Helvetica, 14"
set output "8_sat_accuracy_precision.jpeg"
set grid
set xrange [-15:15]
set yrange [-10:20]
set ylabel "North [m]"
set xlabel "East [m]"
set key Left left
set title "Accuracy-Precision (with respect to CORRECT coordinates)- 2DRMS"
file1="4_GPS_3_GAL_GNSS_SDR_solutions.txt"
file2="8_GAL_GNSS_SDR_solutions.txt"
file3="8_GPS_GNSS_SDR_solutions.txt"
#values to copy from statistic file
DRMS_1=2*3.077 #it is 2*DRMS combined
DRMS_2=2*1.87 # gal
DRMS_3=2*2.034 # gps
#difference with respect to the reference position
#values to copy from statistic file
delta_E_1=-1.812 #combined
delta_N_1= 3.596 #combined
delta_E_2= 1.191 #gal
delta_N_2= 1.923 #gal
delta_E_3= -0.560 #gps
delta_N_3= 1.323 #gps
set parametric
#dummy variable is t for curves, u/v for surfaces
set size square
set angle degree
set trange [0:360]
plot file1 u 9:10 with points pointsize 0.3 lc rgb "green" title "4 GPS-3 GAL",\
file3 u 9:10 with points pointsize 0.3 lc rgb "red" title "8 GPS",\
file2 u 9:10 with points pointsize 0.3 lc rgb "blue" title "8 GAL",\
DRMS_1*sin(t)+delta_E_1,DRMS_1*cos(t)+delta_N_1 lw 2 lc rgb "green" notitle,\
DRMS_3*sin(t)+delta_E_3,DRMS_3*cos(t)+delta_N_3 lw 2 lc rgb "red" notitle,\
DRMS_2*sin(t)+delta_E_2,DRMS_2*cos(t)+delta_N_2 lw 2 lc rgb "blue" notitle

23
src/utils/gnuplot/statistics/4_GPS_3_GAL_GNSS_SDR_statitics.txt Normal file
View File

@ -0,0 +1,23 @@
Num of GPS observation 4
Num of GALILEO observation 3
GDOP mean= 2.380532594
ENU computed at (IFEN true coordinates): ref Longitude = 11.808005630, Ref Latitude = 48.171497670 for Accuracy
ENU computed at (average coordinates) mean Longitude = 11.807981252, mean Latitude = 48.171530020 for Precision
ACCURACY (respect true position)
East offset [m] = -1.812959237, East st. dev = 1.899085141
Nord offset [m] = 3.596061973,Noth st. dev = 2.422058671
Up offset [m] = 8.995532878, Up st. dev = 3.881428324
DRMS= 3.077806456
DUE_DRMS= 6.155612912
CEP= 2.565164055
MRSE= 4.953622757
SEP= 12.514572993
PRECISION (respect average solution)
East offset [m] = 0.000000000, East st. dev = 1.899086239
Nord offset [m] = -0.000000001, ,Noth st. dev = 2.422059160
Up offset [m]= -0.000000003, Up st. dev = 3.881427482
----------------------------------------------------------------------------------------------

23
src/utils/gnuplot/statistics/8_GAL_GNSS_SDR_statitics.txt Normal file
View File

@ -0,0 +1,23 @@
Num of GPS observation 0
Num of GALILEO observation 8
GDOP mean= 1.769225604
ENU computed at (IFEN true coordinates): ref Longitude = 11.808005630, Ref Latitude = 48.171497670 for Accuracy
ENU computed at (average coordinates) mean Longitude = 11.808021645, mean Latitude = 48.171514975 for Precision
ACCURACY (respect true position)
East offset [m] = 1.191616778, East st. dev = 1.370472661
Nord offset [m] = 1.923075914,Noth st. dev = 1.272461214
Up offset [m] = 13.774563698, Up st. dev = 3.492269580
DRMS= 1.870121081
DUE_DRMS= 3.740242162
CEP= 1.556390643
MRSE= 3.961476957
SEP= 8.003582836
PRECISION (respect average solution)
East offset [m] = -0.000000002, East st. dev = 1.370472897
Nord offset [m] = -0.000000001, ,Noth st. dev = 1.272461012
Up offset [m]= 0.000000002, Up st. dev = 3.492269562
----------------------------------------------------------------------------------------------

23
src/utils/gnuplot/statistics/8_GPS_GNSS_SDR_statitics.txt Normal file
View File

@ -0,0 +1,23 @@
Num of GPS observation 8
Num of GALILEO observation 0
GDOP mean= 2.002216944
ENU computed at (IFEN true coordinates): ref Longitude = 11.808005630, Ref Latitude = 48.171497670 for Accuracy
ENU computed at (average coordinates) mean Longitude = 11.807998091, mean Latitude = 48.171509585 for Precision
ACCURACY (respect true position)
East offset [m] = -0.560396234, East st. dev = 1.105718017
Nord offset [m] = 1.323685667,Noth st. dev = 1.707810937
Up offset [m] = 10.792857384, Up st. dev = 3.121160956
DRMS= 2.034509899
DUE_DRMS= 4.069019799
CEP= 1.678044871
MRSE= 3.725704798
SEP= 7.079246885
PRECISION (respect average solution)
East offset [m] = 0.000000000, East st. dev = 1.105718027
Nord offset [m] = -0.000000005, ,Noth st. dev = 1.707811217
Up offset [m]= -0.000000005, Up st. dev = 3.121160800
----------------------------------------------------------------------------------------------