From bece48e90479a643976826e70f392032e459aa94 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Mon, 10 Oct 2022 16:12:31 +0200 Subject: [PATCH] FIX: vtl_engine debugged --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- src/algorithms/PVT/libs/vtl_data.cc | 19 ++--- src/algorithms/PVT/libs/vtl_engine.cc | 90 +++++++++++++----------- 3 files changed, 62 insertions(+), 51 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1d878f0fc..816851ca3 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1097,6 +1097,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.init_storage(n_sats); new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + new_vtl_data.sat_number=n_sats; for (int n = 0; n < n_sats; n++) { new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; @@ -1139,9 +1140,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(0)=rx_position_and_time[3]; new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; - new_vtl_data.debug_print(); //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); + + new_vtl_data.debug_print(); } // compute Ground speed and COG double ground_speed_ms = 0.0; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 9cd1fef4d..c2a8f6bff 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -30,6 +30,7 @@ void Vtl_Data::init_storage(int n_sats) sat_dts = arma::mat(n_sats, 2); sat_var = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats); + sat_CN0_dB_hz = arma::colvec(n_sats); int sat_number = n_sats; pr_m = arma::vec(n_sats); @@ -40,7 +41,7 @@ void Vtl_Data::init_storage(int n_sats) rx_v = arma::mat(1, 3); rx_dts = arma::mat(1, 2); rx_var = arma::vec(1); - + rx_pvt_var = arma::vec(8); epoch_tow_s = 0; sample_counter = 0; } @@ -48,13 +49,13 @@ void Vtl_Data::init_storage(int n_sats) void Vtl_Data::debug_print() { std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; - sat_p.print("VTL Sat Positions"); - sat_v.print("VTL Sat Velocities"); - sat_dts.print("VTL Sat clocks"); - sat_var.print("VTL Sat clock variances"); + // sat_p.print("VTL Sat Positions"); + // sat_v.print("VTL Sat Velocities"); + // sat_dts.print("VTL Sat clocks"); + // sat_var.print("VTL Sat clock variances"); sat_health_flag.print("VTL Sat health"); - - pr_m.print("Satellite Code pseudoranges [m]"); - doppler_hz.print("satellite Carrier Dopplers [Hz]"); - carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); + + // 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_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 10fb0f831..f2da2ba86 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -28,14 +28,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here using arma::as_scalar; - using arma::dot; -// // ################## Kalman filter initialization ###################################### - -// // covariances (static) - kf_P_x_ini = arma::zeros(8, 8); //TODO: use a real value. - kf_x = arma::zeros(8, 1); - kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); - double kf_dt=1e-3; + // using arma::dot; + // ################## Kalman filter initialization ###################################### + // covariances (static) + kf_P_x = arma::zeros(8, 8); //TODO: use a real value. + kf_x = arma::zeros(8, 1); + kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); + double kf_dt=1e-1; kf_Q = arma::zeros(8, 8); kf_F = arma::zeros(8, 8); @@ -53,8 +52,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) kf_y = arma::zeros(2*new_data.sat_number, 1); kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix -// // ################## Kalman Tracking ###################################### -// // receiver solution from rtklib_solver + // ################## Kalman Tracking ###################################### + // receiver solution from rtklib_solver kf_x(0)=new_data.rx_p(0); kf_x(1)=new_data.rx_p(1); kf_x(2)=new_data.rx_p(2); @@ -83,6 +82,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) // //zDot_u=zDot_u0+kf_x_pri(5); // //cdeltat_u=cdeltat_u0+kf_x_pri(6); // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); +// // //from error state variables to variables +// // //x_u=x_u0+kf_x_pri(0); +// // //y_u=y_u0+kf_x_pri(1); +// // //z_u=z_u0+kf_x_pri(2); +// // //xDot_u=xDot_u0+kf_x_pri(3); +// // //yDot_u=yDot_u0+kf_x_pri(4); +// // //zDot_u=zDot_u0+kf_x_pri(5); +// // //cdeltat_u=cdeltat_u0+kf_x_pri(6); +// // //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); // From state variables definition x_u=kf_x(0); y_u=kf_x(1); @@ -113,7 +121,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) rhoDot_pri(i)=(new_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u; } - kf_H = arma::zeros(8, 2*new_data.sat_number); + kf_H = arma::zeros(2*new_data.sat_number,8); for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling { @@ -149,45 +157,45 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) //} //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation - kf_x = kf_x + kf_K * (kf_y-dot(kf_H,kf_x)); // updated state estimation + kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated 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_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) +// // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) -// // ################## Geometric Transformation ###################################### +// // // ################## Geometric Transformation ###################################### -// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori -// { -// //compute pseudorange posteriori estimation -// // rho_est(i)=; -// //compute LOS sat-receiver vector components posteriori -// // a_x(i)=; -// // a_y(i)=; -// // a_z(i)=; -// //compute pseudorange rate posteriori estimation -// // rhoDot_est(i)=; -// } +// // for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori +// // { +// // //compute pseudorange posteriori estimation +// // // rho_est(i)=; +// // //compute LOS sat-receiver vector components posteriori +// // // a_x(i)=; +// // // a_y(i)=; +// // // a_z(i)=; +// // //compute pseudorange rate posteriori estimation +// // // rhoDot_est(i)=; +// // } -// kf_H = arma::zeros(8, 2*new_data.sat_number); +// // kf_H = arma::zeros(8, 2*new_data.sat_number); -// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling -// { -// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) -// kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; -// kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; -// } +// // for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling +// // { +// // // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error) +// // kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 1.0; +// // kf_H(i+new_data.sat_number, 3) = a_x(i); kf_H(i+new_data.sat_number, 4) = a_y(i); kf_H(i+new_data.sat_number, 5) = a_z(i); kf_H(i+new_data.sat_number, 7) = 1.0; +// // } -// //Re-calculate error measurement vector with the most recent data available -// //kf_delta_y=kf_H*kf_delta_x -// //Filtered pseudorange error measurement (in m): -// //delta_rho_filt=; -// //Filtered Doppler error measurement (in Hz): -// //delta_doppler_filt=; +// // //Re-calculate error measurement vector with the most recent data available +// // //kf_delta_y=kf_H*kf_delta_x +// // //Filtered pseudorange error measurement (in m): +// // //delta_rho_filt=; +// // //Filtered Doppler error measurement (in Hz): +// // //delta_doppler_filt=; - //TODO: Fill the tracking commands outputs - // Notice: keep the same satellite order as in the Vtl_Data matrices - // sample code +// //TODO: Fill the tracking commands outputs +// // Notice: keep the same satellite order as in the Vtl_Data matrices +// // sample code TrackingCmd trk_cmd; trk_cmd.carrier_freq_hz = 0; trk_cmd.carrier_freq_rate_hz_s = 0;