From b815ee4d9dd45bdd6c1efb7d2216926aa61b4547 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 11 Mar 2023 19:32:16 +0000 Subject: [PATCH] FORMAT: clang-format applied --- src/algorithms/PVT/libs/rtklib_solver.cc | 34 +- src/algorithms/PVT/libs/vtl_data.cc | 8 +- src/algorithms/PVT/libs/vtl_data.h | 32 +- src/algorithms/PVT/libs/vtl_engine.cc | 767 +++++++++--------- src/algorithms/PVT/libs/vtl_engine.h | 42 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 77 +- 6 files changed, 489 insertions(+), 471 deletions(-) mode change 100755 => 100644 src/algorithms/PVT/libs/rtklib_solver.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_data.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_data.h mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_engine.cc mode change 100755 => 100644 src/algorithms/PVT/libs/vtl_engine.h diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100755 new mode 100644 index f5696a96b..7dbc2f711 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -168,7 +168,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, try { 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_vtl_dump_filename.insert(end_filename, "_vtl"); 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(); } } - if (d_flag_dump_mat_enabled) + if (d_flag_dump_mat_enabled) { try { @@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE 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_uint8_vars = 1; 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); if (reinterpret_cast(matfp) != nullptr) { - std::array dims{1, static_cast(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 @@ -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); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - } Mat_Close(matfp); @@ -486,7 +484,7 @@ bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; - const int32_t number_of_double_vars = 21; + const int32_t number_of_double_vars = 21; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -1893,7 +1891,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] vtl_engine.vtl_loop(vtl_data); - } else { @@ -2001,7 +1998,7 @@ bool Rtklib_Solver::get_PVT(const std::map &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) { @@ -2085,10 +2082,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file if (enable_vtl == true) - + try { - double tmp_double; uint32_t tmp_uint32; // TOW @@ -2104,7 +2100,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //tmp_double = rx_position_and_time[3]; tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&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 p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = p_vec_m[0]; @@ -2113,8 +2109,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); + + std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); tmp_double = v_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_vec_m[1]; @@ -2122,7 +2118,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); + std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); tmp_double = a_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_vec_m[1]; @@ -2132,7 +2128,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // position/velocity/acceleration variance/ (units^2) (9 x double) - std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); + std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); tmp_double = p_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_var_vec_m[1]; @@ -2141,7 +2137,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); + std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); tmp_double = v_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = v_var_vec_m[1]; @@ -2149,7 +2145,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = v_var_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); + vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); tmp_double = a_var_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = a_var_vec_m[1]; @@ -2158,13 +2154,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); + tmp_double = this->get_latitude(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Longitude [deg] tmp_double = this->get_longitude(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Height [m] - tmp_double = this->get_height(); + tmp_double = this->get_height(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // NUMBER OF VALID SATS diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc old mode 100755 new mode 100644 index 74057a159..b7d5d7fd7 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -16,8 +16,8 @@ #include "vtl_data.h" -#include "vector" #include "armadillo" +#include "vector" Vtl_Data::Vtl_Data() { @@ -46,7 +46,7 @@ void Vtl_Data::init_storage(int n_sats) rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); rx_pvt_var = arma::vec(8); - + epoch_tow_s = 0; sample_counter = 0; } @@ -61,10 +61,8 @@ void Vtl_Data::debug_print() // sat_health_flag.print("VTL Sat health"); //sat_LOS.print("VTL SAT LOS"); kf_state.print("EKF STATE"); - + //pr_m.print("Satellite Code pseudoranges [m]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } - - diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h old mode 100755 new mode 100644 index b01e49ffe..502b99351 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -36,29 +36,29 @@ public: Vtl_Data(); void init_storage(int n_sats); - arma::mat sat_p; // Satellite ECEF Position [m] - arma::mat sat_v; // Satellite Velocity [m/s] - arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] + arma::mat sat_p; // Satellite ECEF Position [m] + arma::mat sat_v; // Satellite Velocity [m/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_health_flag; // sat health flag (0 is ok) - arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz - arma::mat sat_LOS; // sat LOS - int sat_number; // on-view sat number - + arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz + arma::mat sat_LOS; // sat LOS + int sat_number; // on-view sat number + arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz] arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads] arma::colvec pr_res; // pseudorange residual - - arma::mat rx_p; // Receiver ENU Position [m] - arma::mat rx_v; // Receiver Velocity [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::colvec rx_var; // Receiver position and clock error variance [m^2] - arma::mat kf_state; // KF STATE - arma::mat kf_P; // KF STATE covariance + + arma::mat rx_p; // Receiver ENU Position [m] + arma::mat rx_v; // Receiver Velocity [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::colvec rx_var; // Receiver position and clock error variance [m^2] + arma::mat kf_state; // KF STATE + arma::mat kf_P; // KF STATE covariance // time handling - double PV[6]; // position and Velocity + double PV[6]; // position and Velocity 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(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc old mode 100755 new mode 100644 index 7384e2659..46aeb28b2 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -22,10 +22,10 @@ using namespace std; Vtl_Engine::Vtl_Engine() { - counter=0; - refSampleCounter=0; - n_of_states=11; - kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; + counter = 0; + refSampleCounter = 0; + n_of_states = 11; + kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; kf_x = arma::zeros(n_of_states, 1); } @@ -37,48 +37,50 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - - if (refSampleCounter=0) - { - refSampleCounter=new_data.sample_counter; - } - double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; + + if (refSampleCounter = 0) + { + refSampleCounter = new_data.sample_counter; + } + double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; static double delta_t_cmd = 0; bool flag_cmd = false; - delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command - if(delta_t_cmd>=0.3){ - flag_cmd = true; - delta_t_cmd = 0; // reset timer for vtl trk command - } + delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command + if (delta_t_cmd >= 0.3) + { + flag_cmd = true; + delta_t_cmd = 0; // reset timer for vtl trk command + } // ################## Kalman filter initialization ###################################### //State variables arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) - + kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); - double kf_dt = delta_t_vtl; //0.05; + double kf_dt = delta_t_vtl; //0.05; kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); - bool test = kf_F_fill(kf_F,kf_dt); + bool test = kf_F_fill(kf_F, kf_dt); //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_y = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix - kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; + kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); + ; // ################## Kalman Tracking ###################################### - counter++; //uint64_t + counter++; //uint64_t //new_data.kf_state.print("new_data kf initial"); - uint32_t closure_point=3; - + uint32_t closure_point = 3; + if (counter < closure_point) - { + { // // receiver solution from rtklib_solver - kf_dx=kf_x; + kf_dx = kf_x; kf_x(0) = new_data.rx_p(0); kf_x(1) = new_data.rx_p(1); kf_x(2) = new_data.rx_p(2); @@ -91,7 +93,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_dx = kf_x-kf_dx; + kf_dx = kf_x - kf_dx; kf_dx = kf_F * kf_dx; // state prediction } else @@ -100,7 +102,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt,counter); + test = model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -108,7 +110,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } - + // State error Covariance Matrix Q (PVT) //careful, values for V and T could not be adecuate. kf_Q(0, 0) = 100.0; @@ -122,15 +124,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_Q(8, 8) = .10; kf_Q(9, 9) = 4.0; kf_Q(10, 10) = 10.0; - + // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); - + kf_R(i, i) = 80.0; //*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; //*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; //*50.0/new_data.sat_CN0_dB_hz(i); + // if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ // kf_R(i, i) = 10e4; // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; @@ -159,7 +161,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) a_y = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1); // cout<<"llegado aqui tambien"<5){ + // if(new_data.sat_number>5){ // // dump_vtl_file << "pr_m" - // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) // // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<1500){ - if(u>6){ - t_disparo=t_disparo+dt; - std::cout<<"u : "< 1500) + { + if (u > 6) + { + t_disparo = t_disparo + dt; + std::cout << "u : " << u << endl; + double diam_cohete = 120.0e-3; // 120 mm + double mass_rocket = 50.0; //50Kg - // acc_vec.print("acc_vec"); - // % return - cout<<"modelo 3Dof actuando,t:"< Vtl_Engine::get_position_ecef_m() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[0]; temp[1] = kf_x[1]; temp[2] = kf_x[2]; @@ -655,7 +677,7 @@ std::vector Vtl_Engine::get_position_ecef_m() std::vector Vtl_Engine::get_velocity_ecef_m_s() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[3]; temp[1] = kf_x[4]; temp[2] = kf_x[5]; @@ -665,7 +687,7 @@ std::vector Vtl_Engine::get_velocity_ecef_m_s() std::vector Vtl_Engine::get_accel_ecef_m_s2() { - std::vector temp = {42,42,42}; + std::vector temp = {42, 42, 42}; temp[0] = kf_x[6]; temp[1] = kf_x[7]; temp[2] = kf_x[8]; @@ -674,44 +696,41 @@ std::vector Vtl_Engine::get_accel_ecef_m_s2() } std::vector Vtl_Engine::get_position_var_ecef_m() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(0,0); - temp[1] = kf_P_x(1,1); - temp[2] = kf_P_x(2,2); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(0, 0); + temp[1] = kf_P_x(1, 1); + temp[2] = kf_P_x(2, 2); return temp; } std::vector Vtl_Engine::get_velocity_var_ecef_m_s() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(3,3); - temp[1] = kf_P_x(4,4); - temp[2] = kf_P_x(5,5); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(3, 3); + temp[1] = kf_P_x(4, 4); + temp[2] = kf_P_x(5, 5); return temp; } std::vector Vtl_Engine::get_accel_var_ecef_m_s2() { - std::vector temp = {42,42,42}; - temp[0] = kf_P_x(6,6); - temp[1] = kf_P_x(7,7); - temp[2] = kf_P_x(8,8); + std::vector temp = {42, 42, 42}; + temp[0] = kf_P_x(6, 6); + temp[1] = kf_P_x(7, 7); + temp[2] = kf_P_x(8, 8); return temp; } double Vtl_Engine::get_latitude() { - return -1.0; } double Vtl_Engine::get_longitude() { - - return -1.0; } @@ -722,7 +741,7 @@ double Vtl_Engine::get_height() double Vtl_Engine::get_user_clock_offset_s() { - double temp=0; + double temp = 0; temp = kf_x[9]; return temp; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h old mode 100755 new mode 100644 index 751e30362..5317114a1 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -17,10 +17,10 @@ #ifndef GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H +#include "MATH_CONSTANTS.h" #include "trackingcmd.h" #include "vtl_conf.h" #include "vtl_data.h" -#include "MATH_CONSTANTS.h" #include #include #include @@ -46,22 +46,22 @@ public: void reset(); // reset all internal states void debug_print(); // print debug information - std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs - std::vector get_position_ecef_m(); // get_position_ecef_m - std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s - std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 - std::vector get_position_var_ecef_m(); // get_position_var_ecef_m - std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s - std::vector 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; + std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs + std::vector get_position_ecef_m(); // get_position_ecef_m + std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s + std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 + std::vector get_position_var_ecef_m(); // get_position_var_ecef_m + std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s + std::vector 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; private: Vtl_Conf config; //TODO: Internal VTL persistent variables here - + // Transformation variables arma::colvec d; arma::colvec rho_pri; @@ -73,13 +73,13 @@ private: arma::colvec a_x; arma::colvec a_y; arma::colvec a_z; - + // Kalman filter matrices 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_pre; // Predicted state error covariance matrix 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_H; // system matrix @@ -91,7 +91,7 @@ private: arma::mat kf_y; // measurement vector arma::mat kf_yerr; // ERROR measurement vector arma::mat kf_xerr; // ERROR state vector - arma::mat kf_K; // Kalman gain matrix + arma::mat kf_K; // Kalman gain matrix // Gaussian estimator arma::mat kf_R_est; // measurement error covariance @@ -101,12 +101,12 @@ private: int n_of_states; 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_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 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 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 model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt, int counter); - double EmpujeLkTable(double t_disparo); + bool model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); + double EmpujeLkTable(double t_disparo); }; /** \} */ diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 0bcec91a6..cd61ed51f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -49,16 +49,16 @@ #include // for Mat_VarCreate #include // for mp #include +#include "iostream" #include // for fill_n #include #include // for fmod, round, floor #include // for exception -#include // for cout, cerr +#include +#include // for cout, cerr #include #include #include -#include "iostream" -#include #if HAS_GENERIC_LAMBDA #else @@ -637,7 +637,6 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) //std::cout<< "test cast CH "<sample_counter <<"\n"; if (cmd->channel_id == this->d_channel) { - arma::vec x_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 double delta_t_s = static_cast(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 - 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 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)}, @@ -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_rate = d_x_old_old(3); 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{ - // std::cout<<"yet to soon"<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 + { + // std::cout<<"yet to soon"<sample_counter // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" // << " [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: " // << 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.precision(15); if (!dump_tracking_file) - { - std::cout << "dump_tracking_file not created!"; - } + { + std::cout << "dump_tracking_file not created!"; + } else - { - dump_tracking_file << "doppler_corr" - << ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; - dump_tracking_file.close(); - } + { + dump_tracking_file << "doppler_corr" + << "," << this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3) << "," << old_doppler_rate << "\n"; + dump_tracking_file.close(); + } } } else @@ -1252,7 +1257,7 @@ void kf_tracking::run_Kf() // new code phase estimation 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 d_carrier_phase_kf_rad = d_x_new_new(1);