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

FORMAT: clang-format applied

Browse Source
This commit is contained in:
M.A.Gomez 2023-03-11 19:32:16 +00:00
parent fd69416f4e
commit b815ee4d9d
6 changed files with 489 additions and 471 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/algorithms/PVT/libs/rtklib_solver.cc Executable file → Normal file
View File

@ -168,7 +168,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
try try
{ {
d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit);
uint end_filename = d_dump_filename.length()-4; uint end_filename = d_dump_filename.length() - 4;
d_vtl_dump_filename = d_dump_filename; d_vtl_dump_filename = d_dump_filename;
d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl"); d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl");
d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary); d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary);
@ -218,7 +218,7 @@ Rtklib_Solver::~Rtklib_Solver()
LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what();
} }
} }
if (d_flag_dump_mat_enabled) if (d_flag_dump_mat_enabled)
{ {
try try
{ {
@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const
{ {
// READ DUMP FILE // READ DUMP FILE
const std::string dump_filename = d_vtl_dump_filename; const std::string dump_filename = d_vtl_dump_filename;
const int32_t number_of_double_vars = 27; const int32_t number_of_double_vars = 27;
const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 1; const int32_t number_of_uint8_vars = 1;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
@ -353,7 +353,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr) if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{ {
std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)}; 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); 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_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
@ -474,7 +473,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); 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_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
} }
Mat_Close(matfp); Mat_Close(matfp);
@ -486,7 +484,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 = 21; const int32_t number_of_double_vars = 21;
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;
@ -1893,7 +1891,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
vtl_engine.vtl_loop(vtl_data); vtl_engine.vtl_loop(vtl_data);
} }
else else
{ {
@ -2001,7 +1998,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_clock_drift_ppm(clock_drift_ppm); this->set_clock_drift_ppm(clock_drift_ppm);
// User clock drift [ppm] // User clock drift [ppm]
d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm;
// ######## LOG FILE ######### // ######## LOG FILE #########
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
{ {
@ -2085,10 +2082,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file
if (enable_vtl == true) if (enable_vtl == true)
try try
{ {
double tmp_double; double tmp_double;
uint32_t tmp_uint32; uint32_t tmp_uint32;
// TOW // TOW
@ -2104,7 +2100,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
//tmp_double = rx_position_and_time[3]; //tmp_double = rx_position_and_time[3];
tmp_double = vtl_engine.get_user_clock_offset_s(); tmp_double = vtl_engine.get_user_clock_offset_s();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); 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) // 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_engine.get_position_ecef_m(); std::vector<double> p_vec_m = vtl_engine.get_position_ecef_m();
tmp_double = p_vec_m[0]; tmp_double = p_vec_m[0];
@ -2113,8 +2109,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[2]; tmp_double = p_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s(); std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s();
tmp_double = v_vec_m[0]; tmp_double = v_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[1]; tmp_double = v_vec_m[1];
@ -2122,7 +2118,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = v_vec_m[2]; tmp_double = v_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2(); std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2();
tmp_double = a_vec_m[0]; tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[1]; tmp_double = a_vec_m[1];
@ -2132,7 +2128,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// position/velocity/acceleration variance/ (units^2) (9 x double) // position/velocity/acceleration variance/ (units^2) (9 x double)
std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m(); std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m();
tmp_double = p_var_vec_m[0]; tmp_double = p_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[1]; tmp_double = p_var_vec_m[1];
@ -2141,7 +2137,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s();
tmp_double = v_var_vec_m[0]; tmp_double = v_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[1]; tmp_double = v_var_vec_m[1];
@ -2149,7 +2145,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
tmp_double = v_var_vec_m[2]; tmp_double = v_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2();
tmp_double = a_var_vec_m[0]; tmp_double = a_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[1]; tmp_double = a_var_vec_m[1];
@ -2158,13 +2154,13 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg] // GEO user position Latitude [deg]
tmp_double = this->get_latitude(); tmp_double = this->get_latitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Longitude [deg] // GEO user position Longitude [deg]
tmp_double = this->get_longitude(); tmp_double = this->get_longitude();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Height [m] // GEO user position Height [m]
tmp_double = this->get_height(); tmp_double = this->get_height();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS // NUMBER OF VALID SATS

8
src/algorithms/PVT/libs/vtl_data.cc Executable file → Normal file
View File

@ -16,8 +16,8 @@
#include "vtl_data.h" #include "vtl_data.h"
#include "vector"
#include "armadillo" #include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data() Vtl_Data::Vtl_Data()
{ {
@ -46,7 +46,7 @@ void Vtl_Data::init_storage(int n_sats)
rx_dts = arma::mat(1, 2); rx_dts = arma::mat(1, 2);
rx_var = arma::vec(1); rx_var = arma::vec(1);
rx_pvt_var = arma::vec(8); rx_pvt_var = arma::vec(8);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
} }
@ -61,10 +61,8 @@ void Vtl_Data::debug_print()
// sat_health_flag.print("VTL Sat health"); // sat_health_flag.print("VTL Sat health");
//sat_LOS.print("VTL SAT LOS"); //sat_LOS.print("VTL SAT LOS");
kf_state.print("EKF STATE"); kf_state.print("EKF STATE");
//pr_m.print("Satellite Code pseudoranges [m]"); //pr_m.print("Satellite Code pseudoranges [m]");
//doppler_hz.print("satellite Carrier Dopplers [Hz]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
} }

32
src/algorithms/PVT/libs/vtl_data.h Executable file → Normal file
View File

@ -36,29 +36,29 @@ public:
Vtl_Data(); Vtl_Data();
void init_storage(int n_sats); void init_storage(int n_sats);
arma::mat sat_p; // Satellite ECEF Position [m] arma::mat sat_p; // Satellite ECEF Position [m]
arma::mat sat_v; // Satellite Velocity [m/s] arma::mat sat_v; // Satellite Velocity [m/s]
arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] arma::mat sat_dts; // Satellite clock bias and drift [s,s/s]
arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_var; // sat position and clock error variance [m^2]
arma::colvec sat_health_flag; // sat health flag (0 is ok) arma::colvec sat_health_flag; // sat health flag (0 is ok)
arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz
arma::mat sat_LOS; // sat LOS arma::mat sat_LOS; // sat LOS
int sat_number; // on-view sat number int sat_number; // on-view sat number
arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec pr_m; // Satellite Code pseudoranges [m]
arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz]
arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads]
arma::colvec pr_res; // pseudorange residual arma::colvec pr_res; // pseudorange residual
arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_p; // Receiver ENU Position [m]
arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_v; // Receiver Velocity [m/s]
arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s]
arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s]
arma::colvec rx_var; // Receiver position and clock error variance [m^2] arma::colvec rx_var; // Receiver position and clock error variance [m^2]
arma::mat kf_state; // KF STATE arma::mat kf_state; // KF STATE
arma::mat kf_P; // KF STATE covariance arma::mat kf_P; // KF STATE covariance
// time handling // time handling
double PV[6]; // position and Velocity double PV[6]; // position and Velocity
double epoch_tow_s; // current observation RX time [s] double epoch_tow_s; // current observation RX time [s]
uint64_t sample_counter; // current sample counter associated with RX time [samples from start] uint64_t sample_counter; // current sample counter associated with RX time [samples from start]
void debug_print(); void debug_print();

767
src/algorithms/PVT/libs/vtl_engine.cc Executable file → Normal file
View File

File diff suppressed because it is too large Load Diff

42
src/algorithms/PVT/libs/vtl_engine.h Executable file → Normal file
View File

@ -17,10 +17,10 @@
#ifndef GNSS_SDR_VTL_ENGINE_H #ifndef GNSS_SDR_VTL_ENGINE_H
#define GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H
#include "MATH_CONSTANTS.h"
#include "trackingcmd.h" #include "trackingcmd.h"
#include "vtl_conf.h" #include "vtl_conf.h"
#include "vtl_data.h" #include "vtl_data.h"
#include "MATH_CONSTANTS.h"
#include <armadillo> #include <armadillo>
#include <cstdint> #include <cstdint>
#include <string> #include <string>
@ -46,22 +46,22 @@ public:
void reset(); // reset all internal states void reset(); // reset all internal states
void debug_print(); // print debug information void debug_print(); // print debug information
std::vector<TrackingCmd> trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs std::vector<TrackingCmd> trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs
std::vector<double> get_position_ecef_m(); // get_position_ecef_m 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_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_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_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_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 std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
double get_latitude(); // get_latitude double get_latitude(); // get_latitude
double get_longitude(); // get_longitude double get_longitude(); // get_longitude
double get_height(); // get_height double get_height(); // get_height
double get_user_clock_offset_s(); // get_user_clock_offset_s; double get_user_clock_offset_s(); // get_user_clock_offset_s;
private: private:
Vtl_Conf config; Vtl_Conf config;
//TODO: Internal VTL persistent variables here //TODO: Internal VTL persistent variables here
// Transformation variables // Transformation variables
arma::colvec d; arma::colvec d;
arma::colvec rho_pri; arma::colvec rho_pri;
@ -73,13 +73,13 @@ private:
arma::colvec a_x; arma::colvec a_x;
arma::colvec a_y; arma::colvec a_y;
arma::colvec a_z; arma::colvec a_z;
// Kalman filter matrices // Kalman filter matrices
arma::mat kf_P_x_ini; // initial state error covariance matrix arma::mat kf_P_x_ini; // initial state error covariance matrix
// arma::mat kf_P_x; // state error covariance matrix // arma::mat kf_P_x; // state error covariance matrix
arma::mat kf_P_x_pre; // Predicted state error covariance matrix arma::mat kf_P_x_pre; // Predicted state error covariance matrix
arma::mat kf_P_x; arma::mat kf_P_x;
arma::mat kf_S; // innovation covariance matrix arma::mat kf_S; // innovation covariance matrix
arma::mat kf_F; // state transition matrix arma::mat kf_F; // state transition matrix
arma::mat kf_H; // system matrix arma::mat kf_H; // system matrix
@ -91,7 +91,7 @@ private:
arma::mat kf_y; // measurement vector arma::mat kf_y; // measurement vector
arma::mat kf_yerr; // ERROR measurement vector arma::mat kf_yerr; // ERROR measurement vector
arma::mat kf_xerr; // ERROR state vector arma::mat kf_xerr; // ERROR state vector
arma::mat kf_K; // Kalman gain matrix arma::mat kf_K; // Kalman gain matrix
// Gaussian estimator // Gaussian estimator
arma::mat kf_R_est; // measurement error covariance arma::mat kf_R_est; // measurement error covariance
@ -101,12 +101,12 @@ private:
int n_of_states; int n_of_states;
uint64_t refSampleCounter; uint64_t refSampleCounter;
bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor
bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor
bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation bool obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation
bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt, int counter); bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
double EmpujeLkTable(double t_disparo); double EmpujeLkTable(double t_disparo);
}; };
/** \} */ /** \} */

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

@ -49,16 +49,16 @@
#include <matio.h> // for Mat_VarCreate #include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp #include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include "iostream"
#include <algorithm> // for fill_n #include <algorithm> // for fill_n
#include <array> #include <array>
#include <cmath> // for fmod, round, floor #include <cmath> // for fmod, round, floor
#include <exception> // for exception #include <exception> // for exception
#include <iostream> // for cout, cerr #include <fstream>
#include <iostream> // for cout, cerr
#include <map> #include <map>
#include <numeric> #include <numeric>
#include <vector> #include <vector>
#include "iostream"
#include <fstream>
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
#else #else
@ -637,7 +637,6 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
//std::cout<< "test cast CH "<<cmd->sample_counter <<"\n"; //std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel) if (cmd->channel_id == this->d_channel)
{ {
arma::vec x_tmp; arma::vec x_tmp;
arma::mat F_tmp; arma::mat F_tmp;
@ -645,7 +644,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
//To.Do: apply VTL corrections to the KF states //To.Do: apply VTL corrections to the KF states
double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in;
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
//ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces
F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0},
{0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)},
@ -656,29 +655,35 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
double old_doppler = d_x_old_old(2); double old_doppler = d_x_old_old(2);
double old_doppler_rate = d_x_old_old(3); double old_doppler_rate = d_x_old_old(3);
double old_code_phase_chips = d_x_old_old(0); double old_code_phase_chips = d_x_old_old(0);
if(cmd->enable_carrier_nco_cmd){
if(cmd->enable_code_nco_cmd){
if(abs(d_x_old_old(2) - tmp_x(2))>50){
std::cout <<"channel: "<< this->d_channel
<< " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< " \n";
}else{
std::cout <<"channel: "<< this->d_channel
<< " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2))<< "Hz"
<< " \n";
}
d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}else{ if (cmd->enable_carrier_nco_cmd)
// std::cout<<"yet to soon"<<std::endl; {
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER if (cmd->enable_code_nco_cmd)
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE {
if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
{
std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
}
else
{
std::cout << "channel: " << this->d_channel
<< " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
}
d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
else
{
// std::cout<<"yet to soon"<<std::endl;
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
} }
}
// set vtl corrections flag to inform VTL from gnss_synchro object // set vtl corrections flag to inform VTL from gnss_synchro object
d_vtl_cmd_applied_now = true; d_vtl_cmd_applied_now = true;
@ -688,7 +693,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
// << " SampleCounter origin: " << cmd->sample_counter // << " SampleCounter origin: " << cmd->sample_counter
// << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]"
// << " [s]\n"; // << " [s]\n";
// if(cmd->channel_id ==0) // if(cmd->channel_id ==0)
// { // {
// std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: "
// << delta_t_s << "[s]" // << delta_t_s << "[s]"
@ -702,15 +707,15 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app); dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app);
dump_tracking_file.precision(15); dump_tracking_file.precision(15);
if (!dump_tracking_file) if (!dump_tracking_file)
{ {
std::cout << "dump_tracking_file not created!"; std::cout << "dump_tracking_file not created!";
} }
else else
{ {
dump_tracking_file << "doppler_corr" dump_tracking_file << "doppler_corr"
<< ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; << "," << this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3) << "," << old_doppler_rate << "\n";
dump_tracking_file.close(); dump_tracking_file.close();
} }
} }
} }
else else
@ -1252,7 +1257,7 @@ void kf_tracking::run_Kf()
// new code phase estimation // new code phase estimation
d_code_error_kf_chips = d_x_new_new(0); d_code_error_kf_chips = d_x_new_new(0);
d_x_new_new(0)=0; // reset error estimation because the NCO corrects the code phase d_x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase
// new carrier phase estimation // new carrier phase estimation
d_carrier_phase_kf_rad = d_x_new_new(1); d_carrier_phase_kf_rad = d_x_new_new(1);