mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +00:00
Code Issues Releases Wiki Activity

FIX: bug in vtlEngine

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-16 00:26:14 +01:00
parent 2b6f7d3c08
commit 028e71dc2f
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -91,7 +91,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
double acc_x = 0; double acc_x = 0;
double acc_y = 0; double acc_y = 0;
double acc_z = 0; double acc_z = 0;
test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt); test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt);
kf_x(6) = acc_x; kf_x(6) = acc_x;
kf_x(7) = acc_y; kf_x(7) = acc_y;
kf_x(8) = acc_z; kf_x(8) = acc_z;
@ -369,8 +369,6 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
//vector velocidad //vector velocidad
u_vec = kf_x.rows(3, 5); u_vec = kf_x.rows(3, 5);
u_vec.print("u_vec");
// 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
double u = norm(u_vec, 2); double u = norm(u_vec, 2);
@ -382,10 +380,11 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
double mass_rocket=50.0; //50Kg double mass_rocket=50.0; //50Kg
if(t_disparo<.2){ if(t_disparo<.2){
u_dir={.90828, -.13984, .388756}; u_dir={.90828, -.13984, -.388756};
}else{ }else{
u_dir = u_vec / u; u_dir = u_vec / u;
} }
// u_dir.print("u_dir");
// lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]);
// [T, sound_v, P, densidad] = atmosisa(lla(3)); // [T, sound_v, P, densidad] = atmosisa(lla(3));
// sound_v=320;% @ 5km and -17.5C // sound_v=320;% @ 5km and -17.5C