mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-04-27 05:03:15 +00:00
ADD: vtl raw and mat files
This commit is contained in:
M.A. Gomez
parent
bf1a7e8c2c
commit
40d3e96902
@ -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
2
src/algorithms/PVT/libs/rtklib_solver.h
Normal file → Executable 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{};
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user