mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

[ADD] Velocity covariances to dump file

This commit is contained in:
M.A. Gomez 2023-07-12 18:22:31 +02:00
parent e840593033
commit f886fcd085
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
1 changed files with 51 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -194,7 +194,7 @@ bool Rtklib_Solver::save_matfile() const
{
// READ DUMP FILE
const std::string dump_filename = d_dump_filename;
const int32_t number_of_double_vars = 21;
const int32_t number_of_double_vars = 27;
const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 3;
const int32_t number_of_float_vars = 2;
@ -243,6 +243,12 @@ bool Rtklib_Solver::save_matfile() const
auto cov_xy = std::vector<double>(num_epoch);
auto cov_yz = std::vector<double>(num_epoch);
auto cov_zx = std::vector<double>(num_epoch);
auto cov_vxx = std::vector<double>(num_epoch);
auto cov_vyy = std::vector<double>(num_epoch);
auto cov_vzz = std::vector<double>(num_epoch);
auto cov_vxy = std::vector<double>(num_epoch);
auto cov_vyz = std::vector<double>(num_epoch);
auto cov_vzx = std::vector<double>(num_epoch);
auto latitude = std::vector<double>(num_epoch);
auto longitude = std::vector<double>(num_epoch);
auto height = std::vector<double>(num_epoch);
@ -278,6 +284,12 @@ bool Rtklib_Solver::save_matfile() const
dump_file.read(reinterpret_cast<char *>(&cov_xy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_yz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_zx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vxx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vyy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vzz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vxy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vyz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vzx[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 *>(&height[i]), sizeof(double));
@ -374,6 +386,30 @@ bool Rtklib_Solver::save_matfile() const
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vxx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vxx.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vyy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vyy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vzz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vzz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vxy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vxy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vyz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vyz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vzx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vzx.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);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
@ -1744,6 +1780,20 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = pvt_sol.qr[5];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// velocity variance/covariance (m^2/s^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double)
tmp_double = pvt_sol.qvr[0];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.qvr[1];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.qvr[2];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.qvr[3];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.qvr[4];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = pvt_sol.qvr[5];
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg]
tmp_double = this->get_latitude();
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));