mirror of https://github.com/gnss-sdr/gnss-sdr
Added GNSS-SDR statistics over PVT and gnuplot scripts
This commit is contained in:
marabra
parent
cefd2de5af
commit
0303ac536a
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 47 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 52 KiB |
File diff suppressed because it is too large
Load Diff
|
|
@ -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"
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 54 KiB |
|
|
@ -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"
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 49 KiB |
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
----------------------------------------------------------------------------------------------
|
||||
|
|
@ -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
|
||||
----------------------------------------------------------------------------------------------
|
||||
|
|
@ -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
|
||||
----------------------------------------------------------------------------------------------
|
||||
Loading…
Reference in New Issue