mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-05 19:42:56 +00:00
Code Issues Releases Wiki Activity

MOD: Working with SPIRENT record, doppler and RAW pseudorange

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-19 19:35:47 +02:00
parent 3285b4d639
commit 26cf7d1950
4 changed files with 35 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1117,11 +1117,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_health_flag(n) = svh.at(n);
new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; new_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)
//new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges
//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)
//cout<<"dtr "<<rx_position_and_time[3]*SPEED_OF_LIGHT_M_S<<"m"; //cout<<"dtr "<<rx_position_and_time[3]*SPEED_OF_LIGHT_M_S<<"m";
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]; //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];
new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; new_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]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
} }
@ -1151,7 +1151,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
//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(); //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;

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

@ -31,6 +31,7 @@ void Vtl_Data::init_storage(int n_sats)
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); sat_CN0_dB_hz = arma::colvec(n_sats);
sat_LOS = arma::mat(n_sats, 3);
int sat_number = n_sats; int sat_number = n_sats;
pr_m = arma::vec(n_sats); pr_m = arma::vec(n_sats);
@ -51,13 +52,14 @@ 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");
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]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
} }

1
src/algorithms/PVT/libs/vtl_data.h
View File

@ -39,6 +39,7 @@ public:
arma::colvec sat_var; // sat position and clock error variance [m^2] 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_health_flag; // sat health flag (0 is ok)
arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz
arma::mat sat_LOS; // sat LOS
int sat_number; // on-view sat number int sat_number; // on-view sat number
arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec pr_m; // Satellite Code pseudoranges [m]

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

@ -76,12 +76,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// // Kalman state prediction (time update) // // Kalman state prediction (time update)
cout << " KF RTKlib STATE" << kf_x; cout << " KF RTKlib STATE" << kf_x;
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
// cout << " KF priori STATE diference" << kf_x-new_data.kf_state; // cout << " KF priori STATE diference" << kf_x-new_data.kf_state;
//from error state variables to variables //from error state variables to variables
kf_xerr=kf_x-new_data.kf_state; kf_xerr=kf_x-new_data.kf_state;
// From state variables definition // From state variables definition
// TODO: cast to type properly
x_u=kf_x(0); x_u=kf_x(0);
y_u=kf_x(1); y_u=kf_x(1);
z_u=kf_x(2); z_u=kf_x(2);
@ -99,16 +100,27 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
a_z = arma::zeros(new_data.sat_number, 1); a_z = arma::zeros(new_data.sat_number, 1);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{ {
d(i)=sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u)); //d(i) is the distance sat(i) to receiver
d(i)=(new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u);
d(i)=d(i)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u);
d(i)=d(i)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u);
d(i)=sqrt(d(i));
//compute pseudorange estimation //compute pseudorange estimation
rho_pri(i)=d(i)+cdeltat_u; rho_pri(i)=d(i)+cdeltat_u;
//compute LOS sat-receiver vector components //compute LOS sat-receiver vector components
a_x(i)=-(new_data.sat_p(i, 0)-x_u)/d(i); a_x(i)=-(new_data.sat_p(i, 0)-x_u)/d(i);
a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i); a_y(i)=-(new_data.sat_p(i, 1)-y_u)/d(i);
a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i); a_z(i)=-(new_data.sat_p(i, 2)-z_u)/d(i);
new_data.sat_LOS(i,0)=a_x(i);
new_data.sat_LOS(i,1)=a_y(i);
new_data.sat_LOS(i,2)=a_z(i);
//compute pseudorange rate estimation //compute pseudorange rate estimation
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); 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);
//rhoDot_pri(i)=(new_data.sat_v(i, 0)-0)*a_x(i)+(new_data.sat_v(i, 1)-0)*a_y(i)+(new_data.sat_v(i, 2)-0)*a_z(i);
} }
cout << " V_LOS sat" << rhoDot_pri;
kf_H = arma::zeros(2*new_data.sat_number,8); kf_H = arma::zeros(2*new_data.sat_number,8);
@ -124,13 +136,18 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//kf_y(i) = delta_rho(i); // i-Satellite //kf_y(i) = delta_rho(i); // i-Satellite
//kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite //kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite
kf_y(i)=new_data.pr_m(i); kf_y(i)=new_data.pr_m(i);
kf_yerr(i)=kf_y(i)-rho_pri(i)-0.000157*SPEED_OF_LIGHT_M_S; kf_yerr(i)=kf_y(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S;
float Lambda_GPS_L1=0.1902937; float Lambda_GPS_L1=0.1902937;
kf_y(i+new_data.sat_number)=-new_data.doppler_hz(i)*Lambda_GPS_L1; kf_y(i+new_data.sat_number)=(rhoDot_pri(i))/Lambda_GPS_L1;
kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-rhoDot_pri(i); kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-new_data.doppler_hz(i);
//rhoDot_pri(i)=(rhoDot_pri(i))/Lambda_GPS_L1;
} }
cout << " KF measurement vector difference" << kf_yerr; //cout << " KF measurement vector difference" << kf_yerr;
cout << " kf_yerr" << kf_yerr;
//rhoDot_pri.print("DOPPLER stimated [Hz]");
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling
{ {
// 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)
@ -146,7 +163,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation kf_x = kf_x + kf_K * (kf_yerr); // 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
//cout << " KF posteriori STATE" << kf_x; //cout << " KF posteriori STATE" << kf_x;
cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state; //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################
@ -189,6 +206,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
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;
} }