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: some major issues fiexed. KF not exploding with clock bypassed

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-08 14:35:33 +00:00
parent 92665492bc
commit 30233b177b
1 changed files with 29 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,21 +35,17 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
// ################## Kalman filter initialization ###################################### // ################## Kalman filter initialization ######################################
// covariances (static) // covariances (static)
kf_P_x = arma::eye(8, 8)*100.0; //TODO: use a real value. kf_P_x = arma::eye(8, 8); //TODO: use a real value.
kf_x = arma::zeros(8, 1); kf_x = arma::zeros(8, 1);
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
double kf_dt=0.1; double kf_dt=0.1;
kf_Q = arma::zeros(8, 8); kf_Q = arma::eye(8, 8);
kf_F = arma::zeros(8, 8); kf_F = arma::eye(8, 8);
kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt; kf_F(0, 3) = kf_dt;
kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt; kf_F(1, 4) = kf_dt;
kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt; kf_F(2, 5) = kf_dt;
kf_F(3, 3) = 1.0; kf_F(6, 7) = kf_dt;
kf_F(4, 4) = 1.0;
kf_F(5, 5) = 1.0;
kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt;
kf_F(7, 7) = 1.0;
kf_H = arma::zeros(2*new_data.sat_number,8); kf_H = arma::zeros(2*new_data.sat_number,8);
kf_y = arma::zeros(2*new_data.sat_number, 1); kf_y = arma::zeros(2*new_data.sat_number, 1);
@ -71,7 +67,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_x(4) = new_data.rx_v(1); kf_x(4) = new_data.rx_v(1);
kf_x(5) = new_data.rx_v(2); 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); kf_x(7) = 1e-8;
} else { } else {
// receiver solution from previous KF step // receiver solution from previous KF step
kf_x(0) = new_data.kf_state[0]; kf_x(0) = new_data.kf_state[0];
@ -81,16 +78,15 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_x(4) = new_data.kf_state[4]; kf_x(4) = new_data.kf_state[4];
kf_x(5) = new_data.kf_state[5]; kf_x(5) = new_data.kf_state[5];
kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0); kf_x(6) = new_data.kf_state[6];//new_data.rx_dts(0);
kf_x(7) = new_data.rx_dts(1); kf_x(7) = 1e-8;
kf_P_x=new_data.kf_P; kf_P_x=new_data.kf_P;
} }
// State error Covariance Matrix Q (PVT) // State error Covariance Matrix Q (PVT)
for (int32_t i = 0; i < 8; i++) // for (int32_t i = 0; i < 8; i++)
{ // {
// It is diagonal 8x8 matrix // // It is diagonal 8x8 matrix
kf_Q(i, i) = 1.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate. // kf_Q(i, i) = 1.0;//new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate.
} // }
// Measurement error Covariance Matrix R assembling // Measurement error Covariance Matrix R assembling
for (int32_t i = 0; i < new_data.sat_number; i++) for (int32_t i = 0; i < new_data.sat_number; i++)
{ {
@ -112,8 +108,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
xDot_u=kf_x(3); xDot_u=kf_x(3);
yDot_u=kf_x(4); yDot_u=kf_x(4);
zDot_u=kf_x(5); zDot_u=kf_x(5);
cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S; cdeltat_u=new_data.rx_dts(0)*SPEED_OF_LIGHT_M_S;
cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S; cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S;
d = arma::zeros(new_data.sat_number, 1); d = arma::zeros(new_data.sat_number, 1);
rho_pri = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1);
@ -139,7 +135,7 @@ 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)+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);
@ -155,8 +151,8 @@ 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)=rho_pri(i)-new_data.pr_m(i);//-0.000157*SPEED_OF_LIGHT_M_S;
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)-rhoDot_pri(i);
} }
@ -177,8 +173,16 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_xerr = kf_K * (kf_yerr); // Error state estimation
kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error)
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
new_data.kf_P=kf_P_x;
new_data.kf_state=kf_x; //updated state estimation new_data.kf_state=kf_x; //updated state estimation
//new_data.kf_state.print("computed kf_state"); //new_data.kf_state.print("computed 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;
new_data.kf_state(6)=new_data.kf_state(6)/SPEED_OF_LIGHT_M_S;
new_data.kf_state(7)=1e-8/SPEED_OF_LIGHT_M_S;
// TODO: compare how KF state diverges from RTKLIB solution! // TODO: compare how KF state diverges from RTKLIB solution!
fstream dump_vtl_file; fstream dump_vtl_file;
@ -190,20 +194,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
else { else {
dump_vtl_file << "kf_xerr"<< ","<<kf_xerr(0)<< ","<<kf_xerr(1)<< ","<<kf_xerr(2)<< ","<<kf_xerr(3)<< ","<<kf_xerr(4)<< ","<<kf_xerr(5)<< ","<<kf_xerr(6)<< ","<<kf_xerr(7)<<endl; dump_vtl_file << "kf_xerr"<< ","<<kf_xerr(0)<< ","<<kf_xerr(1)<< ","<<kf_xerr(2)<< ","<<kf_xerr(3)<< ","<<kf_xerr(4)<< ","<<kf_xerr(5)<< ","<<kf_xerr(6)<< ","<<kf_xerr(7)<<endl;
dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(7)<<endl; dump_vtl_file << "kf_state"<< ","<<new_data.kf_state(0)<< ","<<new_data.kf_state(1)<< ","<<new_data.kf_state(2)<< ","<<new_data.kf_state(3)<< ","<<new_data.kf_state(4)<< ","<<new_data.kf_state(5)<< ","<<new_data.kf_state(6)<< ","<<new_data.kf_state(7)<<endl;
dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p[2]<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl; dump_vtl_file << "rtklib_state"<< ","<<new_data.rx_p(0)<< ","<< new_data.rx_p(1)<<","<< new_data.rx_p[2]<<","<< new_data.rx_v(0)<<","<< new_data.rx_v(1)<<","<< new_data.rx_v(2)<<","<< new_data.rx_dts(0)<<","<< new_data.rx_dts(1)<<endl;
dump_vtl_file << "sat_first_LOS"<< ","<<new_data.sat_LOS(1,0)<< ","<< new_data.sat_LOS(1,1)<<","<< new_data.sat_LOS(1,2)<<endl;
dump_vtl_file.close(); dump_vtl_file.close();
} }
// 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;
kf_x(6)=cdeltat_u/SPEED_OF_LIGHT_M_S;
kf_x(7)=cdeltatDot_u/SPEED_OF_LIGHT_M_S;
//new_data.pr_res.print(" pr RESIDUALS"); //new_data.pr_res.print(" pr RESIDUALS");
//!new_data.kf_state.print(" KF RTKlib STATE"); //!new_data.kf_state.print(" KF RTKlib STATE");
// cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100; // cout << " KF posteriori STATE diference %100" << (kf_x-new_data.kf_state)/new_data.kf_state*100;