From 704deb61842e82d7576390a0b40a972ccf36d4c6 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 16:09:48 +0100 Subject: [PATCH 01/12] MOD: vtl tracking --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 63ae58f71..c087eb6dc 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2324,7 +2324,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); + //flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); } if (flag_pvt_valid == true) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 78b98c2a9..498813360 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -658,17 +658,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if(abs(d_x_old_old(2) - tmp_x(2))>50){ - std::cout << " tracking_cmd TOO FAR: " + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd TOO FAR: " << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" << " \n"; }else{ - std::cout << " tracking_cmd NEAR: " + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd NEAR: " << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" << " \n"; - //d_x_old_old(2) = tmp_x(2); //replace the Code Phase state + //d_x_old_old(2) = tmp_x(2); //replace DOPPLER + //d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE } - //d_x_old_old(0) = tmp_x(0); //replace the Code Phase state // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; @@ -698,7 +700,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) else { dump_tracking_file << "doppler_corr" - << ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "," << old_doppler_rate << "," << old_code_phase_chips << "\n"; + << ","<< this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3)<< "," << old_doppler_rate << "\n"; dump_tracking_file.close(); } } From 1991ab4aa6423fb49046c64920d3137b2e271b33 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 16:10:23 +0100 Subject: [PATCH 02/12] MOD: vtl_engine --- src/algorithms/PVT/libs/vtl_engine.cc | 101 +++++++++++--------------- 1 file changed, 42 insertions(+), 59 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56fd0e876..dc582295c 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -41,7 +41,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) int n_of_states=11; static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; static arma::mat kf_x = arma::zeros(n_of_states, 1); - arma::mat kf_dx = arma::zeros(n_of_states, 1); + static arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); @@ -85,19 +85,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) else { // receiver solution from previous KF step - // kf_x(0) = x_u; - // kf_x(1) = y_u; - // kf_x(2) = z_u; - // kf_x(3) = xDot_u; - // kf_x(4) = yDot_u; - // kf_x(5) = zDot_u; - // kf_x(6) = xDot2_u; - // kf_x(7) = yDot2_u; - // kf_x(8) = zDot2_u; - // kf_x(9) = cdeltat_u; - // kf_x(10) = cdeltatDot_u; - - //kf_P_x = new_data.kf_P; + kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; + kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; + kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } // State error Covariance Matrix Q (PVT) @@ -108,9 +98,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 8.0; kf_Q(4, 4) = 8.0; kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = .1; - kf_Q(7, 7) = .1; - kf_Q(8, 8) = .1; + kf_Q(6, 6) = 10; + kf_Q(7, 7) = 10; + kf_Q(8, 8) = 10; kf_Q(9, 9) = 4.0; kf_Q(10, 10) = 100.0; @@ -118,18 +108,16 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) for (int32_t i = 0; i < new_data.sat_number; i++) { // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) - kf_R(i, i) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 1.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - // if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){ - // kf_R(i, i) = 10e4; - // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - // } + if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ + kf_R(i, i) = 10e4; + kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + cout<<"channel: "<.5){ - kf_xerr.print("kf_xERR: "); - cout<<"kf_dt: "<5){ - // dump_vtl_file << "pr_m" - // << "," << new_data.pr_m(0) << "," << new_data.pr_m(1) << "," << new_data.pr_m(2) - // << "," << new_data.pr_m(3) << "," << new_data.pr_m(4)<<"," << new_data.pr_m(5) << endl; - // dump_vtl_file << "prDot_m" - // << "," << new_data.doppler_hz(0)<< "," << new_data.doppler_hz(1)<< "," << new_data.doppler_hz(2) - // << "," << new_data.doppler_hz(3) << "," << new_data.doppler_hz(4)<< "," << new_data.doppler_hz(5)<< endl; - // dump_vtl_file << "sat_position" - // << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << "," << kf_xerr(5) - // << "," << kf_xerr(6) << "," << kf_xerr(7) << endl; - // dump_vtl_file << "sat_velocity" - // << "," << 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 << "pr_m" + << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)< Date: Wed, 15 Feb 2023 16:33:02 +0100 Subject: [PATCH 03/12] ADD: model3DoF prototype --- src/algorithms/PVT/libs/vtl_engine.cc | 42 ++++++++++++++++++--------- src/algorithms/PVT/libs/vtl_engine.h | 1 + 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index dc582295c..dd005866c 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -44,19 +44,19 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static arma::mat kf_dx = arma::zeros(n_of_states, 1); // covariances (static) - kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); + kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); double kf_dt = delta_t_vtl; //0.05; kf_Q = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states); bool test = kf_F_fill(kf_F,kf_dt); - //kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); - kf_y = arma::zeros(2 * new_data.sat_number, 1); - kf_yerr = arma::zeros(2 * new_data.sat_number, 1); + //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); + kf_y = arma::zeros(3 * new_data.sat_number, 1); + kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_xerr = arma::zeros(n_of_states, 1); - kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix - kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; + kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix + kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; // ################## Kalman Tracking ###################################### static uint32_t counter = 0; //counter counter = counter + 1; //uint64_t @@ -79,15 +79,22 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S; - kf_dx=kf_x-kf_dx; + kf_dx = kf_x-kf_dx; kf_dx = kf_F * kf_dx; // state prediction } else { // receiver solution from previous KF step - kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; - kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; - kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; + double acc_x = 0; + double acc_y = 0; + double acc_z = 0; + test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt); + kf_x(6) = acc_x; + kf_x(7) = acc_y; + kf_x(8) = acc_z; + // kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; + // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; + // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; } // State error Covariance Matrix Q (PVT) @@ -98,11 +105,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_Q(3, 3) = 8.0; kf_Q(4, 4) = 8.0; kf_Q(5, 5) = 8.0; - kf_Q(6, 6) = 10; - kf_Q(7, 7) = 10; - kf_Q(8, 8) = 10; + kf_Q(6, 6) = .10; + kf_Q(7, 7) = .10; + kf_Q(8, 8) = .10; kf_Q(9, 9) = 4.0; - kf_Q(10, 10) = 100.0; + kf_Q(10, 10) = 10.0; // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) @@ -110,10 +117,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 100.0;//*50.0/new_data.sat_CN0_dB_hz(i); if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ kf_R(i, i) = 10e4; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; cout<<"channel: "< Date: Wed, 15 Feb 2023 17:05:49 +0100 Subject: [PATCH 04/12] ADD: EmpujeLkTable prototype --- src/algorithms/PVT/libs/vtl_engine.cc | 58 ++++++++++++++++++++++++++- src/algorithms/PVT/libs/vtl_engine.h | 3 +- 2 files changed, 58 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index dd005866c..05f3aa510 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -331,7 +331,61 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r return -1; } -bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double kf_dt) +bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt) { - return -1; + arma::colvec u_vec; + arma::colvec acc_vec; + arma::colvec u_dir; + arma::colvec gravity_ECEF = {-7.826024, 0.8616969, -5.833042}; //lat=36.533333 lon=-6.283333 + static double t_disparo; + double Empuje; + double densidad=1.0; + double ballistic_coef = 0.007; + double PI = acos(-1.0); + //vector velocidad + 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)); + + //modulo de la velocidad + double u = norm(u_vec, 2); + + if(u>4){ + t_disparo=t_disparo+dt; + double diam_cohete=120.0e-3;// 120 mm + double mass_rocket=50.0; //50Kg + + if(t_disparo<.2){ + u_dir={.90828, -.13984, .388756}; + }else{ + u_dir = u_vec / u; + } + // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); + // [T, sound_v, P, densidad] = atmosisa(lla(3)); + // sound_v=320;% @ 5km and -17.5C + // Mach=u/sound_v; + // CD0 = Cd0_M_LookTable(Mach); + // % ballistic_coef is Cd0/mass_rocket; + // ballistic_coef=CD0/mass_rocket; + // Empuje = EmpujeLookTable(t_disparo); + + acc_vec = -(PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir + +gravity_ECEF+Empuje*u_dir; + + // % return + acc_x = acc_vec(0); + acc_y = acc_vec(1); + acc_z = acc_vec(2); + }else{ + // % return + acc_x = 0; + acc_y = 0; + acc_z = 0; + } + + return -1; +} + +double Vtl_Engine::EmpujeLkTable(double t_disparo) +{ + return -1.0; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 6c869c96e..9f04e1939 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -88,7 +88,8 @@ private: 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 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 kf_dt); + bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double dt); + double EmpujeLkTable(double t_disparo); }; /** \} */ From 3cb61f6f2db2e0fb82f3a09466c2ce79b6f24a3d Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:09:37 +0100 Subject: [PATCH 05/12] ADD: empuje lkTable --- src/algorithms/PVT/libs/vtl_engine.cc | 197 +++++++++++++++++++++++++- 1 file changed, 193 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 05f3aa510..605f9b61e 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -341,7 +341,6 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x double Empuje; double densidad=1.0; double ballistic_coef = 0.007; - double PI = acos(-1.0); //vector velocidad 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)); @@ -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; // 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; // % 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) -{ - 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; } \ No newline at end of file From 2115bc0aab63bafe45d97e7174fb7a488128a7bd Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:18:49 +0100 Subject: [PATCH 06/12] MOD: model3DoF --- src/algorithms/PVT/libs/vtl_engine.cc | 11 ++++++++--- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 605f9b61e..56b4073ae 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -331,13 +331,13 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r 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 acc_vec; arma::colvec u_dir; 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 densidad=1.0; double ballistic_coef = 0.007; @@ -347,6 +347,10 @@ bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x //modulo de la velocidad double u = norm(u_vec, 2); + + // if(t<1000){ + // t_disparo=0; + // } if(u>4){ t_disparo=t_disparo+dt; @@ -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); // % ballistic_coef is 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 +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_z = acc_vec(2); }else{ + t_disparo=0; // % return acc_x = 0; acc_y = 0; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 9f04e1939..3611373fe 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -88,7 +88,7 @@ private: 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 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); }; From 08c68d4af902bd9f88a3028064aa18d837d13b7c Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 19:44:21 +0100 Subject: [PATCH 07/12] MOD: filtro aceleracion vtl --- src/algorithms/PVT/libs/vtl_engine.cc | 47 +++++++++++++++++++-------- src/algorithms/PVT/libs/vtl_engine.h | 3 +- 2 files changed, 36 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56b4073ae..27d014b86 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -42,6 +42,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.; static arma::mat kf_x = arma::zeros(n_of_states, 1); static arma::mat kf_dx = arma::zeros(n_of_states, 1); + + //TODO: resolver el problema cuando cambie el numero de sat!! + // static arma::colvec rhoDot_pri_old = arma::zeros(new_data.sat_number, 1); // covariances (static) kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); @@ -117,7 +120,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 100.0;//*50.0/new_data.sat_CN0_dB_hz(i); + kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ kf_R(i, i) = 10e4; @@ -137,6 +140,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) d = arma::zeros(new_data.sat_number, 1); rho_pri = arma::zeros(new_data.sat_number, 1); rhoDot_pri = arma::zeros(new_data.sat_number, 1); + rhoDot2_pri = arma::zeros(new_data.sat_number, 1); rho_pri_filt = arma::zeros(new_data.sat_number, 1); rhoDot_pri_filt = arma::zeros(new_data.sat_number, 1); @@ -154,26 +158,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) new_data.sat_LOS(i, 2) = a_z(i); } - kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); + kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); + + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt; + } // Kalman estimation (measurement update) - test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); + test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // Kalman filter update step - kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S) + kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S) arma::mat B= (kf_P_x * kf_H.t()) ; kf_K = B * arma::inv(kf_S); // Kalman gain - kf_xerr = kf_K * (kf_yerr); // Error state estimation + kf_xerr = kf_K * (kf_yerr); // Error state estimation //kf_xerr.row(5)=kf_K.row(5)*kf_yerr; arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H); kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t() ; // update state estimation error covariance matrix kf_dx=kf_x; - kf_x = kf_x-kf_xerr; // updated state estimation (a priori + error) + kf_x = kf_x-kf_xerr; // updated state estimation (a priori + error) // // ################## Geometric Transformation ###################################### test = obsv_calc(rho_pri,rhoDot_pri,a_x, a_y, a_z,new_data.sat_number,new_data.sat_p,new_data.sat_v,kf_x); + + for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities + { + //acc_effect(i)=(a_x(i)*kf_state(7,t)+a_y(chan,t)*kf_state(8,t)+a_z(chan,t)*kf_state(9,t)); + //rhoDot2_pri(chan,t)=(rhoDot_pri(chan,t)-rhoDot_pri(chan,t-1))/kf_dt; + //rhoDot2_pri(chan,t)=-acc_effect(chan,t); + } + test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt); // Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x kf_yerr = kf_H * kf_xerr; @@ -191,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s = 0;//-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -278,6 +295,13 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma kf_H(i + sat_number, 8) = az(i)*kf_dt; kf_H(i + sat_number, 10) = 1.0; + kf_H(i + 2*sat_number, 3) = 0;//ax(i); + kf_H(i + 2*sat_number, 4) = 0;//ay(i); + kf_H(i + 2*sat_number, 5) = 0;//az(i); + kf_H(i + 2*sat_number, 6) = ax(i); + kf_H(i + 2*sat_number, 7) = ay(i); + kf_H(i + 2*sat_number, 8) = az(i); + kf_H(i + 2*sat_number, 10) = kf_dt; } return -1; @@ -321,12 +345,13 @@ bool Vtl_Engine::obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec return -1; } -bool Vtl_Engine::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 Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x) { for (int32_t i = 0; i < sat_number; i++) // Measurement vector { kf_yerr(i) = rho_pri(i) - pr_m(i); kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i); + kf_yerr(i + 2*sat_number) = -rhoDot2_pri(i); } return -1; } @@ -347,11 +372,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k //modulo de la velocidad double u = norm(u_vec, 2); - - // if(t<1000){ - // t_disparo=0; - // } - + if(u>4){ t_disparo=t_disparo+dt; double diam_cohete=120.0e-3;// 120 mm diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 3611373fe..d41bb6b60 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -56,6 +56,7 @@ private: arma::colvec d; arma::colvec rho_pri; arma::colvec rhoDot_pri; + arma::colvec rhoDot2_pri; arma::colvec rho_pri_filt; arma::colvec rhoDot_pri_filt; arma::colvec doppler_hz_filt; @@ -87,7 +88,7 @@ private: bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation 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 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::mat rhoDot2_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); double EmpujeLkTable(double t_disparo); }; From 3f348aabd92ca67b954ea8b8ea1c7d54ce42c134 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 15 Feb 2023 20:13:54 +0100 Subject: [PATCH 08/12] FIX: parser velocity error --- src/algorithms/PVT/libs/vtl_engine.cc | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 27d014b86..e5ecfc5d1 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -166,7 +166,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // rhoDot2_pri(i)=(rhoDot_pri(i)-rhoDot_pri_old(i))/kf_dt; } // Kalman estimation (measurement update) - test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x); + test = kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri*0, new_data.pr_m, new_data.doppler_hz, kf_x); kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction // Kalman filter update step @@ -208,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s = 0;//-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -367,7 +367,9 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k double densidad=1.0; double ballistic_coef = 0.007; //vector velocidad - u_vec = kf_x.rows(4, 6); + kf_x.print("kf_x: "); + 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 @@ -391,10 +393,12 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k // % ballistic_coef is Cd0/mass_rocket; // ballistic_coef=CD0/mass_rocket; Empuje = EmpujeLkTable(t_disparo); - + cout<<"Empuje: "< Date: Wed, 15 Feb 2023 20:34:21 +0100 Subject: [PATCH 09/12] FIX: lkTable bug --- src/algorithms/PVT/libs/vtl_engine.cc | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index e5ecfc5d1..1359531e6 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -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 ballistic_coef = 0.007; //vector velocidad - kf_x.print("kf_x: "); + 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)); //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){ t_disparo=t_disparo+dt; + cout<<"t_disparo: "< Date: Thu, 16 Feb 2023 00:26:14 +0100 Subject: [PATCH 10/12] FIX: bug in vtlEngine --- src/algorithms/PVT/libs/vtl_engine.cc | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 1359531e6..51995847a 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -91,7 +91,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) double acc_x = 0; double acc_y = 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(7) = acc_y; kf_x(8) = acc_z; @@ -208,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) // sample code trk_cmd.carrier_phase_rads = 0; // difficult of calculation trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction - trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8))/ Lambda_GPS_L1; + trk_cmd.carrier_freq_rate_hz_s =-(a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8)) / Lambda_GPS_L1; trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true; @@ -369,8 +369,6 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k //vector velocidad 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 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 if(t_disparo<.2){ - u_dir={.90828, -.13984, .388756}; + u_dir={.90828, -.13984, -.388756}; }else{ u_dir = u_vec / u; } + // u_dir.print("u_dir"); // lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]); // [T, sound_v, P, densidad] = atmosisa(lla(3)); // sound_v=320;% @ 5km and -17.5C @@ -399,7 +398,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 +gravity_ECEF+Empuje*u_dir; - //acc_vec.print("acc_vec"); + // acc_vec.print("acc_vec"); // % return acc_x = acc_vec(0); acc_y = acc_vec(1); From 1a50d75ee04470e6460df55079a9e2ce1b78b302 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 17 Feb 2023 17:48:35 +0100 Subject: [PATCH 11/12] MOD: async into rocket model --- src/algorithms/PVT/libs/rtklib_solver.cc | 45 +++-- src/algorithms/PVT/libs/vtl_engine.cc | 187 ++++++++++-------- src/algorithms/PVT/libs/vtl_engine.h | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 34 ++-- 4 files changed, 154 insertions(+), 114 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2cd0ab619..75a95731f 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1248,9 +1248,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_dts(0) = rx_position_and_time[3]; vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] - //new_vtl_data.debug_print(); - //vtl_data.kf_state.print("kf_state_input"); vtl_engine.vtl_loop(vtl_data); + // cout<<"llegado aqui fuera"< &gnss_observables_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.rx_dts(1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - for (int n = 0; n<6; n++) - { - tmp_double = vtl_data.sat_p(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.pr_m(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.doppler_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_CN0_dB_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } + // for (int n = 0; n<6; n++) + // { + // tmp_double = vtl_data.sat_p(n, 0); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_p(n, 1); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_p(n, 2); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 0); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 1); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_v(n, 2); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.pr_m(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.doppler_hz(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // tmp_double = vtl_data.sat_CN0_dB_hz(n); + // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // } // GEO user position Latitude [deg] tmp_double = this->get_latitude(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 51995847a..fe0e8b612 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -36,6 +36,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) static uint64_t refSampleCounter = new_data.sample_counter; double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; refSampleCounter = new_data.sample_counter; + static double delta_t_cmd = 0; + bool flag_cmd = false; + delta_t_cmd = delta_t_cmd+delta_t_vtl; // update timer for vtl trk command + if(delta_t_cmd>=0.3){ + flag_cmd = true; + delta_t_cmd = 0; // reset timer for vtl trk command + } // ################## Kalman filter initialization ###################################### //State variables int n_of_states=11; @@ -91,7 +98,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) double acc_x = 0; double acc_y = 0; double acc_z = 0; - test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt); + test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt,counter); kf_x(6) = acc_x; kf_x(7) = acc_y; kf_x(8) = acc_z; @@ -122,12 +129,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 400.0;//*50.0/new_data.sat_CN0_dB_hz(i); - if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ - kf_R(i, i) = 10e4; - kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; - kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; - cout<<"channel: "<90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ + // kf_R(i, i) = 10e4; + // kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; + // kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4; + // cout<<"channel: "<5){ - dump_vtl_file << "pr_m" - << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) - << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<5){ + // // dump_vtl_file << "pr_m" + // // << "," << kf_yerr(0)<< "," << kf_yerr(1)<< "," << kf_yerr(2) + // // << "," << kf_yerr(3) << "," << kf_yerr(4)<< "," << kf_yerr(5)<4){ - t_disparo=t_disparo+dt; - cout<<"t_disparo: "<1500){ + if(u>6){ + t_disparo=t_disparo+dt; + std::cout<<"u : "<enable_carrier_nco_cmd){ + if(cmd->enable_code_nco_cmd){ + if(abs(d_x_old_old(2) - tmp_x(2))>50){ + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd TOO FAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + }else{ + std::cout <<"channel: "<< this->d_channel + << " tracking_cmd NEAR: " + << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + << " \n"; + } + d_x_old_old(2) = tmp_x(2); //replace DOPPLER + // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + + }else{ + // std::cout<<"yet to soon"<50){ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; }else{ - std::cout <<"channel: "<< this->d_channel - << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; - //d_x_old_old(2) = tmp_x(2); //replace DOPPLER - //d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE + // not 0.5 seg yet } From 1a3df78ba80125df91c018958941ec7602d36644 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Fri, 17 Feb 2023 21:58:00 +0000 Subject: [PATCH 12/12] MOD: remove sat variables from pvt.dat file --- src/algorithms/PVT/libs/rtklib_solver.cc | 104 +---------------------- 1 file changed, 3 insertions(+), 101 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 75a95731f..9cbecbced 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -205,7 +205,7 @@ bool Rtklib_Solver::save_matfile() const // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 23; //+2 MAGL - const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat + const int32_t number_of_double_vars_sat = 0; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; @@ -258,15 +258,6 @@ bool Rtklib_Solver::save_matfile() const auto clk_bias = std::vector(num_epoch); auto clk_drift = std::vector(num_epoch); - auto sat_posX_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posY_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posZ_m = std::vector>(6, std::vector(num_epoch)); - auto sat_velX = std::vector>(6, std::vector(num_epoch)); - auto sat_velY = std::vector>(6, std::vector(num_epoch)); - auto sat_velZ = std::vector>(6, std::vector(num_epoch)); - auto sat_prg_m = std::vector>(6, std::vector(num_epoch)); - auto sat_dopp_hz = std::vector>(6, std::vector(num_epoch)); - auto sat_CN0_dBhz = std::vector>(6, std::vector(num_epoch)); auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); @@ -306,18 +297,7 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); - for (uint32_t chan = 0; chan < 6; chan++) - { - dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_CN0_dBhz[chan][i]), sizeof(double)); - } + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); @@ -340,33 +320,6 @@ bool Rtklib_Solver::save_matfile() const return false; } - auto sat_posX_m_aux = std::vector(6 * num_epoch); - auto sat_posY_m_aux = std::vector(6 * num_epoch); - auto sat_posZ_m_aux = std::vector(6 * num_epoch); - auto sat_velX_aux = std::vector(6 * num_epoch); - auto sat_velY_aux = std::vector(6 * num_epoch); - auto sat_velZ_aux = std::vector(6 * num_epoch); - auto sat_prg_m_aux = std::vector(6 * num_epoch); - auto sat_dopp_hz_aux = std::vector(6 * num_epoch); - auto sat_CN0_dBhz_aux = std::vector(6 * num_epoch); - - uint32_t k = 0U; - for (int64_t j = 0; j < num_epoch; j++) - { - for (uint32_t i = 0; i < 6; i++) - { - sat_posX_m_aux[k] = sat_posX_m[i][j]; - sat_posY_m_aux[k] = sat_posY_m[i][j]; - sat_posZ_m_aux[k] = sat_posZ_m[i][j]; - sat_velX_aux[k] = sat_velX[i][j]; - sat_velY_aux[k] = sat_velY[i][j]; - sat_velZ_aux[k] = sat_velZ[i][j]; - sat_prg_m_aux[k] = sat_prg_m[i][j]; - sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; - sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j]; - k++; - } - } // WRITE MAT FILE mat_t *matfp; @@ -450,36 +403,6 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - std::array dims_sat{static_cast(6), static_cast(num_epoch)}; - matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1417,28 +1340,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = vtl_data.rx_dts(1); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // for (int n = 0; n<6; n++) - // { - // tmp_double = vtl_data.sat_p(n, 0); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_p(n, 1); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_p(n, 2); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 0); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 1); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_v(n, 2); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.pr_m(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.doppler_hz(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // tmp_double = vtl_data.sat_CN0_dB_hz(n); - // d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // } - + // GEO user position Latitude [deg] tmp_double = this->get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double));