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

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-10-12 10:56:20 +02:00
commit 1b0015c13b
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
1 changed files with 63 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
src/tests/system-tests/position_test.cc
View File

@ -451,24 +451,25 @@ void PositionSystemTest::check_results()
rtklib_solver_dump_reader pvt_reader; rtklib_solver_dump_reader pvt_reader;
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename); pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
int64_t n_epochs = pvt_reader.num_epochs(); int64_t n_epochs = pvt_reader.num_epochs();
R_eb_e = arma::zeros(n_epochs, 3); R_eb_e = arma::zeros(3, n_epochs);
V_eb_e = arma::zeros(n_epochs, 3); V_eb_e = arma::zeros(3, n_epochs);
LLH = arma::zeros(n_epochs, 3); LLH = arma::zeros(3, n_epochs);
receiver_time_s = arma::zeros(n_epochs, 1); receiver_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0; int64_t current_epoch = 0;
while (pvt_reader.read_binary_obs()) while (pvt_reader.read_binary_obs())
{ {
receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s; receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
R_eb_e(current_epoch, 0) = pvt_reader.rr[0]; R_eb_e(0, current_epoch) = pvt_reader.rr[0];
R_eb_e(current_epoch, 1) = pvt_reader.rr[1]; R_eb_e(1, current_epoch) = pvt_reader.rr[1];
R_eb_e(current_epoch, 2) = pvt_reader.rr[2]; R_eb_e(2, current_epoch) = pvt_reader.rr[2];
V_eb_e(current_epoch, 0) = pvt_reader.rr[3]; V_eb_e(0, current_epoch) = pvt_reader.rr[3];
V_eb_e(current_epoch, 1) = pvt_reader.rr[4]; V_eb_e(1, current_epoch) = pvt_reader.rr[4];
V_eb_e(current_epoch, 2) = pvt_reader.rr[5]; V_eb_e(2, current_epoch) = pvt_reader.rr[5];
LLH(current_epoch, 0) = pvt_reader.latitude; LLH(0, current_epoch) = pvt_reader.latitude;
LLH(current_epoch, 1) = pvt_reader.longitude; LLH(1, current_epoch) = pvt_reader.longitude;
LLH(current_epoch, 2) = pvt_reader.height; LLH(2, current_epoch) = pvt_reader.height;
arma::vec tmp_r_enu;
arma::vec tmp_r_enu = {0, 0, 0};
cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu); cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
R_eb_enu.insert_cols(current_epoch, tmp_r_enu); R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
@ -508,20 +509,18 @@ void PositionSystemTest::check_results()
{ {
stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl; stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl;
} }
if (FLAGS_static_scenario)
{ stm << "---- ACCURACY ----" << std::endl;
stm << "---- ACCURACY ----" << std::endl; stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; stm << "CEP = " << 0.62 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [m]" << std::endl;
stm << "CEP = " << 0.62 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [m]" << std::endl; stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "Bias 2D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl;
stm << "Bias 2D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl; stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 2.0)) << " [m]" << std::endl;
stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 2.0)) << " [m]" << std::endl; stm << std::endl;
stm << std::endl;
}
stm << "---- PRECISION ----" << std::endl; stm << "---- PRECISION ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
@ -556,56 +555,55 @@ void PositionSystemTest::check_results()
spirent_motion_csv_dump_reader ref_reader; spirent_motion_csv_dump_reader ref_reader;
ref_reader.open_obs_file(FLAGS_ref_motion_filename); ref_reader.open_obs_file(FLAGS_ref_motion_filename);
int64_t n_epochs = ref_reader.num_epochs(); int64_t n_epochs = ref_reader.num_epochs();
ref_R_eb_e = arma::zeros(n_epochs, 3); ref_R_eb_e = arma::zeros(3, n_epochs);
ref_V_eb_e = arma::zeros(n_epochs, 3); ref_V_eb_e = arma::zeros(3, n_epochs);
ref_LLH = arma::zeros(n_epochs, 3); ref_LLH = arma::zeros(3, n_epochs);
ref_time_s = arma::zeros(n_epochs, 1); ref_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0; int64_t current_epoch = 0;
while (ref_reader.read_csv_obs()) while (ref_reader.read_csv_obs())
{ {
ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0; ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0;
ref_R_eb_e(current_epoch, 0) = ref_reader.Pos_X; ref_R_eb_e(0, current_epoch) = ref_reader.Pos_X;
ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y; ref_R_eb_e(1, current_epoch) = ref_reader.Pos_Y;
ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z; ref_R_eb_e(2, current_epoch) = ref_reader.Pos_Z;
ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X; ref_V_eb_e(0, current_epoch) = ref_reader.Vel_X;
ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y; ref_V_eb_e(1, current_epoch) = ref_reader.Vel_Y;
ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z; ref_V_eb_e(2, current_epoch) = ref_reader.Vel_Z;
ref_LLH(current_epoch, 0) = ref_reader.Lat; ref_LLH(0, current_epoch) = ref_reader.Lat;
ref_LLH(current_epoch, 1) = ref_reader.Long; ref_LLH(1, current_epoch) = ref_reader.Long;
ref_LLH(current_epoch, 2) = ref_reader.Height; ref_LLH(2, current_epoch) = ref_reader.Height;
current_epoch++; current_epoch++;
} }
//interpolation of reference data to receiver epochs timestamps //interpolation of reference data to receiver epochs timestamps
arma::mat ref_interp_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); arma::mat ref_interp_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); arma::mat ref_interp_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3); arma::mat ref_interp_LLH = arma::zeros(3, LLH.n_cols);
arma::vec tmp_vector; arma::vec tmp_vector;
for (int n = 0; n < 3; n++) for (int n = 0; n < 3; n++)
{ {
arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_R_eb_e.row(n), receiver_time_s, tmp_vector);
ref_interp_R_eb_e.col(n) = tmp_vector; ref_interp_R_eb_e.row(n) = tmp_vector.t();
arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_V_eb_e.row(n), receiver_time_s, tmp_vector);
ref_interp_V_eb_e.col(n) = tmp_vector; ref_interp_V_eb_e.row(n) = tmp_vector.t();
arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_LLH.row(n), receiver_time_s, tmp_vector);
ref_interp_LLH.col(n) = tmp_vector; ref_interp_LLH.row(n) = tmp_vector.t();
} }
//compute error vectors //compute error vectors
arma::mat error_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); arma::mat error_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); arma::mat error_LLH = arma::zeros(3, LLH.n_cols);
arma::mat error_LLH = arma::zeros(LLH.n_rows, 3);
error_R_eb_e = R_eb_e - ref_interp_R_eb_e; error_R_eb_e = R_eb_e - ref_interp_R_eb_e;
error_V_eb_e = V_eb_e - ref_interp_V_eb_e; error_V_eb_e = V_eb_e - ref_interp_V_eb_e;
error_LLH = LLH - ref_interp_LLH; error_LLH = LLH - ref_interp_LLH;
arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_rows, 1); arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_cols, 1);
arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 1); arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_cols, 1);
for (uint64_t n = 0; n < R_eb_e.n_rows; n++) for (uint64_t n = 0; n < R_eb_e.n_cols; n++)
{ {
error_module_R_eb_e(n) = arma::norm(error_R_eb_e.row(n)); error_module_R_eb_e(n) = arma::norm(error_R_eb_e.col(n));
error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n)); error_module_V_eb_e(n) = arma::norm(error_V_eb_e.col(n));
} }
//Error statistics //Error statistics
arma::vec tmp_vec; arma::vec tmp_vec;
//RMSE, Mean, Variance and peaks //RMSE, Mean, Variance and peaks
@ -657,9 +655,13 @@ void PositionSystemTest::check_results()
g1.set_title("3D ECEF error coordinates"); g1.set_title("3D ECEF error coordinates");
g1.set_grid(); g1.set_grid();
//conversion between arma::vec and std:vector //conversion between arma::vec and std:vector
std::vector<double> X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_x = error_R_eb_e.row(0);
std::vector<double> Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_y = error_R_eb_e.row(1);
std::vector<double> Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_z = error_R_eb_e.row(2);
std::vector<double> X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows);
std::vector<double> Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows);
std::vector<double> Z(arma_vec_error_z.colptr(0), arma_vec_error_z.colptr(0) + arma_vec_error_z.n_rows);
g1.cmd("set key box opaque"); g1.cmd("set key box opaque");
g1.plot_xyz(X, Y, Z, "ECEF 3D error"); g1.plot_xyz(X, Y, Z, "ECEF 3D error");