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: VTL engine update

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-13 23:25:01 +00:00
parent b699173208
commit 3285b4d639
2 changed files with 14 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1120,7 +1120,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
//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)
new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]; //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.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];
} }

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

@ -101,13 +101,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
{ {
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)=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));
//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);
//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)+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);
} }
kf_H = arma::zeros(2*new_data.sat_number,8); kf_H = arma::zeros(2*new_data.sat_number,8);
@ -122,17 +122,19 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
{ {
//kf_y(i) = delta_rho(i); // i-Satellite //kf_y(i) = delta_rho(i); // i-Satellite
kf_y(i)=new_data.pr_m(i);
kf_yerr(i)=kf_y(i)-rho_pri(i);
//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.sat_number)=-new_data.doppler_hz(i)/0.1902937+cdeltatDot_u; kf_y(i)=new_data.pr_m(i);
kf_yerr(i)=kf_y(i)-rho_pri(i)-0.000157*SPEED_OF_LIGHT_M_S;
float Lambda_GPS_L1=0.1902937;
kf_y(i+new_data.sat_number)=-new_data.doppler_hz(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)-rhoDot_pri(i);
} }
//cout << " KF yerr" << kf_yerr; cout << " KF measurement vector difference" << kf_yerr;
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)
kf_R(i, i) = 1.0; //TODO: use a valid value. kf_R(i, i) = 0.1; //TODO: use a valid value.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0;
} }
@ -140,22 +142,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
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
//for (int32_t i = 0; i < new_data.sat_number; i++) //Error measurement vector
//{
// kf_delta_y(i)=new_data.pr_m(i)+delta_rho(i)-rho_pri(i); // pseudorange error
// kf_delta_y(i+new_data.sat_number)=new_data.doppler_hz(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error
//}
//kf_delta_x = kf_K * kf_delta_y; // updated error state estimation
//cout << " KF measures ERROR" << kf_yerr; //cout << " KF measures ERROR" << kf_yerr;
//cout << " KF measure ERROR stimation" << kf_H*kf_x; kf_x = kf_x + kf_K * (kf_yerr); // updated state estimation
cout << " KF innovation" << kf_yerr-kf_H*kf_xerr;
kf_x = kf_x + kf_K * (kf_yerr-kf_H*kf_xerr); // 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;
// kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################