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: 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_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)
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);
//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";
//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.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]
//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);
//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";
// 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_var.print("VTL Sat clock variances");
// sat_health_flag.print("VTL Sat health");
sat_LOS.print("VTL SAT LOS");
//sat_LOS.print("VTL SAT LOS");
// kf_state.print("EKF STATE");
pr_m.print("Satellite Code pseudoranges [m]");

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

@ -64,7 +64,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(3)=new_data.rx_v(0);
kf_x(4)=new_data.rx_v(1);
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);
for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT)
@ -74,7 +74,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
}
// Kalman state prediction (time update)
kf_x.print(" KF RTKlib STATE");
//kf_x.print(" KF RTKlib STATE");
new_data.kf_state=kf_x;
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
@ -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,2)=a_z(i);
//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)-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(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.print("V_LOS sat");
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.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.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");
@ -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_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 ######################################
@ -205,7 +204,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
trk_cmd.enable_code_nco_cmd = true;
trk_cmd.sample_counter = new_data.sample_counter;
trk_cmd_outs.push_back(trk_cmd);
new_data.debug_print();
//new_data.debug_print();
return true;
}