mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-12 15:56:02 +00:00
Code Issues Releases Wiki Activity

ADD: vtl raw and mat files

Browse Source
This commit is contained in:
M.A. Gomez
2023-03-09 21:30:15 +01:00
parent bf1a7e8c2c
commit 40d3e96902
6 changed files with 476 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -162,6 +162,19 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what();
}
}
if (d_vtl_dump_file.is_open() == false)
{
try
{
d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit);
d_vtl_dump_file.open(d_dump_filename + "_vtl", std::ios::out | std::ios::binary);
LOG(INFO) << "PVT VTL dump enabled Log file: " << d_dump_filename + "_vtl";
}
catch (const std::ofstream::failure &e)
{
LOG(WARNING) << "Exception opening VTL dump file " << e.what();
}
}
}
}
@@ -195,6 +208,7 @@ Rtklib_Solver::~Rtklib_Solver()
try
{
save_matfile();
save_vtl_matfile();
}
catch (const std::exception &ex)
{
@@ -204,17 +218,266 @@ Rtklib_Solver::~Rtklib_Solver()
}
bool Rtklib_Solver::save_vtl_matfile() const
{
// READ DUMP FILE
const std::string dump_filename = d_dump_filename+ "_vtl";
const int32_t number_of_double_vars = 27;
const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 1;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars;
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
{
dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate);
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem opening VTL dump file:" << e.what() << '\n';
return false;
}
// count number of epochs and rewind
int64_t num_epoch = 0LL;
if (dump_file.is_open())
{
std::cout << "Generating .mat file for VTL " << dump_filename << '\n';
const std::ifstream::pos_type size = dump_file.tellg();
num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
dump_file.seekg(0, std::ios::beg);
}
else
{
return false;
}
//todo: cambiarlo
auto TOW_at_current_symbol_ms = std::vector<uint32_t>(num_epoch);
auto week = std::vector<uint32_t>(num_epoch);
auto RX_time = std::vector<double>(num_epoch);
auto user_clk_offset = std::vector<double>(num_epoch);
auto pos_x = std::vector<double>(num_epoch);
auto pos_y = std::vector<double>(num_epoch);
auto pos_z = std::vector<double>(num_epoch);
auto vel_x = std::vector<double>(num_epoch);
auto vel_y = std::vector<double>(num_epoch);
auto vel_z = std::vector<double>(num_epoch);
auto acc_x = std::vector<double>(num_epoch);
auto acc_y = std::vector<double>(num_epoch);
auto acc_z = std::vector<double>(num_epoch);
auto cov_xx = std::vector<double>(num_epoch);
auto cov_yy = std::vector<double>(num_epoch);
auto cov_zz = std::vector<double>(num_epoch);
auto cov_vx = std::vector<double>(num_epoch);
auto cov_vy = std::vector<double>(num_epoch);
auto cov_vz = std::vector<double>(num_epoch);
auto cov_ax = std::vector<double>(num_epoch);
auto cov_ay = std::vector<double>(num_epoch);
auto cov_az = 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);
auto valid_sats = std::vector<uint8_t>(num_epoch);
auto gdop = std::vector<double>(num_epoch);
auto pdop = std::vector<double>(num_epoch);
auto hdop = std::vector<double>(num_epoch);
auto vdop = std::vector<double>(num_epoch);
try
{
if (dump_file.is_open())
{
for (int64_t i = 0; i < num_epoch; i++)
{
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&week[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&RX_time[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&user_clk_offset[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_xx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_yy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_zz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_ax[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_ay[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_az[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));
dump_file.read(reinterpret_cast<char *>(&valid_sats[i]), sizeof(uint8_t));
dump_file.read(reinterpret_cast<char *>(&gdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&hdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vdop[i]), sizeof(double));
}
}
dump_file.close();
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << '\n';
return false;
}
// WRITE MAT FILE
mat_t *matfp;
matvar_t *matvar;
std::string filename = dump_filename;
filename.erase(filename.length() - 4, 4);
filename.append(".mat");
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vx.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_ax", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_ay", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_ay.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_az", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.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);
matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
return true;
}
bool Rtklib_Solver::save_matfile() const
{
// READ DUMP FILE
const std::string dump_filename = d_dump_filename;
const int32_t number_of_double_vars = 23; //+2 MAGL
const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat
const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 3;
const int32_t number_of_float_vars = 2;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(double) * number_of_double_vars_sat * 6 +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars +
sizeof(float) * number_of_float_vars;
@@ -259,19 +522,6 @@ 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 clk_bias = std::vector<double>(num_epoch);
auto clk_drift = std::vector<double>(num_epoch);
auto sat_posX_m = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_posY_m = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_posZ_m = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_velX = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_velY = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_velZ = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_prg_m = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_dopp_hz = std::vector<std::vector<double>>(6, std::vector<double>(num_epoch));
auto sat_CN0_dBhz = std::vector<std::vector<double>>(6, 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);
@@ -307,21 +557,6 @@ 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 *>(&clk_bias[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&clk_drift[i]), sizeof(double));
for (uint32_t chan = 0; chan < 6; chan++)
{
dump_file.read(reinterpret_cast<char *>(&sat_posX_m[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&sat_posY_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_velY[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 *>(&sat_CN0_dBhz[chan][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));
@@ -344,34 +579,6 @@ bool Rtklib_Solver::save_matfile() const
return false;
}
auto sat_posX_m_aux = std::vector<double>(6 * num_epoch);
auto sat_posY_m_aux = std::vector<double>(6 * num_epoch);
auto sat_posZ_m_aux = std::vector<double>(6 * num_epoch);
auto sat_velX_aux = std::vector<double>(6 * num_epoch);
auto sat_velY_aux = std::vector<double>(6 * num_epoch);
auto sat_velZ_aux = std::vector<double>(6 * num_epoch);
auto sat_prg_m_aux = std::vector<double>(6 * num_epoch);
auto sat_dopp_hz_aux = std::vector<double>(6 * num_epoch);
auto sat_CN0_dBhz_aux = std::vector<double>(6 * num_epoch);
uint32_t k = 0U;
for (int64_t j = 0; j < num_epoch; j++)
{
for (uint32_t i = 0; i < 6; i++)
{
sat_posX_m_aux[k] = sat_posX_m[i][j];
sat_posY_m_aux[k] = sat_posY_m[i][j];
sat_posZ_m_aux[k] = sat_posZ_m[i][j];
sat_velX_aux[k] = sat_velX[i][j];
sat_velY_aux[k] = sat_velY[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];
sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j];
k++;
}
}
// WRITE MAT FILE
mat_t *matfp;
matvar_t *matvar;
@@ -446,44 +653,6 @@ bool Rtklib_Solver::save_matfile() const
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_bias.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("clk_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_drift.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
std::array<size_t, 2> dims_sat{static_cast<size_t>(6), static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
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_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("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_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);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
@@ -1713,13 +1882,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
//new_vtl_data.debug_print();
//vtl_data.kf_state.print("kf_state_input");
vtl_engine.vtl_loop(vtl_data);
//vtl_data.kf_state.print("kf_state_output");
// pvt_sol.rr[0] = vtl_data.kf_state[0];
// pvt_sol.rr[1] = vtl_data.kf_state[1];
// pvt_sol.rr[2] = vtl_data.kf_state[2];
// pvt_sol.rr[3] = vtl_data.kf_state[3];
// pvt_sol.rr[4] = vtl_data.kf_state[4];
// pvt_sol.rr[5] = vtl_data.kf_state[5];
}
else
{
@@ -1827,7 +1990,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_clock_drift_ppm(clock_drift_ppm);
// User clock drift [ppm]
d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm;
// ######## LOG FILE #########
if (d_flag_dump_enabled == true)
{
@@ -1877,32 +2040,6 @@ 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));
tmp_double = vtl_data.rx_dts(0);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.rx_dts(1);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
for (int n = 0; n < 6; n++)
{
tmp_double = vtl_data.sat_p(n, 0);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_p(n, 1);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_p(n, 2);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_v(n, 0);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_v(n, 1);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = vtl_data.sat_v(n, 2);
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));
tmp_double = vtl_data.sat_CN0_dB_hz(n);
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));
@@ -1934,6 +2071,104 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what();
}
// VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file
if (enable_vtl == true)
try
{
double tmp_double;
uint32_t tmp_uint32;
// TOW
tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms;
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_uint32), sizeof(uint32_t));
// WEEK
tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009());
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_uint32), sizeof(uint32_t));
// PVT GPS time
tmp_double = gnss_observables_map.cbegin()->second.RX_time;
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// User clock offset [s]
//tmp_double = rx_position_and_time[3];
tmp_double = vtl_data.get_user_clock_offset_s();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double)
std::vector<double> p_vec_m = vtl_data.get_position_ecef_m();
tmp_double = p_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_vec_m = vtl_data.get_velocity_ecef_m_s();
tmp_double = v_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> a_vec_m = vtl_data.get_accel_ecef_m_s2();
tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// position/velocity/acceleration variance/ (units^2) (9 x double)
std::vector<double> p_var_vec_m = vtl_data.get_position_var_ecef_m();
tmp_double = p_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_var_vec_m = vtl_data.get_velocity_var_ecef_m_s();
tmp_double = v_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
vector<double> a_var_vec_m = vtl_data.get_accel_var_ecef_m_s2();
tmp_double = a_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg]
tmp_double = this->get_latitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Longitude [deg]
tmp_double = this->get_longitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Height [m]
tmp_double = this->get_height();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS
d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t));
// GDOP / PDOP / HDOP / VDOP
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[0]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[1]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[2]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[3]), sizeof(double));
}
catch (const std::ofstream::failure &e)
{
LOG(WARNING) << "Exception writing VTL dump file " << e.what();
}
}
}
}

2
src/algorithms/PVT/libs/rtklib_solver.h Normal file → Executable file
View File

@@ -132,6 +132,7 @@ public:
private:
bool save_matfile() const;
bool save_vtl_matfile() const;
void check_has_orbit_clock_validity(const std::map<int, Gnss_Synchro>& obs_map);
void get_has_biases(const std::map<int, Gnss_Synchro>& obs_map);
@@ -152,6 +153,7 @@ private:
std::string d_dump_filename;
std::ofstream d_dump_file;
std::ofstream d_vtl_dump_file;
rtk_t d_rtk{};
nav_t d_nav_data{};
Monitor_Pvt d_monitor_pvt{};

92
src/algorithms/PVT/libs/vtl_data.cc
View File

@@ -16,13 +16,15 @@
#include "vtl_data.h"
#include "vector"
#include "armadillo"
Vtl_Data::Vtl_Data()
{
epoch_tow_s = 0;
sample_counter = 0;
kf_state = arma::mat(8,1);
kf_P = arma::mat(8,8);
kf_state = arma::mat(11,1);
kf_P = arma::mat(11,11);
}
void Vtl_Data::init_storage(int n_sats)
@@ -66,3 +68,89 @@ void Vtl_Data::debug_print()
//doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
}
std::vector<double> Vtl_Data::get_position_ecef_m()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_state[0];
temp[1] = kf_state[1];
temp[2] = kf_state[2];
return temp;
}
std::vector<double> Vtl_Data::get_velocity_ecef_m_s()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_state[3];
temp[1] = kf_state[4];
temp[2] = kf_state[5];
return temp;
}
std::vector<double> Vtl_Data::get_accel_ecef_m_s2()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_state[6];
temp[1] = kf_state[7];
temp[2] = kf_state[8];
return temp;
}
std::vector<double> Vtl_Data::get_position_var_ecef_m()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_P(0,0);
temp[1] = kf_P(1,1);
temp[2] = kf_P(2,2);
return temp;
}
std::vector<double> Vtl_Data::get_velocity_var_ecef_m_s()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_P(3,3);
temp[1] = kf_P(4,4);
temp[2] = kf_P(5,5);
return temp;
}
std::vector<double> Vtl_Data::get_accel_var_ecef_m_s2()
{
std::vector<double> temp = {42,42,42};
temp[0] = kf_P(6,6);
temp[1] = kf_P(7,7);
temp[2] = kf_P(8,8);
return temp;
}
double Vtl_Data::get_latitude()
{
return -1.0;
}
double Vtl_Data::get_longitude()
{
return -1.0;
}
double Vtl_Data::get_height()
{
return -1.0;
}
double Vtl_Data::get_user_clock_offset_s()
{
double temp=0;
temp=kf_state[9];
return temp;
}

11
src/algorithms/PVT/libs/vtl_data.h
View File

@@ -22,6 +22,7 @@
#include <armadillo>
#include <cstdint>
#include <string>
#include <vector>
/** \addtogroup PVT
* \{ */
@@ -61,6 +62,16 @@ public:
double epoch_tow_s; // current observation RX time [s]
uint64_t sample_counter; // current sample counter associated with RX time [samples from start]
void debug_print();
std::vector<double> get_position_ecef_m(); // get_position_ecef_m
std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s
std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2
std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
double get_latitude(); // get_latitude
double get_longitude(); // get_longitude
double get_height(); // get_height
double get_user_clock_offset_s(); // get_user_clock_offset_s;
};

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

@@ -172,7 +172,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_xerr = kf_K * (kf_yerr); // Error state estimation
kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error)
new_data.kf_state = kf_x;
new_data.kf_P = kf_P_x;
if(abs(delta_t_vtl)>.5){
kf_xerr.print("kf_xERR: ");
cout<<"kf_dt: "<<delta_t_vtl<<endl;

12
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@@ -658,13 +658,13 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
double old_code_phase_chips = d_x_old_old(0);
if(abs(d_x_old_old(2) - tmp_x(2))>50){
std::cout << " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< " \n";
// std::cout << " tracking_cmd TOO FAR: "
// << abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
// << " \n";
}else{
std::cout << " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< " \n";
// std::cout << " tracking_cmd NEAR: "
// << abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
// << " \n";
//d_x_old_old(2) = tmp_x(2); //replace the Code Phase state
}