From 5b44ec996535585c5625b91f36e8d29b0fd21416 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Thu, 9 Mar 2023 23:52:05 +0100 Subject: [PATCH] MOD: remove static variables --- src/algorithms/PVT/libs/rtklib_solver.cc | 14 +-- src/algorithms/PVT/libs/vtl_data.cc | 86 ------------------- src/algorithms/PVT/libs/vtl_data.h | 10 --- src/algorithms/PVT/libs/vtl_engine.cc | 104 +++++++++++++++++++++-- src/algorithms/PVT/libs/vtl_engine.h | 18 +++- 5 files changed, 121 insertions(+), 111 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 27a723118..8f7c827c8 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -2091,11 +2091,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] //tmp_double = rx_position_and_time[3]; - tmp_double = vtl_data.get_user_clock_offset_s(); + 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_data.get_position_ecef_m(); + std::vector p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = p_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = p_vec_m[1]; @@ -2103,7 +2103,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = p_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_vec_m = vtl_data.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]; @@ -2111,7 +2111,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_data.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]; @@ -2121,7 +2121,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_data.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]; @@ -2130,7 +2130,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_data.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]; @@ -2138,7 +2138,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_data.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]; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index ddfb2bc78..74057a159 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -23,8 +23,6 @@ Vtl_Data::Vtl_Data() { epoch_tow_s = 0; sample_counter = 0; - kf_state = arma::mat(11,1); - kf_P = arma::mat(11,11); } void Vtl_Data::init_storage(int n_sats) @@ -69,88 +67,4 @@ void Vtl_Data::debug_print() // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } -std::vector Vtl_Data::get_position_ecef_m() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[0]; - temp[1] = kf_state[1]; - temp[2] = kf_state[2]; - - return temp; -} - -std::vector Vtl_Data::get_velocity_ecef_m_s() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[3]; - temp[1] = kf_state[4]; - temp[2] = kf_state[5]; - - return temp; -} - -std::vector Vtl_Data::get_accel_ecef_m_s2() -{ - std::vector temp = {42,42,42}; - temp[0] = kf_state[6]; - temp[1] = kf_state[7]; - temp[2] = kf_state[8]; - - return temp; -} -std::vector Vtl_Data::get_position_var_ecef_m() -{ - std::vector 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 Vtl_Data::get_velocity_var_ecef_m_s() -{ - std::vector 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 Vtl_Data::get_accel_var_ecef_m_s2() -{ - std::vector 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; -} diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 182098ba2..b01e49ffe 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -62,16 +62,6 @@ 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 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; }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 08a212083..6cae756df 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -22,25 +22,31 @@ 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.; + kf_x = arma::zeros(n_of_states, 1); } Vtl_Engine::~Vtl_Engine() { } -bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) +bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - static uint64_t refSampleCounter = new_data.sample_counter; + if (refSampleCounter=0) + { + refSampleCounter=new_data.sample_counter; + } double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; // ################## Kalman filter initialization ###################################### //State variables - int n_of_states=11; - static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; - static arma::mat kf_x = arma::zeros(n_of_states, 1); + arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) @@ -58,8 +64,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; // ################## Kalman Tracking ###################################### - static uint32_t counter = 0; //counter - counter = counter + 1; //uint64_t + counter++; //uint64_t //new_data.kf_state.print("new_data kf initial"); uint32_t closure_point=3; @@ -338,4 +343,89 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i); } return -1; +} + +std::vector Vtl_Engine::get_position_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[0]; + temp[1] = kf_x[1]; + temp[2] = kf_x[2]; + + return temp; +} + +std::vector Vtl_Engine::get_velocity_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[3]; + temp[1] = kf_x[4]; + temp[2] = kf_x[5]; + + return temp; +} + +std::vector Vtl_Engine::get_accel_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_x[6]; + temp[1] = kf_x[7]; + temp[2] = kf_x[8]; + + return temp; +} +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); + + 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); + + 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); + + return temp; +} + +double Vtl_Engine::get_latitude() +{ + + return -1.0; +} + +double Vtl_Engine::get_longitude() +{ + + + return -1.0; +} + +double Vtl_Engine::get_height() +{ + return -1.0; +} + +double Vtl_Engine::get_user_clock_offset_s() +{ + double temp=0; + temp = kf_x[9]; + + return temp; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 4cce7371b..dd13d6f4d 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -42,11 +42,21 @@ public: void configure(Vtl_Conf config_); //set config parameters //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) - bool vtl_loop(Vtl_Data& new_data); + bool vtl_loop(Vtl_Data new_data); 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; private: Vtl_Conf config; @@ -67,6 +77,7 @@ private: 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_F; // state transition matrix @@ -84,6 +95,11 @@ private: // Gaussian estimator arma::mat kf_R_est; // measurement error covariance + + uint32_t counter; + 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