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 + 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;
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_y = 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(7) = acc_y;
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)
{
std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
// 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";
// 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(2) = tmp_x(2); // replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
else