mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 14:25:00 +00:00
Code Issues Releases Wiki Activity

FIX: vtl_engine debugged

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-10 16:12:31 +02:00
parent c34782762c
commit bece48e904
3 changed files with 62 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -1097,6 +1097,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.init_storage(n_sats); new_vtl_data.init_storage(n_sats);
new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; 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.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++) for (int n = 0; n < n_sats; n++)
{ {
new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n];
@ -1139,9 +1140,10 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.rx_dts(0)=rx_position_and_time[3]; new_vtl_data.rx_dts(0)=rx_position_and_time[3];
new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; 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? //Call the VTL engine loop: miguel: Should we wait until valid PVT solution?
vtl_engine.vtl_loop(new_vtl_data); vtl_engine.vtl_loop(new_vtl_data);
new_vtl_data.debug_print();
} }
// compute Ground speed and COG // compute Ground speed and COG
double ground_speed_ms = 0.0; double ground_speed_ms = 0.0;

19
src/algorithms/PVT/libs/vtl_data.cc
View File

@ -30,6 +30,7 @@ void Vtl_Data::init_storage(int n_sats)
sat_dts = arma::mat(n_sats, 2); sat_dts = arma::mat(n_sats, 2);
sat_var = arma::vec(n_sats); sat_var = arma::vec(n_sats);
sat_health_flag = arma::vec(n_sats); sat_health_flag = arma::vec(n_sats);
sat_CN0_dB_hz = arma::colvec(n_sats);
int sat_number = n_sats; int sat_number = n_sats;
pr_m = arma::vec(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_v = arma::mat(1, 3);
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);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
} }
@ -48,13 +49,13 @@ void Vtl_Data::init_storage(int n_sats)
void Vtl_Data::debug_print() void Vtl_Data::debug_print()
{ {
std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; 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_p.print("VTL Sat Positions");
sat_v.print("VTL Sat Velocities"); // sat_v.print("VTL Sat Velocities");
sat_dts.print("VTL Sat clocks"); // sat_dts.print("VTL Sat clocks");
sat_var.print("VTL Sat clock variances"); // sat_var.print("VTL Sat clock variances");
sat_health_flag.print("VTL Sat health"); sat_health_flag.print("VTL Sat health");
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]");
} }

90
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -28,14 +28,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
{ {
//TODO: Implement main VTL loop here //TODO: Implement main VTL loop here
using arma::as_scalar; using arma::as_scalar;
using arma::dot; // using arma::dot;
// // ################## Kalman filter initialization ###################################### // ################## Kalman filter initialization ######################################
// covariances (static)
// // covariances (static) kf_P_x = arma::zeros(8, 8); //TODO: use a real value.
kf_P_x_ini = arma::zeros(8, 8); //TODO: use a real value. kf_x = arma::zeros(8, 1);
kf_x = arma::zeros(8, 1); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); double kf_dt=1e-1;
double kf_dt=1e-3;
kf_Q = arma::zeros(8, 8); kf_Q = arma::zeros(8, 8);
kf_F = 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_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 kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix
// // ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
// // receiver solution from rtklib_solver // receiver solution from rtklib_solver
kf_x(0)=new_data.rx_p(0); kf_x(0)=new_data.rx_p(0);
kf_x(1)=new_data.rx_p(1); kf_x(1)=new_data.rx_p(1);
kf_x(2)=new_data.rx_p(2); 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); // //zDot_u=zDot_u0+kf_x_pri(5);
// //cdeltat_u=cdeltat_u0+kf_x_pri(6); // //cdeltat_u=cdeltat_u0+kf_x_pri(6);
// //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7); // //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 // From state variables definition
x_u=kf_x(0); x_u=kf_x(0);
y_u=kf_x(1); 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; 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 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_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_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 // // for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori
// { // // {
// //compute pseudorange posteriori estimation // // //compute pseudorange posteriori estimation
// // rho_est(i)=; // // // rho_est(i)=;
// //compute LOS sat-receiver vector components posteriori // // //compute LOS sat-receiver vector components posteriori
// // a_x(i)=; // // // a_x(i)=;
// // a_y(i)=; // // // a_y(i)=;
// // a_z(i)=; // // // a_z(i)=;
// //compute pseudorange rate posteriori estimation // // //compute pseudorange rate posteriori estimation
// // rhoDot_est(i)=; // // // 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 // // 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) // // // 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, 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; // // 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 // // //Re-calculate error measurement vector with the most recent data available
// //kf_delta_y=kf_H*kf_delta_x // // //kf_delta_y=kf_H*kf_delta_x
// //Filtered pseudorange error measurement (in m): // // //Filtered pseudorange error measurement (in m):
// //delta_rho_filt=; // // //delta_rho_filt=;
// //Filtered Doppler error measurement (in Hz): // // //Filtered Doppler error measurement (in Hz):
// //delta_doppler_filt=; // // //delta_doppler_filt=;
//TODO: Fill the tracking commands outputs // //TODO: Fill the tracking commands outputs
// Notice: keep the same satellite order as in the Vtl_Data matrices // // Notice: keep the same satellite order as in the Vtl_Data matrices
// sample code // // sample code
TrackingCmd trk_cmd; TrackingCmd trk_cmd;
trk_cmd.carrier_freq_hz = 0; trk_cmd.carrier_freq_hz = 0;
trk_cmd.carrier_freq_rate_hz_s = 0; trk_cmd.carrier_freq_rate_hz_s = 0;