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

FIX: lkTable bug

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-15 20:34:21 +01:00
parent 3f348aabd9
commit 2b6f7d3c08
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -367,9 +367,9 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
double densidad=1.0; double densidad=1.0;
double ballistic_coef = 0.007; double ballistic_coef = 0.007;
//vector velocidad //vector velocidad
kf_x.print("kf_x: ");
u_vec = kf_x.rows(3, 5); u_vec = kf_x.rows(3, 5);
// u_vec.print("u_vec"); u_vec.print("u_vec");
// double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2)); // double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2));
//modulo de la velocidad //modulo de la velocidad
@ -377,6 +377,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
if(u>4){ if(u>4){
t_disparo=t_disparo+dt; t_disparo=t_disparo+dt;
cout<<"t_disparo: "<<t_disparo<<endl;
double diam_cohete=120.0e-3;// 120 mm double diam_cohete=120.0e-3;// 120 mm
double mass_rocket=50.0; //50Kg double mass_rocket=50.0; //50Kg
@ -398,7 +399,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir
+gravity_ECEF+Empuje*u_dir; +gravity_ECEF+Empuje*u_dir;
acc_vec.print("acc_vec"); //acc_vec.print("acc_vec");
// % return // % return
acc_x = acc_vec(0); acc_x = acc_vec(0);
acc_y = acc_vec(1); acc_y = acc_vec(1);
@ -594,12 +595,15 @@ double Vtl_Engine::EmpujeLkTable(double t_disparo)
{1.74000000000000, 537.866310625605},}; {1.74000000000000, 537.866310625605},};
//encuentra el mas cercano justo anterior. //encuentra el mas cercano justo anterior.
int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max();
if (index_E<(LkTable.n_rows-1)){ arma::uvec index_E = find(LkTable<=t_disparo, 1, "last");
double tdisparo1=LkTable(index_E,0); index_E.print("indice E: ");
double tdisparo2=LkTable(index_E+1,0); // uint kk = index_E(0);
double E1=LkTable(index_E,1); if (index_E(0)<(LkTable.n_rows-1)){
double E2=LkTable(index_E+1,1); double tdisparo1=LkTable(index_E(0),0);
double tdisparo2=LkTable(index_E(0)+1,0);
double E1=LkTable(index_E(0),1);
double E2=LkTable(index_E(0)+1,1);
E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1; E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1;
}else{ }else{