mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-05-05 17:04:11 +00:00
Code Issues Releases Wiki Activity

fix: USER clock adjust from m/s to s

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

10
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
@ -131,7 +131,6 @@ 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)-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(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i);
} }
@ -161,7 +160,12 @@ 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; // States related tu USER clock adjust from m/s to s (by /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;
cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
// // ################## Geometric Transformation ###################################### // // ################## Geometric Transformation ######################################