mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-08-10 16:03:51 +00:00
Code Issues Releases Wiki Activity

fix: kf_tracking bug

Browse Source
This commit is contained in:
M.A. Gomez 2023-03-14 15:29:46 +01:00
parent 2d4c5b4d7b
commit 8445ed3cbb
1 changed files with 19 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -634,18 +634,18 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code())
{
const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
//std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
// std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel)
{
arma::vec x_tmp;
arma::mat F_tmp;
gr::thread::scoped_lock lock(d_setlock);
//To.Do: apply VTL corrections to the KF states
// To.Do: apply VTL corrections to the KF states
double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in;
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
//ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces
// ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces
F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0},
{0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)},
{0.0, 0.0, 1.0, delta_t_s},
@ -658,31 +658,26 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
if (cmd->enable_carrier_nco_cmd)
{
if (cmd->enable_code_nco_cmd)
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
<< " 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
std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n";
}
else
{
// std::cout<<"yet to soon"<<std::endl;
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
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 << "correction not applied" << std::endl;
}
// set vtl corrections flag to inform VTL from gnss_synchro object
@ -1242,7 +1237,7 @@ void kf_tracking::run_Kf()
// Kalman loop
// Prediction
//d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase
// d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase
d_x_new_old = d_F * d_x_old_old;
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;