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

FIX: pseudorange rate error measurements

Browse Source
This commit is contained in:
M.A.Gomez 2022-11-12 13:00:01 +00:00
parent 86ad760015
commit 122c53c185
3 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
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];
} }
@ -1149,6 +1149,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
//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?
new_vtl_data.debug_print();
vtl_engine.vtl_loop(new_vtl_data); vtl_engine.vtl_loop(new_vtl_data);
//new_vtl_data.debug_print(); //new_vtl_data.debug_print();

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

@ -52,11 +52,11 @@ 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"); //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]");

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

@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
} }
// Kalman state prediction (time update) // Kalman state prediction (time update)
kf_x.print(" KF RTKlib STATE"); //kf_x.print(" KF RTKlib STATE");
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
@ -114,11 +114,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
new_data.sat_LOS(i,1)=a_y(i); new_data.sat_LOS(i,1)=a_y(i);
new_data.sat_LOS(i,2)=a_z(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)+cdeltatDot_u;
//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);
} }
rhoDot_pri.print("V_LOS sat");
kf_H = arma::zeros(2*new_data.sat_number,8); kf_H = arma::zeros(2*new_data.sat_number,8);
@ -134,7 +131,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//kf_y(i) = new_data.pr_m(i); // i-Satellite //kf_y(i) = new_data.pr_m(i); // i-Satellite
//kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite //kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite
kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S; kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S;
kf_yerr(i+new_data.sat_number)=rhoDot_pri(i)/Lambda_GPS_L1-new_data.doppler_hz(i); //kf_yerr(i+new_data.sat_number)=(rhoDot_pri(i)-cdeltatDot_u)/Lambda_GPS_L1-new_data.doppler_hz(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 measurement vector difference"); kf_yerr.print("KF measurement vector difference");
@ -162,7 +161,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 diference" << kf_x-new_data.kf_state; //cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################
@ -205,7 +204,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(); //new_data.debug_print();
return true; return true;
} }