mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 22:34:58 +00:00
Code Issues Releases Wiki Activity

MOD: vtl_engine rocket model working

Browse Source
This commit is contained in:
M.A. Gomez 2023-03-14 17:48:33 +01:00
parent 956142cbe2
commit c2f89e9721
2 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -112,6 +112,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_R(i, i) = 80.0; kf_R(i, i) = 80.0;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;
kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0;
if (i == 6)
{
kf_R(i, i) = 10e5;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e5;
kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 10e6;
}
} }
//************************************** //**************************************
@ -146,7 +153,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;
// model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter); model3DoF(acc_x, acc_y, acc_z, kf_x, kf_dt, counter);
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;

18
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -660,19 +660,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
{ {
if (abs(d_x_old_old(2) - tmp_x(2)) > 50) if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
{ {
std::cout << "channel: " << this->d_channel // std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: " // << " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz" // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n"; // << " \n";
} }
else else
{ {
std::cout << "channel: " << this->d_channel // std::cout << "channel: " << this->d_channel
<< " tracking_cmd NEAR: " // << " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz" // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n"; // << " \n";
} }
// d_x_old_old(2) = tmp_x(2); // replace DOPPLER 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(3) = tmp_x(3); //replace DOPPLER RATE
} }
else else