mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 22:34:58 +00:00
Code Issues Releases Wiki Activity

ADD: more info to out mat file

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-04 23:35:31 +00:00
parent 4831c78898
commit c2e161efc2
2 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -205,7 +205,7 @@ bool Rtklib_Solver::save_matfile() const
// READ DUMP FILE // READ DUMP FILE
const std::string dump_filename = d_dump_filename; const std::string dump_filename = d_dump_filename;
const int32_t number_of_double_vars = 22; //+1 MAGL const int32_t number_of_double_vars = 22; //+1 MAGL
const int32_t number_of_double_vars_sat = 6; //+ pos/vel por satelite const int32_t number_of_double_vars_sat = 8; //+ pos/vel/psrange/doppler for sat
const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 3; const int32_t number_of_uint8_vars = 3;
const int32_t number_of_float_vars = 2; const int32_t number_of_float_vars = 2;
@ -263,6 +263,8 @@ bool Rtklib_Solver::save_matfile() const
auto sat_velX = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch)); auto sat_velX = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch));
auto sat_velY = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch)); auto sat_velY = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch));
auto sat_velZ = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch)); auto sat_velZ = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch));
auto sat_prg_m = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch));
auto sat_dopp_hz = std::vector<std::vector<double>>(5, std::vector<double>(num_epoch));
auto latitude = std::vector<double>(num_epoch); auto latitude = std::vector<double>(num_epoch);
auto longitude = std::vector<double>(num_epoch); auto longitude = std::vector<double>(num_epoch);
@ -308,7 +310,9 @@ bool Rtklib_Solver::save_matfile() const
dump_file.read(reinterpret_cast<char *>(&sat_posZ_m[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&sat_posZ_m[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_velX[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&sat_velX[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_velY[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&sat_velY[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_velZ[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&sat_velZ[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_prg_m[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_dopp_hz[chan][i]), sizeof(double));
} }
dump_file.read(reinterpret_cast<char *>(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&latitude[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&longitude[i]), sizeof(double));
@ -338,6 +342,8 @@ bool Rtklib_Solver::save_matfile() const
auto sat_velX_aux = std::vector<double>(5 * num_epoch); auto sat_velX_aux = std::vector<double>(5 * num_epoch);
auto sat_velY_aux = std::vector<double>(5 * num_epoch); auto sat_velY_aux = std::vector<double>(5 * num_epoch);
auto sat_velZ_aux = std::vector<double>(5 * num_epoch); auto sat_velZ_aux = std::vector<double>(5 * num_epoch);
auto sat_prg_m_aux = std::vector<double>(5 * num_epoch);
auto sat_dopp_hz_aux = std::vector<double>(5 * num_epoch);
uint32_t k = 0U; uint32_t k = 0U;
for (int64_t j = 0; j < num_epoch; j++) for (int64_t j = 0; j < num_epoch; j++)
@ -350,6 +356,8 @@ bool Rtklib_Solver::save_matfile() const
sat_velX_aux[k] = sat_velX[i][j]; sat_velX_aux[k] = sat_velX[i][j];
sat_velY_aux[k] = sat_velY[i][j]; sat_velY_aux[k] = sat_velY[i][j];
sat_velZ_aux[k] = sat_velZ[i][j]; sat_velZ_aux[k] = sat_velZ[i][j];
sat_prg_m_aux[k] = sat_prg_m[i][j];
sat_dopp_hz_aux[k] = sat_dopp_hz[i][j];
k++; k++;
} }
} }
@ -428,7 +436,7 @@ bool Rtklib_Solver::save_matfile() const
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_z2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0); matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@ -451,6 +459,13 @@ bool Rtklib_Solver::save_matfile() const
matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
@ -1211,9 +1226,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; vtl_data.rx_pvt_var[1] = pvt_sol.qr[1];
vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; vtl_data.rx_pvt_var[2] = pvt_sol.qr[2];
//TODO: get direct estimations for V T variances, instead: //TODO: get direct estimations for V T variances, instead:
vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]; //in general minor than position.
vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; vtl_data.rx_pvt_var[4] = pvt_sol.qr[1];
vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; vtl_data.rx_pvt_var[5] = pvt_sol.qr[2];
vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time
vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler
//receiver clock offset and receiver clock drift //receiver clock offset and receiver clock drift
@ -1386,7 +1401,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = pvt_sol.qr[5]; tmp_double = pvt_sol.qr[5];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.rr[5]; tmp_double = vtl_data.rx_dts(0);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
for (int n = 0; n<5; n++) for (int n = 0; n<5; n++)
{ {
@ -1401,7 +1416,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = vtl_data.sat_v(n, 1); tmp_double = vtl_data.sat_v(n, 1);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_v(n, 2); tmp_double = vtl_data.sat_v(n, 2);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.pr_m(n);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.doppler_hz(n);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
// GEO user position Latitude [deg] // GEO user position Latitude [deg]

3
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -191,7 +191,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
else { else {
dump_vtl_file << "kf_xerr"<< ","<<kf_xerr(0)<< ","<<kf_xerr(1)<< ","<<kf_xerr(2)<< ","<<kf_xerr(3)<< ","<<kf_xerr(4)<< ","<<kf_xerr(5)<< ","<<kf_xerr(6)<< ","<<kf_xerr(7)<<endl; dump_vtl_file << "kf_xerr"<< ","<<kf_xerr(0)<< ","<<kf_xerr(1)<< ","<<kf_xerr(2)<< ","<<kf_xerr(3)<< ","<<kf_xerr(4)<< ","<<kf_xerr(5)<< ","<<kf_xerr(6)<< ","<<kf_xerr(7)<<endl;
dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(7)<<endl; dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(7)<<endl;
dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p[2]<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl; dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p[2]<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl;
dump_vtl_file << "sat_first_LOS"<< ","<<new_data.sat_LOS(1,0)<< ","<< new_data.sat_LOS(1,1)<<","<< new_data.sat_LOS(1,2)<<endl;
dump_vtl_file.close(); dump_vtl_file.close();
} }