mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 22:34:58 +00:00
ADD: empuje lkTable
This commit is contained in:
M.A. Gomez
parent
e5e30afd42
commit
3cb61f6f2d
@ -341,7 +341,6 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
|
|||||||
double Empuje;
|
double Empuje;
|
||||||
double densidad=1.0;
|
double densidad=1.0;
|
||||||
double ballistic_coef = 0.007;
|
double ballistic_coef = 0.007;
|
||||||
double PI = acos(-1.0);
|
|
||||||
//vector velocidad
|
//vector velocidad
|
||||||
u_vec = kf_x.rows(4, 6);
|
u_vec = kf_x.rows(4, 6);
|
||||||
// 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));
|
||||||
@ -368,7 +367,7 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
|
|||||||
// ballistic_coef=CD0/mass_rocket;
|
// ballistic_coef=CD0/mass_rocket;
|
||||||
// Empuje = EmpujeLookTable(t_disparo);
|
// Empuje = EmpujeLookTable(t_disparo);
|
||||||
|
|
||||||
acc_vec = -(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;
|
||||||
|
|
||||||
// % return
|
// % return
|
||||||
@ -386,6 +385,196 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x
|
|||||||
}
|
}
|
||||||
|
|
||||||
double Vtl_Engine::EmpujeLkTable(double t_disparo)
|
double Vtl_Engine::EmpujeLkTable(double t_disparo)
|
||||||
{
|
{
|
||||||
return -1.0;
|
double E;
|
||||||
|
arma::mat LkTable={
|
||||||
|
{0.0, 391.083112445424},
|
||||||
|
{0.0100578034682081, 385.626317230813},
|
||||||
|
{0.0201156069364162, 379.253652903964},
|
||||||
|
{0.0301734104046243, 372.850418310078},
|
||||||
|
{0.0402312138728324, 366.435105395212},
|
||||||
|
{0.0502890173410405, 359.948724887310},
|
||||||
|
{0.0603468208092486, 353.452370826679},
|
||||||
|
{0.0704046242774566, 346.915160536406},
|
||||||
|
{0.0804624277456647, 340.353374212744},
|
||||||
|
{0.0905202312138728, 339.982366920698},
|
||||||
|
{0.100578034682081, 339.649644036322},
|
||||||
|
{0.110635838150289, 339.313119301332},
|
||||||
|
{0.120693641618497, 338.971249841340},
|
||||||
|
{0.130751445086705, 338.626092336370},
|
||||||
|
{0.140809248554913, 338.277926096280},
|
||||||
|
{0.150867052023121, 337.923860794114},
|
||||||
|
{0.160924855491329, 337.564686821652},
|
||||||
|
{0.170982658959538, 337.204565166310},
|
||||||
|
{0.181040462427746, 336.836327593982},
|
||||||
|
{0.191098265895954, 337.612574596978},
|
||||||
|
{0.201156069364162, 338.389812277202},
|
||||||
|
{0.211213872832370, 339.172197383571},
|
||||||
|
{0.221271676300578, 339.950864133333},
|
||||||
|
{0.231329479768786, 340.734286946588},
|
||||||
|
{0.241387283236994, 341.513599696996},
|
||||||
|
{0.251445086705202, 342.296487929689},
|
||||||
|
{0.261502890173410, 343.077730186615},
|
||||||
|
{0.271560693641619, 343.862401553296},
|
||||||
|
{0.281618497109827, 344.645045182721},
|
||||||
|
{0.291676300578035, 345.430222609429},
|
||||||
|
{0.301734104046243, 346.216363751051},
|
||||||
|
{0.311791907514451, 347.003086022300},
|
||||||
|
{0.321849710982659, 347.790403464500},
|
||||||
|
{0.331907514450867, 348.576632890101},
|
||||||
|
{0.341965317919075, 349.365041767204},
|
||||||
|
{0.352023121387283, 350.154345504950},
|
||||||
|
{0.362080924855491, 350.943780688588},
|
||||||
|
{0.372138728323699, 351.735714016931},
|
||||||
|
{0.382196531791908, 352.523676077225},
|
||||||
|
{0.392254335260116, 353.317052045327},
|
||||||
|
{0.402312138728324, 354.110411623268},
|
||||||
|
{0.412369942196532, 354.903279067692},
|
||||||
|
{0.422427745664740, 355.692559158889},
|
||||||
|
{0.432485549132948, 356.490274978154},
|
||||||
|
{0.442543352601156, 357.283451900588},
|
||||||
|
{0.452601156069364, 358.078507291348},
|
||||||
|
{0.462658959537572, 358.871323078342},
|
||||||
|
{0.472716763005780, 359.668187019647},
|
||||||
|
{0.482774566473988, 360.465893772213},
|
||||||
|
{0.492832369942197, 361.264486481857},
|
||||||
|
{0.502890173410405, 362.057713788838},
|
||||||
|
{0.512947976878613, 362.855398652135},
|
||||||
|
{0.523005780346821, 363.651958513907},
|
||||||
|
{0.533063583815029, 364.453139119421},
|
||||||
|
{0.543121387283237, 365.248494477390},
|
||||||
|
{0.553179190751445, 366.047440571574},
|
||||||
|
{0.563236994219653, 366.844541693072},
|
||||||
|
{0.573294797687861, 367.645722991578},
|
||||||
|
{0.583352601156069, 368.431281642859},
|
||||||
|
{0.593410404624277, 369.218032671753},
|
||||||
|
{0.603468208092486, 370.005763838889},
|
||||||
|
{0.613526011560694, 370.793449087224},
|
||||||
|
{0.623583815028902, 371.577331994297},
|
||||||
|
{0.633641618497110, 372.361872668242},
|
||||||
|
{0.643699421965318, 373.139234321453},
|
||||||
|
{0.653757225433526, 373.922395308171},
|
||||||
|
{0.663815028901734, 374.698638233448},
|
||||||
|
{0.673872832369942, 375.477674739791},
|
||||||
|
{0.683930635838150, 376.256375688174},
|
||||||
|
{0.693988439306358, 377.561519397833},
|
||||||
|
{0.704046242774567, 378.859304679191},
|
||||||
|
{0.714104046242775, 380.167718611141},
|
||||||
|
{0.724161849710983, 381.477582211590},
|
||||||
|
{0.734219653179191, 382.785006750526},
|
||||||
|
{0.744277456647399, 384.092555921096},
|
||||||
|
{0.754335260115607, 385.403347233778},
|
||||||
|
{0.764393063583815, 386.680222025901},
|
||||||
|
{0.774450867052023, 387.960348600215},
|
||||||
|
{0.784508670520231, 389.241465657245},
|
||||||
|
{0.794566473988439, 390.517609695133},
|
||||||
|
{0.804624277456647, 391.778341124162},
|
||||||
|
{0.814682080924856, 393.038366474572},
|
||||||
|
{0.824739884393064, 394.291210028831},
|
||||||
|
{0.834797687861272, 395.545183108074},
|
||||||
|
{0.844855491329480, 396.779476009029},
|
||||||
|
{0.854913294797688, 398.005131564908},
|
||||||
|
{0.864971098265896, 399.217607990930},
|
||||||
|
{0.875028901734104, 400.433585037519},
|
||||||
|
{0.885086705202312, 401.639603766860},
|
||||||
|
{0.895144508670520, 402.824960888722},
|
||||||
|
{0.905202312138728, 403.999818917039},
|
||||||
|
{0.915260115606936, 405.164413592803},
|
||||||
|
{0.925317919075145, 406.332094769783},
|
||||||
|
{0.935375722543353, 407.489826389568},
|
||||||
|
{0.945433526011561, 408.638695158694},
|
||||||
|
{0.955491329479769, 409.788208430892},
|
||||||
|
{0.965549132947977, 410.920696471959},
|
||||||
|
{0.975606936416185, 412.071117188526},
|
||||||
|
{0.985664739884393, 413.227362102269},
|
||||||
|
{0.995722543352601, 414.376295532213},
|
||||||
|
{1.00578034682081, 415.517208260982},
|
||||||
|
{1.01583815028902, 416.685111645473},
|
||||||
|
{1.02589595375723, 417.874034388355},
|
||||||
|
{1.03595375722543, 419.050152356493},
|
||||||
|
{1.04601156069364, 420.226540416000},
|
||||||
|
{1.05606936416185, 421.431674949807},
|
||||||
|
{1.06612716763006, 422.651832857732},
|
||||||
|
{1.07618497109827, 423.879432015756},
|
||||||
|
{1.08624277456647, 425.096859052146},
|
||||||
|
{1.09630057803468, 426.370514159926},
|
||||||
|
{1.10635838150289, 427.681420607676},
|
||||||
|
{1.11641618497110, 428.990653815442},
|
||||||
|
{1.12647398843931, 430.308007852515},
|
||||||
|
{1.13653179190751, 431.623136377095},
|
||||||
|
{1.14658959537572, 432.942354714805},
|
||||||
|
{1.15664739884393, 434.260396644686},
|
||||||
|
{1.16670520231214, 435.580636665589},
|
||||||
|
{1.17676300578035, 436.903819649131},
|
||||||
|
{1.18682080924855, 438.235977095674},
|
||||||
|
{1.19687861271676, 439.760282970165},
|
||||||
|
{1.20693641618497, 441.285566201751},
|
||||||
|
{1.21699421965318, 442.823791210454},
|
||||||
|
{1.22705202312139, 444.354330708051},
|
||||||
|
{1.23710982658960, 445.893589623130},
|
||||||
|
{1.24716763005780, 447.437414139676},
|
||||||
|
{1.25722543352601, 449.016306037988},
|
||||||
|
{1.26728323699422, 450.605733417807},
|
||||||
|
{1.27734104046243, 452.197570123945},
|
||||||
|
{1.28739884393064, 453.799041921642},
|
||||||
|
{1.29745664739884, 455.395152193126},
|
||||||
|
{1.30751445086705, 457.004209080179},
|
||||||
|
{1.31757225433526, 458.623015526914},
|
||||||
|
{1.32763005780347, 460.238728741943},
|
||||||
|
{1.33768786127168, 461.860881948233},
|
||||||
|
{1.34774566473988, 463.495649237401},
|
||||||
|
{1.35780346820809, 465.129826626932},
|
||||||
|
{1.36786127167630, 466.767033528504},
|
||||||
|
{1.37791907514451, 468.408766204263},
|
||||||
|
{1.38797687861272, 470.059173838784},
|
||||||
|
{1.39803468208092, 471.721126584341},
|
||||||
|
{1.40809248554913, 473.414912587257},
|
||||||
|
{1.41815028901734, 475.097553575053},
|
||||||
|
{1.42820809248555, 476.805256760032},
|
||||||
|
{1.43826589595376, 478.512506689219},
|
||||||
|
{1.44832369942197, 480.230235676597},
|
||||||
|
{1.45838150289017, 481.950516788809},
|
||||||
|
{1.46843930635838, 483.675596525192},
|
||||||
|
{1.47849710982659, 485.408520318711},
|
||||||
|
{1.48855491329480, 487.160694244723},
|
||||||
|
{1.49861271676301, 488.912737035966},
|
||||||
|
{1.50867052023121, 490.676335211920},
|
||||||
|
{1.51872832369942, 492.446193358219},
|
||||||
|
{1.52878612716763, 494.230761014528},
|
||||||
|
{1.53884393063584, 496.014213821540},
|
||||||
|
{1.54890173410405, 497.800012215749},
|
||||||
|
{1.55895953757225, 499.586844428474},
|
||||||
|
{1.56901734104046, 501.389791490613},
|
||||||
|
{1.57907514450867, 503.197873321221},
|
||||||
|
{1.58913294797688, 505.017976823598},
|
||||||
|
{1.59919075144509, 506.842077493572},
|
||||||
|
{1.60924855491329, 508.681008225194},
|
||||||
|
{1.61930635838150, 510.531745060971},
|
||||||
|
{1.62936416184971, 512.383464849326},
|
||||||
|
{1.63942196531792, 514.249250804510},
|
||||||
|
{1.64947976878613, 516.119098877381},
|
||||||
|
{1.65953757225434, 518.005308788433},
|
||||||
|
{1.66959537572254, 519.910761252352},
|
||||||
|
{1.67965317919075, 521.812275649970},
|
||||||
|
{1.68971098265896, 523.727747256148},
|
||||||
|
{1.69976878612717, 526.524959987653},
|
||||||
|
{1.70982658959538, 529.326074922632},
|
||||||
|
{1.71988439306358, 532.152158438731},
|
||||||
|
{1.72994219653179, 534.995939192065},
|
||||||
|
{1.74000000000000, 537.866310625605},};
|
||||||
|
|
||||||
|
//encuentra el mas cercano justo anterior.
|
||||||
|
int index_E = LkTable.elem(find(LkTable<=t_disparo)).max();
|
||||||
|
if (index_E<(LkTable.n_rows-1)){
|
||||||
|
double tdisparo1=LkTable(index_E,0);
|
||||||
|
double tdisparo2=LkTable(index_E+1,0);
|
||||||
|
double E1=LkTable(index_E,1);
|
||||||
|
double E2=LkTable(index_E+1,1);
|
||||||
|
|
||||||
|
E=(t_disparo-tdisparo1)*(E2-E1)/(tdisparo2-tdisparo1)+E1;
|
||||||
|
}else{
|
||||||
|
E=0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return E;
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user