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

MOD: model3DoF

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-15 19:18:49 +01:00
parent 3cb61f6f2d
commit 2115bc0aab
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -331,13 +331,13 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
return -1; return -1;
} }
bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt) bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt)
{ {
arma::colvec u_vec; arma::colvec u_vec;
arma::colvec acc_vec; arma::colvec acc_vec;
arma::colvec u_dir; arma::colvec u_dir;
arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333 arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333
static double t_disparo; static double t_disparo=0;
double Empuje; double Empuje;
double densidad=1.0; double densidad=1.0;
double ballistic_coef = 0.007; double ballistic_coef = 0.007;
@ -348,6 +348,10 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
//modulo de la velocidad //modulo de la velocidad
double u = norm(u_vec, 2); double u = norm(u_vec, 2);
// if(t<1000){
// t_disparo=0;
// }
if(u>4){ if(u>4){
t_disparo=t_disparo+dt; t_disparo=t_disparo+dt;
double diam_cohete=120.0e-3;// 120 mm double diam_cohete=120.0e-3;// 120 mm
@ -365,7 +369,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
// CD0 = Cd0_M_LookTable(Mach); // CD0 = Cd0_M_LookTable(Mach);
// % ballistic_coef is Cd0/mass_rocket; // % ballistic_coef is Cd0/mass_rocket;
// ballistic_coef=CD0/mass_rocket; // ballistic_coef=CD0/mass_rocket;
// Empuje = EmpujeLookTable(t_disparo); Empuje = EmpujeLkTable(t_disparo);
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;
@ -375,6 +379,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
acc_y = acc_vec(1); acc_y = acc_vec(1);
acc_z = acc_vec(2); acc_z = acc_vec(2);
}else{ }else{
t_disparo=0;
// % return // % return
acc_x = 0; acc_x = 0;
acc_y = 0; acc_y = 0;

2
src/algorithms/PVT/libs/vtl_engine.h
View File

@ -88,7 +88,7 @@ private:
bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor
bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation
bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt); bool model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat kf_x,double dt);
double EmpujeLkTable(double t_disparo); double EmpujeLkTable(double t_disparo);
}; };