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

MOD: reference use of Vtl_data

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-27 00:25:42 +01:00
parent f716b04002
commit f14e1bfa28
4 changed files with 83 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1102,65 +1102,66 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
/* satellite positions, velocities and clocks */ /* satellite positions, velocities and clocks */
satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data());
Vtl_Data new_vtl_data; //Vtl_Data vtl_data;
new_vtl_data.init_storage(n_sats); vtl_data.init_storage(n_sats);
new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; 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 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; 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]; vtl_data.sat_p(n, 0) = rs[0 + 6 * n];
new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; vtl_data.sat_p(n, 1) = rs[1 + 6 * n];
new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; vtl_data.sat_p(n, 2) = rs[2 + 6 * n];
new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; vtl_data.sat_v(n, 0) = rs[3 + 6 * n];
new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; vtl_data.sat_v(n, 1) = rs[4 + 6 * n];
new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; vtl_data.sat_v(n, 2) = rs[5 + 6 * n];
new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; vtl_data.sat_dts(n, 0) = dts[0 + 2 * n];
new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; vtl_data.sat_dts(n, 1) = dts[1 + 2 * n];
new_vtl_data.sat_var(n) = var[n]; vtl_data.sat_var(n) = var[n];
new_vtl_data.sat_health_flag(n) = svh.at(n); vtl_data.sat_health_flag(n) = svh.at(n);
new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0];
// TODO: first version of VTL works only with ONE frequency band (band #0 is L1) // TODO: first version of VTL works only with ONE frequency band (band #0 is L1)
//To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
//corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts)
new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2]; vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]+SPEED_OF_LIGHT_M_S * dts[n * 2];
new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0];
new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
new_vtl_data.pr_res(n) = pr_residual_vec[n]; vtl_data.pr_res(n) = pr_residual_vec[n];
} }
//VTL input data extraction from rtklib structures //VTL input data extraction from rtklib structures
/* Receiver position, velocity and clock */ /* Receiver position, velocity and clock */
/* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */
new_vtl_data.rx_p(0) = pvt_sol.rr[0]; vtl_data.rx_p(0) = pvt_sol.rr[0];
new_vtl_data.rx_p(1) = pvt_sol.rr[1]; vtl_data.rx_p(1) = pvt_sol.rr[1];
new_vtl_data.rx_p(2) = pvt_sol.rr[2]; vtl_data.rx_p(2) = pvt_sol.rr[2];
new_vtl_data.rx_v(0) = pvt_sol.rr[3]; vtl_data.rx_v(0) = pvt_sol.rr[3];
new_vtl_data.rx_v(1) = pvt_sol.rr[4]; vtl_data.rx_v(1) = pvt_sol.rr[4];
new_vtl_data.rx_v(2) = pvt_sol.rr[5]; vtl_data.rx_v(2) = pvt_sol.rr[5];
/* Receiver position, velocity and clock variances*/ /* Receiver position, velocity and clock variances*/
new_vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; vtl_data.rx_pvt_var[0] = pvt_sol.qr[0];
new_vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; vtl_data.rx_pvt_var[1] = pvt_sol.qr[1];
new_vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; vtl_data.rx_pvt_var[2] = pvt_sol.qr[2];
//TODO: get direct estimations for V T variances, instead: //TODO: get direct estimations for V T variances, instead:
new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position.
new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1;
new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1;
new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time
new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler
//receiver clock offset and receiver clock drift //receiver clock offset and receiver clock drift
new_vtl_data.rx_dts(0) = rx_position_and_time[3]; vtl_data.rx_dts(0) = rx_position_and_time[3];
new_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]
//new_vtl_data.debug_print(); //new_vtl_data.debug_print();
double PVT[6]={0}; //vtl_data.kf_state.print("kf_state_input");
vtl_engine.vtl_loop(new_vtl_data, PVT); vtl_engine.vtl_loop(vtl_data);
// pvt_sol.rr[0]=PVT[0]; //vtl_data.kf_state.print("kf_state_output");
// pvt_sol.rr[1]=PVT[1]; // pvt_sol.rr[0] = vtl_data.kf_state[0];
// pvt_sol.rr[2]=PVT[2]; // pvt_sol.rr[1] = vtl_data.kf_state[1];
// pvt_sol.rr[3]=PVT[3]; // pvt_sol.rr[2] = vtl_data.kf_state[2];
// pvt_sol.rr[4]=PVT[4]; // pvt_sol.rr[3] = vtl_data.kf_state[3];
// pvt_sol.rr[5]=PVT[5]; // pvt_sol.rr[4] = vtl_data.kf_state[4];
// pvt_sol.rr[5] = vtl_data.kf_state[5];
} }
else{ else{
//MAGL: the code should not enter here once the vtl has started //MAGL: the code should not enter here once the vtl has started

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

@ -21,6 +21,8 @@ Vtl_Data::Vtl_Data()
{ {
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
kf_state = arma::mat(8,1);
kf_P = arma::mat(8,8);
} }
void Vtl_Data::init_storage(int n_sats) void Vtl_Data::init_storage(int n_sats)
@ -44,8 +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);
kf_state = arma::mat(8,1);
kf_P = arma::mat(8,8);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
} }
@ -59,7 +60,7 @@ void Vtl_Data::debug_print()
// 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");
//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]");

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

@ -28,7 +28,7 @@ Vtl_Engine::~Vtl_Engine()
{ {
} }
bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) 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;
@ -59,31 +59,31 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
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 ######################################
static uint32_t counter=0; //counter static uint32_t counter=0; //counter
static double prueba[6]={0}; //counter counter = counter+1; //uint64_t
counter=counter+1; //uint64_t
cout << "counter" << counter<<endl; cout << "counter" << counter<<endl;
//new_data.kf_state.print("new_data kf initial");
if(counter<3000){ // if(counter<3000){ //
// 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);
kf_x(3)=new_data.rx_v(0); kf_x(3) = new_data.rx_v(0);
kf_x(4)=new_data.rx_v(1); kf_x(4) = new_data.rx_v(1);
kf_x(5)=new_data.rx_v(2); kf_x(5) = new_data.rx_v(2);
kf_x(6)=new_data.rx_dts(0); kf_x(6) = new_data.rx_dts(0);
kf_x(7)=new_data.rx_dts(1); kf_x(7) = new_data.rx_dts(1);
} } else {
else{ // receiver solution from previous KF step
kf_x(0)=prueba[0]; kf_x(0) = new_data.kf_state[0];
kf_x(1)=prueba[1]; kf_x(1) = new_data.kf_state[1];
kf_x(2)=prueba[2]; kf_x(2) = new_data.kf_state[2];
kf_x(3)=prueba[3]; kf_x(3) = new_data.kf_state[3];
kf_x(4)=prueba[4]; kf_x(4) = new_data.kf_state[4];
kf_x(5)=prueba[5]; kf_x(5) = new_data.kf_state[5];
kf_x(6)=new_data.rx_dts(0); kf_x(6) = new_data.rx_dts(0);
kf_x(7)=new_data.rx_dts(1); kf_x(7) = new_data.rx_dts(1);
kf_P_x=new_data.kf_P; kf_P_x=new_data.kf_P;
} }
// State error Covariance Matrix Q (PVT) // State error Covariance Matrix Q (PVT)
@ -93,7 +93,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_Q(i, i) = 50.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. kf_Q(i, i) = 50.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate.
} }
// Measurement error Covariance Matrix R assembling // Measurement error Covariance Matrix R assembling
for (int32_t i = 0; i < new_data.sat_number; i++) 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) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
kf_R(i, i) = 5.0; //TODO: fill with real values. kf_R(i, i) = 5.0; //TODO: fill with real values.
@ -101,12 +101,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
} }
// Kalman state prediction (time update) // Kalman state prediction (time update)
kf_x.print(" KF RTKlib STATE");
// cout<<"state_fed"<<endl;
// for(int i=0;i<6;i++){
// cout<<prueba[i]<<endl;
// }
new_data.kf_state=kf_x; new_data.kf_state=kf_x;
kf_x = kf_F * kf_x; // state prediction kf_x = kf_F * kf_x; // state prediction
kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction kf_P_x= kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
@ -166,7 +160,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i); kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i);
} }
kf_yerr.print("KF innovation");
// DOUBLES DIFFERENCES // DOUBLES DIFFERENCES
// kf_yerr = arma::zeros(2*new_data.sat_number, 1); // kf_yerr = arma::zeros(2*new_data.sat_number, 1);
@ -183,37 +176,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S) kf_S = kf_H * kf_P_x* kf_H.t() + kf_R; // innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain
kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_xerr = kf_K * (kf_yerr); // Error state estimation
//kf_xerr.print(" kf_xerr"); //kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error)
kf_x = kf_x + kf_xerr; // updated state estimation (a priori + error)
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.print(" KF STATE"); new_data.kf_state=kf_x; //updated state estimation
//new_data.kf_state.print("computed kf_state");
// TODO: compare how KF state diverges from RTKLIB solution! // TODO: compare how KF state diverges from RTKLIB solution!
fstream dump_vtl_file; fstream dump_vtl_file;
dump_vtl_file.open("dump_vtl_file.csv", ios::out|ios::app); dump_vtl_file.open("dump_vtl_file.csv", ios::out|ios::app);
dump_vtl_file.precision(15);
if (!dump_vtl_file) { if (!dump_vtl_file) {
cout << "File not created!"; cout << "File not created!";
} }
else { else {
dump_vtl_file << "kf_state"<< ","<<kf_x(0)<< ","<<kf_x(1)<< ","<<kf_x(2)<< ","<<kf_x(3)<< ","<<kf_x(4)<< ","<<kf_x(5)<< ","<<kf_x(6)<< ","<<kf_x(7)<<endl; dump_vtl_file << "kf_xerr"<< ","<<kf_xerr(0)<< ","<<kf_xerr(1)<< ","<<kf_xerr(2)<< ","<<kf_xerr(3)<< ","<<kf_xerr(4)<< ","<<kf_xerr(5)<< ","<<kf_xerr(6)<< ","<<kf_xerr(7)<<endl;
dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p(2)<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl; dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(7)<<endl;
dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p[2]<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl;
dump_vtl_file.close(); dump_vtl_file.close();
} }
PVT_out[0]=kf_x(0);
PVT_out[1]=kf_x(1);
PVT_out[2]=kf_x(2);
PVT_out[3]=kf_x(3);
PVT_out[4]=kf_x(4);
PVT_out[5]=kf_x(5);
// States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
prueba[0]=kf_x(0);
prueba[1]=kf_x(1);
prueba[2]=kf_x(2);
prueba[3]=kf_x(3);
prueba[4]=kf_x(4);
prueba[5]=kf_x(5);
// kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
// kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
@ -222,7 +206,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
//new_data.pr_res.print(" pr RESIDUALS"); //new_data.pr_res.print(" pr RESIDUALS");
//!new_data.kf_state.print(" KF RTKlib STATE"); //!new_data.kf_state.print(" KF RTKlib STATE");
cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100; // cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################
@ -286,7 +270,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6])
trk_cmd.enable_code_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true;
trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.sample_counter = new_data.sample_counter;
trk_cmd_outs.push_back(trk_cmd); trk_cmd_outs.push_back(trk_cmd);
new_data.debug_print();
return true; return true;
} }

2
src/algorithms/PVT/libs/vtl_engine.h
View File

@ -42,7 +42,7 @@ public:
void configure(Vtl_Conf config_); //set config parameters void configure(Vtl_Conf config_); //set config parameters
//TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...)
bool vtl_loop(Vtl_Data new_data, double PVT_out[6]); bool vtl_loop(Vtl_Data& new_data);
void reset(); // reset all internal states void reset(); // reset all internal states
void debug_print(); // print debug information void debug_print(); // print debug information